Skip to content

Keepout zone unable to change dynamically #5410

Closed
Closed
Keepout zone unable to change dynamically#5410
@mini-1235

Description

@mini-1235
mini-1235
opened on Jul 31, 2025

Bug report

Steps to reproduce issue

I am trying to dynamically change keepout zone by calling the service

 ros2 service call /keepout_filter_mask_server/load_map nav2_msgs/srv/LoadMap "{map_url: '/root/nav2_ws/src/navigation2/nav2_bringup/maps/depot_keepout_dup.yaml'}"

after this service is called, there should be a /keepout_filter_mask received

the log also shows that we have received it

[component_container_isolated-10] [INFO] [1753993363.636777201] [map_io]: Loading image_file: /root/nav2_ws/src/navigation2/nav2_bringup/maps/depot_keepout_dup.pgm
[component_container_isolated-10] [INFO] [1753993363.638780913] [map_io]: Read map /root/nav2_ws/src/navigation2/nav2_bringup/maps/depot_keepout_dup.pgm: 604 X 307 map @ 0.05 m/cell
[component_container_isolated-10] [WARN] [1753993363.639295950] [global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /keepout_filter_mask topic. Updating old filter mask.

however, the map remain unchanged after I call the service

Reproduction instructions

  1. Run tb4 demo
ros2 launch nav2_bringup tb4_simulation_launch.py headless:=0
  1. call the service
ros2 service call /keepout_filter_mask_server/load_map nav2_msgs/srv/LoadMap "{map_url: '/root/nav2_ws/src/navigation2/nav2_bringup/maps/depot_keepout_dup.yaml'}"

depot_keepout_dup.pgm is shown as follow

Image
  1. the keepout zone should be updated, however it remains unchanged

If I drive the robot near the keepout zone, it will partially change, although not entirely correct

Image

Additional information

I believe this is related to this part of code

navigation2/nav2_costmap_2d/src/layered_costmap.cpp

Lines 158 to 250 in c5531cb

minx_ = miny_ = std::numeric_limits<double>::max();
maxx_ = maxy_ = std::numeric_limits<double>::lowest();
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
plugin != plugins_.end(); ++plugin)
{
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
RCLCPP_WARN(
rclcpp::get_logger(
"nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
(*plugin)->getName().c_str());
}
}
for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
filter != filters_.end(); ++filter)
{
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
(*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
RCLCPP_WARN(
rclcpp::get_logger(
"nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending filter is %s",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
(*filter)->getName().c_str());
}
}
int x0, xn, y0, yn;
combined_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
combined_costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
x0 = std::max(0, x0);
xn = std::min(static_cast<int>(combined_costmap_.getSizeInCellsX()), xn + 1);
y0 = std::max(0, y0);
yn = std::min(static_cast<int>(combined_costmap_.getSizeInCellsY()), yn + 1);
RCLCPP_DEBUG(
rclcpp::get_logger(
"nav2_costmap_2d"), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
if (xn < x0 || yn < y0) {
return;
}
if (filters_.size() == 0) {
// If there are no filters enabled just update costmap sequentially by each plugin
combined_costmap_.resetMap(x0, y0, xn, yn);
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
plugin != plugins_.end(); ++plugin)
{
(*plugin)->updateCosts(combined_costmap_, x0, y0, xn, yn);
}
} else {
// Costmap Filters enabled
// 1. Update costmap by plugins
primary_costmap_.resetMap(x0, y0, xn, yn);
for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
plugin != plugins_.end(); ++plugin)
{
(*plugin)->updateCosts(primary_costmap_, x0, y0, xn, yn);
}
// 2. Copy processed costmap window to a final costmap.
// primary_costmap_ remain to be untouched for further usage by plugins.
if (!combined_costmap_.copyWindow(primary_costmap_, x0, y0, xn, yn, x0, y0)) {
RCLCPP_ERROR(
rclcpp::get_logger("nav2_costmap_2d"),
"Can not copy costmap (%i,%i)..(%i,%i) window",
x0, y0, xn, yn);
throw std::runtime_error{"Can not copy costmap"};
}
// 3. Apply filters over the plugins in order to make filters' work
// not being considered by plugins on next updateMap() calls
for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
filter != filters_.end(); ++filter)
{
(*filter)->updateCosts(combined_costmap_, x0, y0, xn, yn);
}
}

and I have managed to solve this problem by updating the min_x, min_y, max_x, max_y here

navigation2/nav2_costmap_2d/src/layered_costmap.cpp

Line 186 in c5531cb

(*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);

which would involve adding a subscriber to /keepout_mask in costmap_filter.cpp

I am open to other suggestions/solutions

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions