Skip to content

Commit

Permalink
Add a min_obstacle_height param to the nav2_costmap_2d's ObstacleLaye…
Browse files Browse the repository at this point in the history
…r plugin (ros-navigation#3211)

This allows considering full range observations, specified by the
  <data source>.min_obstacle_height
and
  <data source>.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.
  • Loading branch information
milidam authored Sep 22, 2022
1 parent 664c09f commit 0b4179b
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 1 deletion.
1 change: 1 addition & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 11 additions & 1 deletion nav2_costmap_2d/plugins/obstacle_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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("")));
Expand All @@ -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);
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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");
Expand Down

0 comments on commit 0b4179b

Please sign in to comment.