[Draft] CollisionMonitor: add CostmapSource + dataset-based bag test #5642

Lotusymt · 2025-10-23T07:27:48Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Fixes/Implements #4794
Primary OS tested on	Ubuntu 22.04 (ROS 2 Rolling)
Robotic platform tested on	unit tests + launch test (no physical robot)
Does this PR contain AI generated software?	Yes and it is marked inline in the code
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

Add CostmapSource to Collision Monitor to allow collision checks against a subscribed local costmap (nav2_msgs/Costmap).
Wire CostmapSource into CollisionMonitor::configureSources() and expose params:
- *.topic, *.cost_threshold (0–255), *.treat_unknown_as_obstacle (bool).
Provide minimal example in params/collision_monitor_params.yaml.
Update README.md:
- Add Costmap to the list of supported sources.
- Add a concise note about latency warning when using a costmap source.
Add dataset-based integration test (bag replay):
- test/collision_monitor_node_bag.cpp (metrics: time-to-stop, hold-stop%, time-to-resume, false-stop%).
- test/collision_monitor_node_bag.launch.py to bring up CM, play a tiny bag, and run gtest.
- Tiny mcap.zstd bag + test YAML installed for get_package_share_directory().

Description of documentation updates required from your changes

docs.nav2.org (separate PR): add a short .. warning:: box on the Collision Monitor page describing the trade-offs of using a costmap source (persistence vs. latency/staleness), and list the new parameters.
Ensure the new source and parameters are included in any parameter reference and tuning sections.
(This PR) README.md already updated with a brief note and source entry.

Description of how this change was tested

Unit/launch tests: Added a launch gtest that replays a tiny bag and checks CM behavior (time-to-stop, hold-stop%, time-to-resume, false-stop%). Ran colcon test --packages-select nav2_collision_monitor; existing unit tests + the new launch test pass on Ubuntu.
ABI: Verified that ABI was preserved.

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
Should this be backported to current distributions? If so, tag with backport-*.

mergify · 2025-10-23T08:29:11Z

@Lotusymt, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski

Checkout Linting / CI jobs that are failing. You may need to pull in main / rebase as well.

Please provide some comments inline in the test files so someone can understand what each are doing -- its a little difficult to read them and understand (1) fully what each do, (2) the difference in what one covers the other doesn't, and (3) the intent

But, by in large, this looks great!

nav2_collision_monitor/package.xml

SteveMacenski · 2025-10-23T16:33:46Z

nav2_collision_monitor/CMakeLists.txt

 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(rosgraph_msgs REQUIRED)


Add this to the build testing section

SteveMacenski · 2025-10-23T16:34:21Z

nav2_collision_monitor/README.md

+
+> **⚠️ when using CostmapSource**
+> Collision Monitor normally **bypasses the costmap** to minimize reaction latency using fresh sensor data.
+


Suggested change

> Use at your own caution or when using external costmap sources from derived sources.

SteveMacenski · 2025-10-23T16:35:10Z

nav2_collision_monitor/params/collision_monitor_params.yaml

      max_height: 0.5
      use_global_height: False
      enabled: True
+    costmap:


You can keep this in the file as an example, but please comment out so its not active. Also change the topic to local_costmap/costmap so namespaces works

SteveMacenski · 2025-10-23T16:37:37Z

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

