Add costmap source to collision monitor

[tonynajjar]

Feature request

Feature description

I know the collision monitor bypasses the costmap on purpose for faster response but for my application I care less about faster response and more about leveraging the persistence of some of the costmap layers because I lack a 360 degrees perception coverage.
So I'm proposing a CM source that does collision-checking against a costmap it subscribes to.
What do you think?

Implementation considerations

[SteveMacenski]

I'd merge a PR as long as the documentation in the readme and website warned against doing it for the reasons you know about (but that you can if needed)

[tonynajjar]

I gave it a shot here: angsa-robotics#7
So now it's an even simpler first issue for someone to integrate this into Nav2 main and to battle-test it against more scenarios (e.g. rolling/non-rolling costmaps)

[Lotusymt]

Hi all — I’d like to take this issue.

Progress:
Implemented a CostmapSource for Collision Monitor, adapted from @tonynajjar’s prototype (angsa-robotics#7).

Added unit tests (gtest):
Lethal cell → exactly one point at expected center.
Stale messages are ignored (timeout respected).
Cost threshold logic (254 vs 253) works.
Frame transform (map→base_link) is applied correctly.

Questions:

1. For tests: are unit tests sufficient, or would you prefer that I also add a small rosbag-driven launch test (deterministic, 10–15s bag) to exercise the data flow end-to-end in CI?
2. Any specific distro targets I should verify locally (Humble/Iron/Rolling)?

Next steps:

1. Add whichever integration test you prefer (or skip if unit tests are enough).
2. Update README/website to note trade-offs (latency vs persistence).
3. Add a brief CHANGELOG.rst entry and ensure linters pass.

Thanks—please let me know if I’m missing anything important for this PR.

[SteveMacenski]

We would love the help!

On distros, typically we add things to the main branch which targets rolling but can be compiled against jazzy and kilted. If the changes don’t break ABI, then we can backport!

For testing, unit and integration tests would be necessary. Right now we don’t have any rosbag-based testing in Nav2 so I would actually be very interested in your thoughts about a thoughtful design pattern that we could leverage with an example from this PR for the future - if you’re up for it!

[Lotusymt]

Thanks! Targeting main (Rolling) with ABI stability for Jazzy/Kilted. I’ll add unit + integration tests. I’d love to explore a rosbag-aided integration example and will share thoughts in a short outline by this weekend.

[Lotusymt]

Hi! I have a very early idea and wanted to ask if this sounds feasible and worth trying.

Outline:

1. Run a minimal full simulation with a moving obstacle so the local costmap changes over time, and check that CollisionMonitor (with the new CostmapSource) reacts correctly.
   Why this is different/useful: as far as I can tell, current CM checks are more preliminary and don’t explicitly validate CM behavior under a time-varying costmap driven by a moving obstacle. A dynamic scenario can surface issues that static tests miss (e.g., update cadence, TF timing/tolerance, QoS interactions, or costmap latency). Even if ROS reports message drops, having an end-to-end dynamic test helps catch real integration gaps where everything compiles, but the robot’s behavior isn’t robust. This is my understanding—please correct me if I missed something.

2. Keep it deterministic by default (repeatable runs). Later, we could add an optional seeded-random mode for local/nightly use (not merge-blocking).

3. Add a record-on-fail option: if an assertion fails, record a small rosbag (key topics only) and provide a simple replay entrypoint. Rationale: this exercises the full sensing → costmap → CM path and makes failures reproducible, not just logs.

If these directions seem reasonable, I’d really appreciate your suggestions (or pointers) on scope, topic list, and any pitfalls I should watch for. Thanks!

[Lotusymt]

Hi @SteveMacenski — just a quick follow-up 😊

I’ve verified ABI stability (no public changes) and rerun all nav2_collision_monitor tests — everything passes cleanly.
Just wanted to check whether you had a chance to review my earlier note about the integration test direction.

Thanks!

[SteveMacenski]

Not sure why this notification didn't appear in my tray last week, weird.

Run a minimal full simulation

These are definitely useful yes! I categorize these though as "smoke tests" since they are testing the full system to check if it all works (or fails). If it fails, that gives you the smoke that there's a fire somewhere, but not alot of information about where. This is a definite useful addition (see Nav2 System Tests) but not as a replacement for unit-level testing to make sure individual methods perform as you believe they should with a couple of edge situations, and system-level testing of the source used in the server with some notional inputs through the node ROS interfaces.

The broader steps that you propose are perhaps good for debugging live problems with a robot, however in a controlled environment like CI we should test things to make sure they work at a unit level (i.e. methods), at a system level (i.e. node), and optionally a smoke test with the entire system. If we test in sim --> wait for failure --> upload a rosbag, then some human in the loop has to actually download that bag and log files and debug by hand the failure. But, having this in addition to the system and unit testing could work well.

What I thought you were proposing was to record a rosbag of data from a simulation or a live robot and run that data through the collision monitor to check for proper handling, stoppages, slow downs, etc from the input data source. We don't have anywhere that we use previously recorded datasets in our test fixtures (Robot Localization has some for example) and that would be a neat addition into Nav2 if we had an architecture that we could use to develop additional tests for things like AMCL or data-centric processing pipelines (particularly around edge cases)

[Lotusymt]

Hi Steve — thanks for the detailed explanation, that really helped clarify the design rationale!
I’ve started drafting the dataset-based test following the Robot Localization example you mentioned. I’m narrowing v1 to CollisionMonitor(costmap source) with a small dataset fixture. I plan to include a minimal test harness (launch + a tiny publisher for /cmd_vel_in) to play the bag and evaluate CM’s output (/cmd_vel) over time.

For v1, I’ll use simple trajectory/metrics checks rather than a single snapshot, e.g.:

1. Time-to-stop after obstacle appears
2. Hold-stop % while obstacle persists
3. Time-to-resume after obstacle clears
4. No-false-stop in clean segments
5. Max overshoot during stop

Would this direction sound good to you? If so, I’ll proceed and open a Draft PR once it’s runnable to get your feedback before tightening thresholds and polishing the structure. Thanks again!