From ca5600401c86f35bfdb2b88e0be0f4bde59b9d4a Mon Sep 17 00:00:00 2001 From: milidam Date: Thu, 22 Sep 2022 19:24:58 +0200 Subject: [PATCH] Add a min_obstacle_height param to the nav2_costmap_2d's ObstacleLayer plugin (#3211) This allows considering full range observations, specified by the .min_obstacle_height and .max_obstacle_height especially used for the raytracing, but to still be able to specify a minimum obstacle height to report obstacles onto the costmap. This is in particular required in the case a PointCloud2 source points slightly towards the ground, sometimes detecting obstacles, that should be cleared once the ground reappears behind the obstacle when it has moved away: we don't want to detect the ground as an obstacle, but still want it to be used in the raytracing to clear the previously detected obstacle. (cherry picked from commit 0b4179b6432c62efdf1e853bce382de2e6706061) --- .../include/nav2_costmap_2d/obstacle_layer.hpp | 1 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) 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 fbced80c80d..754b434930c 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 e7836f9639e..653c0885cae 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");