BinaryFilter resets unexpectedly during stable mask input #5368
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 22.04
- Computer:
- 13th Gen Intel NUC
- ROS2 Version:
- humble
- Version or commit hash:
- ros-humble-navigation21.1.17-1jammy.20241128.061340
- DDS implementation:
- Fast-RTPS
Steps to reproduce issue
This issue arises when using the BinaryFilter costmap plugin and the global costmap is reset during navigation. Even if the robot remains inside the filtered region and the mask data does not change, the filter temporarily switches to its default state, causing inconsistent behavior.
-
Configure a BinaryFilter plugin in the Nav2 costmap (
global_costmap) YAML configuration. Set it to change parameters when the robot enters a region where the mask value exceeds aflip_threshold. -
Create a binary filter mask (e.g., a
.pgmand.yamlpair) that defines a narrow region, such as a hallway. In this region, the pixel values are set above the flip threshold (e.g., mask_data = 80, flip_threshold = 40). -
Launch the robot with the full navigation stack. Make sure:
- The
/costmap_filter_infotopic is publishing a validCostmapFilterInfoMsgwith the correct filter index. - The
/binary_filter_masktopic is publishing the mask data.
- The
-
Allow the robot to enter the filtered region. The costmap logs should confirm that:
binary_stateis set totrue- Filtered parameters (e.g., reduced speed or inflation radius) are applied.
-
Let the system run then trigger a global costmap reset by sending a goal and blocking the robot’s path. This causes the costmap to clear and invokes BinaryFilter::resetFilter(). As a result, the filter incorrectly reverts to the default state, despite the robot still being in the filtered area and mask values remaining valid.
- During this reset, logs will show:
BinaryFilter: Resetting the filter to default state BinaryFilter: Switched off
- During this reset, logs will show:
-
After costmap topics are re-initialized, the filter is reactivated and the correct binary_state is restored:
BinaryFilter: Switched on BinaryFilter: Processed mask_data: 80, binary_state: 1
Expected behavior
Once the robot enters a region where mask_data > flip_threshold, the binary_state should remain true as long as the robot is in that region. The filter should not reset to the default state unless the mask data explicitly changes to values below the threshold.
Actual behavior
While the robot is in a filtered region and receiving mask data of value 80 consistently, the BinaryFilter unexpectedly resets. This causes the binary_state to go from true → false → true, which disrupts the robot’s speed and inflation settings.
Excerpt from log:
BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
...
BinaryFilter: Resetting the filter to default state
BinaryFilter: Switched off
...
BinaryFilter: Switched on
BinaryFilter: Processed mask_data: 80, binary_state: 1
The function responsible for this is in nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp
void BinaryFilter::resetFilter()
{
std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
RCLCPP_INFO(logger_, "BinaryFilter: Resetting the filter to default state");
changeState(default_state_);
filter_info_sub_.reset();
mask_sub_.reset();
if (binary_state_pub_) {
binary_state_pub_->on_deactivate();
binary_state_pub_.reset();
}
}
Additional information
Part of the log:
[planner_server-13] [WARN] [1752796380.667620561] [global_costmap.global_costmap]: BinaryFilter DEBUG: base_ = 0.00, multiplier_ = 1.00, expression = 80.00
[planner_server-13] [WARN] [1752796381.667958052] [global_costmap.global_costmap]: BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
[planner_server-13] [WARN] [1752796381.668021038] [global_costmap.global_costmap]: BinaryFilter DEBUG: base_ = 0.00, multiplier_ = 1.00, expression = 80.00
[planner_server-13] [INFO] [1752796382.182672390] [global_costmap.global_costmap]: BinaryFilter: Resetting the filter to default state
[planner_server-13] [INFO] [1752796382.182707998] [global_costmap.global_costmap]: BinaryFilter: Switched off
[planner_server-13] [INFO] [1752796382.185734726] [global_costmap.global_costmap]: BinaryFilter: Subscribing to "/costmap_filter_info" topic for filter info...
[planner_server-13] [INFO] [1752796382.187768878] [global_costmap.global_costmap]: BinaryFilter: Switched off
[planner_server-13] [INFO] [1752796382.196740754] [global_costmap.global_costmap]: BinaryFilter: Received filter info from /costmap_filter_info topic.
[planner_server-13] [INFO] [1752796382.196785878] [global_costmap.global_costmap]: BinaryFilter: Subscribing to "/binary_filter_mask" topic for filter mask...
[planner_server-13] [WARN] [1752796382.208530886] [global_costmap.global_costmap]: BinaryFilter: New filter mask arrived from /binary_filter_mask topic. Updating old filter mask.
[planner_server-13] [INFO] [1752796382.672191791] [global_costmap.global_costmap]: BinaryFilter: Switched on
[planner_server-13] [WARN] [1752796382.672280090] [global_costmap.global_costmap]: BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
[planner_server-13] [WARN] [1752796382.672291697] [global_costmap.global_costmap]: BinaryFilter DEBUG: base_ = 0.00, multiplier_ = 1.00, expression = 80.00
[planner_server-13] [WARN] [1752796383.672853053] [global_costmap.global_costmap]: BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
[planner_server-13] [WARN] [1752796383.672926617] [global_costmap.global_costmap]: BinaryFilter DEBUG: base_ = 0.00, multiplier_ = 1.00, expression = 80.00
and the corresponding output of ros2 topic echo /binary_state:
data: true
---
data: false
---
data: false
---
data: true
Here is a video that shows the inflation radius changing while the robot remains in the filtered area.
binary_filter_messup.webm
Things I tried:
To prevent the BinaryFilter from switching off incorrectly, I modified the resetFilter() function to use binary_state_ instead of default_state_. This change initially worked: the filter did not reset to off during a costmap reset, and binary_state_ remained true as expected.
However, after the reset, the filter still unexpectedly switched off. Below is a summary of the relevant logs:
[planner_server-13] [INFO] BinaryFilter: Switched on
[planner_server-13] [WARN] BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
[binary_filter_switch.py-1] [INFO] Binary state changed to: True
[planner_server-13] [WARN] BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
[binary_filter_switch.py-1] [INFO] Set inflation_layer.inflation_radius on /local_costmap/local_costmap = 0.22
[planner_server-13] [WARN] BinaryFilter: Processed mask_data: 80, binary_state: 1, flip_threshold: 40.00
[planner_server-13] [WARN] Planning algorithm GridBased failed to generate a valid path to (1.12, 0.11)
[planner_server-13] [INFO] Received request to clear entirely the global_costmap
[planner_server-13] [INFO] BinaryFilter: Resetting the filter to default state
[planner_server-13] [INFO] BinaryFilter: default_state_ = false
[planner_server-13] [INFO] BinaryFilter: binary_state_ = true
[planner_server-13] [INFO] BinaryFilter: Switched on
[planner_server-13] [INFO] BinaryFilter: Subscribing to "/costmap_filter_info" topic for filter info...
[planner_server-13] [INFO] BinaryFilter: Switched off <-- Unexpected reset to off
[planner_server-13] [INFO] BinaryFilter: Received filter info from /costmap_filter_info topic.
[planner_server-13] [INFO] BinaryFilter: Subscribing to "/binary_filter_mask" topic for filter mask...
[planner_server-13] [WARN] BinaryFilter: New filter mask arrived from /binary_filter_mask topic. Updating old filter mask.