diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index fbced80c80..754b434930 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -226,6 +226,7 @@ class ObstacleLayer : public CostmapLayer double * max_y); std::string global_frame_; ///< @brief The global frame for the costmap + double min_obstacle_height_; ///< @brief Max Obstacle Height double max_obstacle_height_; ///< @brief Max Obstacle Height /// @brief Used to project laser scans into point clouds diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index e7836f9639..653c0885ca 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -78,6 +78,7 @@ void ObstacleLayer::onInitialize() declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); + declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); @@ -89,6 +90,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); node->get_parameter(name_ + "." + "combination_method", combination_method_); node->get_parameter("track_unknown_space", track_unknown_space); @@ -296,7 +298,9 @@ ObstacleLayer::dynamicParametersCallback( const auto & param_name = parameter.get_name(); if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == name_ + "." + "max_obstacle_height") { + if (param_name == name_ + "." + "min_obstacle_height") { + min_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "max_obstacle_height") { max_obstacle_height_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { @@ -453,6 +457,12 @@ ObstacleLayer::updateBounds( for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double px = *iter_x, py = *iter_y, pz = *iter_z; + // if the obstacle is too low, we won't add it + if (pz < min_obstacle_height_) { + RCLCPP_DEBUG(logger_, "The point is too low"); + continue; + } + // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { RCLCPP_DEBUG(logger_, "The point is too high");