Add costmap source to collision monitor + dataset-based bag test (backport Jazzy) by arjangupta · Pull Request #5942 · ros-navigation/navigation2

arjangupta · 2026-02-06T04:14:53Z

Quick Preface

Backports #5642 into the Jazzy branch (merge conflicts resolved).

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

(none beyond maintainer checklist)

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 · 2026-02-06T04:15:29Z

@arjangupta, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @jazzy, but it must be in main
to have these changes reflected into new distributions.

arjangupta · 2026-02-06T04:19:09Z

@arjangupta, all pull requests must be targeted towards the main development branch. Once merged into main, it is possible to backport to @jazzy, but it must be in main to have these changes reflected into new distributions.

thank you, it is already in main and this is a backport - do I need to do anything differently?

…igation#5642) * Collision Monitor: add costmap sourced(unit tests) Signed-off-by: Mengting Yang <mengtiy5@uci.edu> * nav2_collision_monitor: modified CostmapSource + dataset-based bag test (v1) Signed-off-by: Mengting Yang <mengtiy5@uci.edu> * modified: nav2_collision_monitor/README.md Signed-off-by: Mengting Yang <mengtiy5@uci.edu> * - annotate files to indicate AI-assisted drafts Signed-off-by: Mengting Yang <mengtiy5@uci.edu> * collision_monitor: fix linter and address issues in the comment Signed-off-by: lotusymt <mengtiy5@uci.edu> * nav2_collision_monitor: fix lint, coverage, and collision_monitor_node_test.cpp Signed-off-by: lotusymt <mengtiy5@uci.edu> * Update nav2_collision_monitor/src/costmap.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> * Update nav2_collision_monitor/src/costmap.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> * Update nav2_collision_monitor/src/costmap.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> * Reverse format change Signed-off-by: lotusymt <mengtiy5@uci.edu> * Reverse format changes and comments Signed-off-by: lotusymt <mengtiy5@uci.edu> * Refactor in_range and count_stopped functions Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> Signed-off-by: lotusymt <mengtiy5@uci.edu> * added costmap source to collision detector and tests( for failure path and collision detector) Signed-off-by: lotusymt <mengtiy5@uci.edu> * fix configuration and test bugs Signed-off-by: lotusymt <mengtiy5@uci.edu> --------- Signed-off-by: Mengting Yang <mengtiy5@uci.edu> Signed-off-by: lotusymt <mengtiy5@uci.edu> Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Arjan Gupta <arjangupta95@gmail.com> Signed-off-by: Arjan Gupta <arjangupta@vermeer.com>

Preserve the jazzy branch's original CMake variable pattern Signed-off-by: Arjan Gupta Signed-off-by: Arjan Gupta <arjangupta@vermeer.com>

tonynajjar · 2026-02-06T07:09:38Z

@arjangupta looks like you have build issues to address, check CI

SteveMacenski

Needs updates; I don't think this would compile. Please do no submit pull requests until you have tested locally and validated that they work.

SteveMacenski · 2026-02-06T18:39:36Z

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

+  nav2_msgs::msg::Costmap::ConstSharedPtr data_;
+
+  /// @brief Subscription handle for the costmap topic.
+  nav2::Subscription<nav2_msgs::msg::Costmap>::SharedPtr data_sub_;


This doesn't exist in Jazzy

SteveMacenski · 2026-02-06T18:39:47Z

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

+#include <nav2_ros_common/lifecycle_node.hpp>
+#include <nav2_ros_common/subscription.hpp>


SteveMacenski · 2026-02-06T18:39:53Z

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

+   * @param base_shift_correction Whether to compensate robot motion during simulation checks.
+   */
+  CostmapSource(
+    const nav2::LifecycleNode::WeakPtr & node,


SteveMacenski · 2026-02-06T18:40:31Z

nav2_collision_monitor/test/collision_detector_node_test.cpp

+  nav2::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr
+    scan_pub_;
+  nav2::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
+    pointcloud_pub_;
+  nav2::Publisher<sensor_msgs::msg::Range>::SharedPtr
+    range_pub_;
+  nav2::Publisher<geometry_msgs::msg::PolygonInstanceStamped>::SharedPtr
+    polygon_source_pub_;
+  nav2::Publisher<nav2_msgs::msg::Costmap>::SharedPtr
+    costmap_pub_;


arjangupta · 2026-02-09T18:19:02Z

Needs updates; I don't think this would compile. Please do no submit pull requests until you have tested locally and validated that they work.

Thanks for the feedback Steve, sorry about the compilation issues.

I will close this for now and take some time to use the correct dependencies to bring in this feature into jazzy. I'll also compile and validate before opening a new PR.

arjangupta mentioned this pull request Feb 6, 2026

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

Merged

8 tasks

Lotusymt and others added 2 commits February 5, 2026 22:31

Improve a CMake dependencies merge conflict resolution

65d907a

Preserve the jazzy branch's original CMake variable pattern Signed-off-by: Arjan Gupta Signed-off-by: Arjan Gupta <arjangupta@vermeer.com>

arjangupta force-pushed the feature/costmap-source-main branch from 1b2ed1f to 65d907a Compare February 6, 2026 04:31

SteveMacenski requested changes Feb 6, 2026

View reviewed changes

arjangupta closed this Feb 9, 2026

arjangupta deleted the feature/costmap-source-main branch February 9, 2026 18:08

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add costmap source to collision monitor + dataset-based bag test (backport Jazzy)#5942

Add costmap source to collision monitor + dataset-based bag test (backport Jazzy)#5942
arjangupta wants to merge 2 commits intoros-navigation:jazzyfrom
arjangupta:feature/costmap-source-main

arjangupta commented Feb 6, 2026

Uh oh!

mergify bot commented Feb 6, 2026

Uh oh!

arjangupta commented Feb 6, 2026

Uh oh!

tonynajjar commented Feb 6, 2026 •

edited

Loading

Uh oh!

SteveMacenski left a comment •

edited

Loading

Uh oh!

SteveMacenski Feb 6, 2026

Uh oh!

SteveMacenski Feb 6, 2026

Uh oh!

SteveMacenski Feb 6, 2026

Uh oh!

SteveMacenski Feb 6, 2026

Uh oh!

arjangupta commented Feb 9, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

		#include <nav2_ros_common/lifecycle_node.hpp>
		#include <nav2_ros_common/subscription.hpp>

Conversation

arjangupta commented Feb 6, 2026

Quick Preface

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 Feb 6, 2026

Uh oh!

arjangupta commented Feb 6, 2026

Uh oh!

tonynajjar commented Feb 6, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

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!

SteveMacenski Feb 6, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Feb 6, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Feb 6, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Feb 6, 2026

Choose a reason for hiding this comment

Uh oh!

arjangupta commented Feb 9, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

tonynajjar commented Feb 6, 2026 •

edited

Loading

SteveMacenski left a comment •

edited

Loading