+{
+public:
+  CostmapSource(
+


Remove extra sapce

SteveMacenski · 2025-10-23T16:40:00Z

nav2_collision_monitor/src/costmap.cpp

+  }
+  std::string source_topic;
+  getParameters(source_topic);
+  rclcpp::QoS qos = rclcpp::SystemDefaultsQoS();


Use Nav2 QoS profiles from nav2_ros_common

SteveMacenski · 2025-10-23T16:40:55Z

nav2_collision_monitor/src/costmap.cpp

+  nav2::declare_parameter_if_not_declared(
+      node, thresh_name, rclcpp::ParameterValue(253));
+  int v = node->get_parameter(thresh_name).as_int();


Use the new declare_or_get_parameter API in nav2::LIfecycleNode

SteveMacenski · 2025-10-23T16:42:11Z

nav2_collision_monitor/src/costmap.cpp

+      const int idx = y * meta.size_x + x;
+
+      const uint8_t c = cm.data[idx];
+      const bool is_obstacle = (c >= cost_threshold_ && c < 255) ||


Use the costmap values for UNKNOWN rather than hardcoding 255

SteveMacenski · 2025-10-23T16:42:37Z

nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py

+    def __init__(self):
+        super().__init__('fake_cm_bag_source')
+        # Use sim time so /clock drives timestamps
+        # self.set_parameters([Parameter('use_sim_time', value=True)])


Commented out?

SteveMacenski · 2025-10-23T16:45:06Z

nav2_collision_monitor/test/collision_monitor_node_bag.cpp

+    // Window where obstacle exists in the bag
+    stop_window_start_ = this->declare_parameter<double>("stop_window_start", 3.0);
+    stop_window_end_ = this->declare_parameter<double>("stop_window_end", 8.0);
+


Suggested change

Lotusymt · 2025-10-23T18:54:30Z

Thanks for the detailed review, Steve!
I’ll address the issues over the next couple of days and push updates soon.

SteveMacenski · 2025-10-23T18:55:28Z

Just so you're aware, I'm leaving tomorrow morning for ROSCon so it might be a little while (a week or so) before I give it another review, but it is not forgotten!

Signed-off-by: Mengting Yang <[email protected]>

…st (v1) Signed-off-by: Mengting Yang <[email protected]>

Signed-off-by: Mengting Yang <[email protected]>

Signed-off-by: lotusymt <[email protected]>

codecov · 2025-10-31T05:45:42Z

Codecov Report

❌ Patch coverage is 87.50000% with 6 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_collision_monitor/src/costmap.cpp	86.36%	6 Missing ⚠️

Files with missing lines	Coverage Δ
...2_collision_monitor/src/collision_monitor_node.cpp	`97.01% <100.00%> (+0.04%)`	⬆️
nav2_collision_monitor/src/costmap.cpp	`86.36% <86.36%> (ø)`

... and 13 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Lotusymt · 2025-10-31T08:40:45Z

Hi Steve,
I know this update may trigger a notification, but it’s not ready yet. I’m still investigating an unexpected test failure, so please skip this revision for now. I’ll ping you once it’s ready. Thanks!

Lotusymt mentioned this pull request Oct 23, 2025

Add costmap source to collision monitor #4794

Open

SteveMacenski reviewed Oct 23, 2025

View reviewed changes

Lotusymt added 5 commits October 30, 2025 05:34

Collision Monitor: add costmap sourced(unit tests)

1743667

Signed-off-by: Mengting Yang <[email protected]>

nav2_collision_monitor: modified CostmapSource + dataset-based bag te…

80bfe5c

…st (v1) Signed-off-by: Mengting Yang <[email protected]>

modified: nav2_collision_monitor/README.md

7bb7663

Signed-off-by: Mengting Yang <[email protected]>

- annotate files to indicate AI-assisted drafts

4203283

Signed-off-by: Mengting Yang <[email protected]>

collision_monitor: fix linter and address issues in the comment

fc27b4c

Signed-off-by: lotusymt <[email protected]>

Lotusymt force-pushed the feature/costmap-source-main branch from b4d7608 to fc27b4c Compare October 31, 2025 03:56


		> ⚠️ when using CostmapSource
		> Collision Monitor normally bypasses the costmap to minimize reaction latency using fresh sensor data.


	> Use at your own caution or when using external costmap sources from derived sources.

[Draft] CollisionMonitor: add CostmapSource + dataset-based bag test #5642

Are you sure you want to change the base?

[Draft] CollisionMonitor: add CostmapSource + dataset-based bag test #5642

Uh oh!

Conversation

Lotusymt commented Oct 23, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mergify bot commented Oct 23, 2025

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Lotusymt commented Oct 23, 2025

Uh oh!

SteveMacenski commented Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Oct 31, 2025

Codecov Report

Uh oh!

Lotusymt commented Oct 31, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented Oct 23, 2025 •

edited

Loading