[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

nav2_collision_monitor/CMakeLists.txt

nav2_collision_monitor/README.md

nav2_collision_monitor/params/collision_monitor_params.yaml

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

nav2_collision_monitor/src/costmap.cpp

nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py

nav2_collision_monitor/test/collision_monitor_node_bag.cpp

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 94.23077% with 3 lines in your changes missing coverage. Please review.

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

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

... and 20 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!

…e_test.cpp Signed-off-by: lotusymt <[email protected]>

nav2_collision_monitor/test/collision_monitor_node_test.cpp

Lotusymt · 2025-11-03T06:26:23Z

Hi @SteveMacenski, this PR is ready for review, and the follow-up docs PR has been opened: ros-navigation/docs.nav2.org#804. Thanks!

Lotusymt · 2025-11-10T19:53:57Z

Hi @SteveMacenski — friendly ping. All checks are green; docs in ros-navigation/docs.nav2.org#804. Let me know if you need any changes. Thanks!

tonynajjar · 2025-11-11T09:34:14Z

hi @Lotusymt, Steve is on vacation and will be back Nov 14

nav2_collision_monitor/src/collision_monitor_node.cpp

nav2_collision_monitor/src/costmap.cpp

nav2_collision_monitor/test/bags/cm_moving_obstacle/fake_cm_bag_source.py

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]>

mergify · 2025-11-18T03:06:52Z

@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).

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]>

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

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

Lotusymt · 2025-11-18T09:39:27Z

Hi Steve,

I’ve updated the PR to address the issues you pointed out:

Removed the coverage exclusion and the old comment.
Cleaned up the formatting you mentioned earlier.

Right now, the uncovered lines are mainly the branch that handles malformed /local_costmap/costmap data, which should not trigger in the normal “happy path”.

If you think we should explicitly cover this failure mode, I’m happy to add a small unit test that publishes invalid data and hits this branch. Please let me know your preference, and I can update the PR accordingly. Thank you!

SteveMacenski · 2025-11-18T17:40:55Z

If you think we should explicitly cover this failure mode, I’m happy to add a small unit test that publishes invalid data and hits this branch

That would be nice, yes. It should be easy by sending it a bogus costmap manually in the unit tests.

Also, do you think the costmap source should be added to the collision detector node as well?

Finally, just need to update the migration guide in ros-navigation/docs.nav2.org#804 and we should be ready to merge!

…h and collision detector) Signed-off-by: lotusymt <[email protected]>

Lotusymt · 2025-11-18T23:53:59Z

Thanks, Steve!

I’ve added the unit test to cover that branch.

I agree it’s more consistent to support CostmapSource in the collision
detector node as well. I’ve added CostmapSource there and included a
corresponding unit tests for the collision detector.

Finally, I’ve updated the migration guide in ros-navigation/docs.nav2.org#804
to mention the new CostmapSource option for both CollisionMonitor and
CollisionDetector.

Please let me know if you’d like any of these changes structured differently.

mergify · 2025-11-18T23:56:36Z

@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 · 2025-11-19T00:30:52Z

Got some errors here in the build:

--- stderr: nav2_collision_monitor                               
ld.lld: error: undefined reference due to --no-allow-shlib-undefined: nav2_collision_monitor::CostmapSource::CostmapSource(std::weak_ptr<nav2::LifecycleNode> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char>> const&, std::shared_ptr<tf2_ros::Buffer>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char>> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char>> const&, std::chrono::duration<long, std::ratio<1l, 1000000000l>> const&, rclcpp::Duration const&, bool)
>>> referenced by libcollision_detector_core.so

ld.lld: error: undefined reference due to --no-allow-shlib-undefined: nav2_collision_monitor::CostmapSource::configure()
>>> referenced by libcollision_detector_core.so
collect2: error: ld returned 1 exit status
gmake[2]: *** [CMakeFiles/collision_detector.dir/build.make:291: collision_detector] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:269: CMakeFiles/collision_detector.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---

Once that is resolved I approve the pair of PRs :-)

mini-1235 · 2025-11-19T11:10:53Z

nav2_collision_monitor/launch/collision_monitor_node.launch.py

 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
+# See the License for the specific languazge governing permissions and


I think this is unrelated to this PR and needs to be reverted

Thanks for catching that — I totally agree this change is unrelated to the PR.
I think it was accidentally modified earlier when I was experimenting with a small script to debug some linter issues.
I’ve now reverted the line back to the original license text, and the linter tests still pass.

mini-1235 · 2025-11-19T11:11:11Z

nav2_collision_monitor/launch/collision_detector_node.launch.py

                output='screen',
                emulate_tty=True,  # https://github.com/ros2/launch/issues/188
