Skip to content

Commit eb2235a

Browse files
SteveMacenskileander-dsouza
SteveMacenski
and
leander-dsouza
authored
Option to Reduce Lethal to High-Cost Navigable To Get Out of Keepout Zones if Wandered In (#5187)
* Adding toggle option of keepout zone Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Default off Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Join conditions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * spell check Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * copilot suggestions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update keepout_filter.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
1 parent 4cade8d commit eb2235a

File tree

3 files changed

+69
-6
lines changed

3 files changed

+69
-6
lines changed

‎nav2_bringup/params/nav2_params.yaml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -229,6 +229,8 @@ local_costmap:
229229
plugin: "nav2_costmap_2d::KeepoutFilter"
230230
enabled: KEEPOUT_ZONE_ENABLED
231231
filter_info_topic: "keepout_costmap_filter_info"
232+
override_lethal_cost: True
233+
lethal_override_cost: 200
232234
inflation_layer:
233235
plugin: "nav2_costmap_2d::InflationLayer"
234236
cost_scaling_factor: 3.0
@@ -281,6 +283,8 @@ global_costmap:
281283
plugin: "nav2_costmap_2d::KeepoutFilter"
282284
enabled: KEEPOUT_ZONE_ENABLED
283285
filter_info_topic: "keepout_costmap_filter_info"
286+
override_lethal_cost: True
287+
lethal_override_cost: 200
284288
speed_filter:
285289
plugin: "nav2_costmap_2d::SpeedFilter"
286290
enabled: SPEED_ZONE_ENABLED

‎nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,12 @@ class KeepoutFilter : public CostmapFilter
102102
nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;
103103

104104
std::string global_frame_; // Frame of current layer (master_grid)
105+
106+
bool override_lethal_cost_{false}; // If true, lethal cost will be overridden
107+
unsigned char lethal_override_cost_{252}; // Value to override lethal cost with
108+
bool last_pose_lethal_{false}; // If true, last pose was lethal
109+
unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u};
110+
unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u};
105111
};
106112

107113
} // namespace nav2_costmap_2d

‎nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp‎

Lines changed: 59 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,15 @@ void KeepoutFilter::initializeFilter(
7474
std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1));
7575

7676
global_frame_ = layered_costmap_->getGlobalFrameID();
77+
78+
declareParameter("override_lethal_cost", rclcpp::ParameterValue(false));
79+
node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_);
80+
declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE));
81+
node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_);
82+
83+
// clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
84+
lethal_override_cost_ = \
85+
std::clamp<unsigned int>(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);
7786
}
7887

7988
void KeepoutFilter::filterInfoCallback(
@@ -149,7 +158,7 @@ void KeepoutFilter::maskCallback(
149158
void KeepoutFilter::process(
150159
nav2_costmap_2d::Costmap2D & master_grid,
151160
int min_i, int min_j, int max_i, int max_j,
152-
const geometry_msgs::msg::Pose2D & /*pose*/)
161+
const geometry_msgs::msg::Pose2D & pose)
153162
{
154163
std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
155164

@@ -244,10 +253,47 @@ void KeepoutFilter::process(
244253
}
245254

246255
// unsigned<-signed conversions.
247-
unsigned const int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
248-
unsigned const int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
249-
unsigned const int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
250-
unsigned const int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
256+
unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
257+
unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
258+
unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
259+
unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
260+
261+
// Let's find the pose's cost if we are allowed to override the lethal cost
262+
bool is_pose_lethal = false;
263+
if (override_lethal_cost_) {
264+
geometry_msgs::msg::Pose2D mask_pose;
265+
if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
266+
unsigned int mask_robot_i, mask_robot_j;
267+
if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
268+
auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
269+
is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
270+
if (is_pose_lethal) {
271+
RCLCPP_WARN_THROTTLE(
272+
logger_, *(clock_), 2000,
273+
"KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
274+
}
275+
}
276+
}
277+
278+
// If in lethal space or just exited lethal space,
279+
// we need to update all possible spaces touched during this state
280+
if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) {
281+
lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_);
282+
mg_min_x_u = lethal_state_update_min_x_;
283+
lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_);
284+
mg_min_y_u = lethal_state_update_min_y_;
285+
lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_);
286+
mg_max_x_u = lethal_state_update_max_x_;
287+
lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_);
288+
mg_max_y_u = lethal_state_update_max_y_;
289+
} else {
290+
// If out of lethal space, reset managed lethal state sizes
291+
lethal_state_update_min_x_ = master_grid.getSizeInCellsX();
292+
lethal_state_update_min_y_ = master_grid.getSizeInCellsY();
293+
lethal_state_update_max_x_ = 0u;
294+
lethal_state_update_max_y_ = 0u;
295+
}
296+
}
251297

252298
unsigned int i, j; // master_grid iterators
253299
unsigned int index; // corresponding index of master_grid
@@ -284,12 +330,19 @@ void KeepoutFilter::process(
284330
if (data == NO_INFORMATION) {
285331
continue;
286332
}
333+
287334
if (data > old_data || old_data == NO_INFORMATION) {
288-
master_array[index] = data;
335+
if (override_lethal_cost_ && is_pose_lethal) {
336+
master_array[index] = lethal_override_cost_;
337+
} else {
338+
master_array[index] = data;
339+
}
289340
}
290341
}
291342
}
292343
}
344+
345+
last_pose_lethal_ = is_pose_lethal;
293346
}
294347

295348
void KeepoutFilter::resetFilter()

0 commit comments

Comments
 (0)