From fa4ec8fee95f52433945b2375c8314cbf3964536 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 May 2025 14:57:39 -0700 Subject: [PATCH 1/8] Adding toggle option of keepout zone Signed-off-by: Steve Macenski --- .../costmap_filters/keepout_filter.hpp | 6 ++ .../costmap_filters/keepout_filter.cpp | 63 +++++++++++++++++-- 2 files changed, 63 insertions(+), 6 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e2225196d72..df3e0b7ab27 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -102,6 +102,12 @@ class KeepoutFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; std::string global_frame_; // Frame of current layer (master_grid) + + bool override_lethal_cost_{false}; // If true, lethal cost will be overridden + unsigned char lethal_override_cost_{252}; // Value to override lethal cost with + bool last_pose_lethal_{false}; // If true, last pose was lethal + unsigned int lethal_state_update_min_x_, lethal_state_update_min_y_; + unsigned int lethal_state_update_max_x_, lethal_state_update_max_y_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index ac01861ef67..fb9980a1611 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -74,6 +74,11 @@ void KeepoutFilter::initializeFilter( std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); global_frame_ = layered_costmap_->getGlobalFrameID(); + + declareParameter("override_lethal_cost", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); + declareParameter("lethal_override_cost", rclcpp::ParameterValue(252)); + node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); } void KeepoutFilter::filterInfoCallback( @@ -149,7 +154,7 @@ void KeepoutFilter::maskCallback( void KeepoutFilter::process( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j, - const geometry_msgs::msg::Pose2D & /*pose*/) + const geometry_msgs::msg::Pose2D & pose) { std::lock_guard guard(*getMutex()); @@ -244,10 +249,49 @@ void KeepoutFilter::process( } // unsigned<-signed conversions. - unsigned const int mg_min_x_u = static_cast(mg_min_x); - unsigned const int mg_min_y_u = static_cast(mg_min_y); - unsigned const int mg_max_x_u = static_cast(mg_max_x); - unsigned const int mg_max_y_u = static_cast(mg_max_y); + unsigned int mg_min_x_u = static_cast(mg_min_x); + unsigned int mg_min_y_u = static_cast(mg_min_y); + unsigned int mg_max_x_u = static_cast(mg_max_x); + unsigned int mg_max_y_u = static_cast(mg_max_y); + + // Lets find the pose's cost if we are allowed to override the lethal cost + bool is_pose_lethal = false; + if (override_lethal_cost_) { + geometry_msgs::msg::Pose2D mask_pose; + if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { + unsigned int mask_robot_i, mask_robot_j; + if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + auto keepout_mask_data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); + is_pose_lethal = (keepout_mask_data == 253 || keepout_mask_data == 254); + if (is_pose_lethal) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out."); + } + } + } + } + + if (override_lethal_cost_) { + // If in lethal space or just exited lethal space, + // we need to update all possible spaces touched during this state + if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) { + lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_); + mg_min_x_u = lethal_state_update_min_x_; + lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_); + mg_min_y_u = lethal_state_update_min_y_; + lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_); + mg_max_x_u = lethal_state_update_max_x_; + lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_); + mg_max_y_u = lethal_state_update_max_y_; + } else { + // If out of lethal space, reset managed lethal state sizes + lethal_state_update_min_x_ = master_grid.getSizeInCellsX(); + lethal_state_update_min_y_ = master_grid.getSizeInCellsY(); + lethal_state_update_max_x_ = 0u; + lethal_state_update_max_y_ = 0u; + } + } unsigned int i, j; // master_grid iterators unsigned int index; // corresponding index of master_grid @@ -284,12 +328,19 @@ void KeepoutFilter::process( if (data == NO_INFORMATION) { continue; } + if (data > old_data || old_data == NO_INFORMATION) { - master_array[index] = data; + if (override_lethal_cost_ && is_pose_lethal) { + master_array[index] = lethal_override_cost_; + } else { + master_array[index] = data; + } } } } } + + last_pose_lethal_ = is_pose_lethal; } void KeepoutFilter::resetFilter() From a9b9449b28acd0702557bffdcb95ae6c92bf60cf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 May 2025 14:58:39 -0700 Subject: [PATCH 2/8] Default off Signed-off-by: Steve Macenski --- nav2_bringup/params/nav2_params.yaml | 4 ++++ nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 19c1edbf591..72ccbe714a9 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -229,6 +229,8 @@ local_costmap: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: True filter_info_topic: "keepout_costmap_filter_info" + override_lethal_cost: True + lethal_override_cost: 200 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -281,6 +283,8 @@ global_costmap: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: True filter_info_topic: "keepout_costmap_filter_info" + override_lethal_cost: True + lethal_override_cost: 200 speed_filter: plugin: "nav2_costmap_2d::SpeedFilter" enabled: True diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index fb9980a1611..945bc06279e 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -75,7 +75,7 @@ void KeepoutFilter::initializeFilter( global_frame_ = layered_costmap_->getGlobalFrameID(); - declareParameter("override_lethal_cost", rclcpp::ParameterValue(true)); + declareParameter("override_lethal_cost", rclcpp::ParameterValue(false)); node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); declareParameter("lethal_override_cost", rclcpp::ParameterValue(252)); node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); From d48fff0adacdb5b0122b182e77d859d61471afdb Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 May 2025 15:03:01 -0700 Subject: [PATCH 3/8] Join conditions Signed-off-by: Steve Macenski --- nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 945bc06279e..9754e221f07 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -270,9 +270,7 @@ void KeepoutFilter::process( } } } - } - if (override_lethal_cost_) { // If in lethal space or just exited lethal space, // we need to update all possible spaces touched during this state if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) { From 283a5a165d65e4681f55fe5a4ed8d2c04a60ca82 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 May 2025 15:04:52 -0700 Subject: [PATCH 4/8] spell check Signed-off-by: Steve Macenski --- nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 9754e221f07..913baf95933 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -254,7 +254,7 @@ void KeepoutFilter::process( unsigned int mg_max_x_u = static_cast(mg_max_x); unsigned int mg_max_y_u = static_cast(mg_max_y); - // Lets find the pose's cost if we are allowed to override the lethal cost + // Let's find the pose's cost if we are allowed to override the lethal cost bool is_pose_lethal = false; if (override_lethal_cost_) { geometry_msgs::msg::Pose2D mask_pose; From 24cb302551e8dd730203685a240a7246f674bd8e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 22 May 2025 15:08:44 -0700 Subject: [PATCH 5/8] copilot suggestions Signed-off-by: Steve Macenski --- .../nav2_costmap_2d/costmap_filters/keepout_filter.hpp | 4 ++-- nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index df3e0b7ab27..a705c74f99a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -106,8 +106,8 @@ class KeepoutFilter : public CostmapFilter bool override_lethal_cost_{false}; // If true, lethal cost will be overridden unsigned char lethal_override_cost_{252}; // Value to override lethal cost with bool last_pose_lethal_{false}; // If true, last pose was lethal - unsigned int lethal_state_update_min_x_, lethal_state_update_min_y_; - unsigned int lethal_state_update_max_x_, lethal_state_update_max_y_; + unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u}; + unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 913baf95933..3d26a6edc41 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -261,8 +261,8 @@ void KeepoutFilter::process( if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { unsigned int mask_robot_i, mask_robot_j; if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { - auto keepout_mask_data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); - is_pose_lethal = (keepout_mask_data == 253 || keepout_mask_data == 254); + auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); + is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); if (is_pose_lethal) { RCLCPP_WARN_THROTTLE( logger_, *(clock_), 2000, From 85f9f50e9d87365f38ddf2deffb8a5bc9153bd0b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 May 2025 10:21:11 -0700 Subject: [PATCH 6/8] Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza Signed-off-by: Steve Macenski --- nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 3d26a6edc41..0ad5f5c65b1 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -79,6 +79,10 @@ void KeepoutFilter::initializeFilter( node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); declareParameter("lethal_override_cost", rclcpp::ParameterValue(252)); node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); + + // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given + lethal_override_cost_ = \ + std::clamp(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE); } void KeepoutFilter::filterInfoCallback( From c4098d59cff857c6d68bfe2390e03fb801d95d67 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 May 2025 10:21:21 -0700 Subject: [PATCH 7/8] Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza Signed-off-by: Steve Macenski --- nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 0ad5f5c65b1..97b0c73b6ff 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -77,7 +77,7 @@ void KeepoutFilter::initializeFilter( declareParameter("override_lethal_cost", rclcpp::ParameterValue(false)); node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); - declareParameter("lethal_override_cost", rclcpp::ParameterValue(252)); + declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE)); node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given From 15900ca03c1d87e441a17e1bb608ddf5bbe4dc09 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 May 2025 10:31:05 -0700 Subject: [PATCH 8/8] Update keepout_filter.cpp Signed-off-by: Steve Macenski --- nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 97b0c73b6ff..0f24178e36d 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -79,7 +79,7 @@ void KeepoutFilter::initializeFilter( node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE)); node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); - + // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given lethal_override_cost_ = \ std::clamp(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);