-                parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+                parameters=[{'autostart': autostart},


mini-1235 · 2025-11-19T11:11:38Z

nav2_collision_monitor/test/CMakeLists.txt

+install(FILES collision_monitor_node_bag.yaml
+  DESTINATION share/${PROJECT_NAME}/test)
+
+find_package(GTest REQUIRED)


Can we use ament_add_gtest instead?

Thanks for pointing this out!
I kept this as an add_executable on purpose: the binary is a gtest-based helper that’s launched from collision_monitor_node_bag.launch.py.
If we register it with ament_add_gtest, colcon test / CTest would try to run it standalone without that launch setup, so it would block waiting for /clock and /cmd_vel or fail with no data.

Ah, I think we can use ament_add_gtest_executable and ament_add_test instead,

navigation2/nav2_lifecycle_manager/test/CMakeLists.txt

Lines 20 to 36 in 60f3db2

ament_add_gtest_executable(test_bond_gtest

test_bond.cpp

)

target_link_libraries(test_bond_gtest

${library_name}

nav2_util::nav2_util_core

rclcpp::rclcpp

)

ament_add_test(test_bond

GENERATE_RESULT_FOR_RETURN_CODE_ZERO

COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py"

WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"

TIMEOUT 20

ENV

TEST_EXECUTABLE=$<TARGET_FILE:test_bond_gtest>

)

, no?

Good point, thank you for pointing that out! Your suggestion would definitely be more consistent with the existing style. Since this PR is already merged, I’ll keep the current version for now, but I’ll keep this pattern in mind for future tests.

mini-1235 · 2025-11-19T11:16:21Z

nav2_collision_monitor/src/costmap.cpp

+  // Cost threshold (0–255). 253 = inscribed 254 = lethal; 255 = NO_INFORMATION.
+  const auto thresh_name = source_name_ + ".cost_threshold";
+  // Minimal change (no range descriptor)
+  int v = node->declare_or_get_parameter<int>(thresh_name, 253);  // declare if missing, else get


Since there is a default value, I don't think the template argument <int> is necessary here

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

Lotusymt mentioned this pull request Oct 23, 2025

Add costmap source to collision monitor #4794

Closed

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

nav2_collision_monitor: fix lint, coverage, and collision_monitor_nod…

09d2f6a

…e_test.cpp Signed-off-by: lotusymt <[email protected]>

Lotusymt commented Nov 3, 2025

View reviewed changes

nav2_collision_monitor/test/collision_monitor_node_test.cpp Show resolved Hide resolved

Lotusymt marked this pull request as ready for review November 3, 2025 06:23

SteveMacenski requested changes Nov 17, 2025

View reviewed changes

Update nav2_collision_monitor/src/costmap.cpp

922e9de

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]>

Lotusymt and others added 4 commits November 17, 2025 20:24

Update nav2_collision_monitor/src/costmap.cpp

71c37d1

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]>

Update nav2_collision_monitor/src/costmap.cpp

a42379d

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Mengting Yang <[email protected]>

Reverse format change

b99dd8f

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

Reverse format changes and comments

04191ae

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

Lotusymt force-pushed the feature/costmap-source-main branch 2 times, most recently from b13d653 to 34dfb73 Compare November 18, 2025 09:23

Refactor in_range and count_stopped functions

5dd68d7

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

Lotusymt force-pushed the feature/costmap-source-main branch from 34dfb73 to 5dd68d7 Compare November 18, 2025 09:37

added costmap source to collision detector and tests( for failure pat…

50148b7

…h and collision detector) Signed-off-by: lotusymt <[email protected]>

SteveMacenski approved these changes Nov 19, 2025

View reviewed changes

SteveMacenski linked an issue Nov 19, 2025 that may be closed by this pull request

Add costmap source to collision monitor #4794

Closed

mini-1235 suggested changes Nov 19, 2025

View reviewed changes

Lotusymt force-pushed the feature/costmap-source-main branch from dbd5e04 to 7b6a842 Compare November 20, 2025 07:25

fix configuration and test bugs

a8e912f

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

Lotusymt force-pushed the feature/costmap-source-main branch from 7b6a842 to a8e912f Compare November 20, 2025 07:31

SteveMacenski approved these changes Nov 20, 2025

View reviewed changes

SteveMacenski merged commit b89bfab into ros-navigation:main Nov 20, 2025
22 checks passed

	ament_add_gtest_executable(test_bond_gtest
	test_bond.cpp
	)
	target_link_libraries(test_bond_gtest
	${library_name}
	nav2_util::nav2_util_core
	rclcpp::rclcpp
	)

	ament_add_test(test_bond
	GENERATE_RESULT_FOR_RETURN_CODE_ZERO
	COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py"
	WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
	TIMEOUT 20
	ENV
	TEST_EXECUTABLE=$<TARGET_FILE:test_bond_gtest>
	)

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

[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!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

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 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Lotusymt commented Oct 31, 2025

Uh oh!

Uh oh!

Lotusymt commented Nov 3, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Lotusymt commented Nov 10, 2025

Uh oh!

tonynajjar commented Nov 11, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mergify bot commented Nov 18, 2025

Uh oh!

Lotusymt commented Nov 18, 2025

Uh oh!

SteveMacenski commented Nov 18, 2025

Uh oh!

Lotusymt commented Nov 18, 2025

Uh oh!

mergify bot commented Nov 18, 2025

Uh oh!

SteveMacenski commented Nov 19, 2025

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!

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented Oct 23, 2025 •

edited

Loading

codecov bot commented Oct 31, 2025 •

edited

Loading

Lotusymt commented Nov 3, 2025 •

edited

Loading