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/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e2225196d72..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 @@ -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_{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 ac01861ef67..0f24178e36d 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -74,6 +74,15 @@ void KeepoutFilter::initializeFilter( std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); global_frame_ = layered_costmap_->getGlobalFrameID(); + + declareParameter("override_lethal_cost", rclcpp::ParameterValue(false)); + 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); } void KeepoutFilter::filterInfoCallback( @@ -149,7 +158,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 +253,47 @@ 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); + + // 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; + 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 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, + "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out."); + } + } + } + + // 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 +330,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()