Skip to content

Add a min_obstacle_height param to the nav2_costmap_2d's ObstacleLayer plugin (backport #3211) #3215

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 1 commit into from
Sep 22, 2022
Merged

Add a min_obstacle_height param to the nav2_costmap_2d's ObstacleLayer plugin (backport #3211) #3215

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
View file
Open in desktop
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
View file
Open in desktop
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