Add exclusion zone functionality to collision monitor#6233
Conversation
- Introduced ExclusionZone class to mask out source points within defined zones. - Updated Source class to integrate exclusion zones and apply filtering during data retrieval. - Modified getData methods across various sources to use getDataImpl for improved clarity. - Enhanced activation and deactivation of exclusion zone visualization in CollisionMonitor. - Added configuration for exclusion zones in the Source class. Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
|
You know, funny enough this was on my local post-it todo list for sometime this quarter after I ran into very annoying things myself with this right before I left for vacation. You seem to be on a wavelength with me at times 😉 Don't you think this would be better implemented in the source base class itself rather than as an extra application-level object to handle? That way per-source you can have control over the exclusion zone(s) which could notionally be set differently? Also, it seems like alot of the polygon logic is shared with the actual polygon class. Maybe some of those can become statics to call and/or abstracted into utilities they can both share? Particularly parsing and transforming. Or maybe even using a polygon internally (?) though I don't know if that actually makes any sense Lots of untested code, but I'm sure you know that already. I like this idea for use for self-footprint exclusion and charging stations as you mention. Both should be documented in the forthcoming docs PR (maybe for the docking station a quick example of how to set that up) |
Love it 😄
I think it's already like that though - the exclusion zones can be specified per-source with the current implementation. The source base class exposes
Yeah there could be some sharing potential, I agree. Will look into it.
Yep, I avoid writing tests before we have the architecture nailed down |
I guess I missed that on a first glance! Wouldn't this better belong in the source though from a separation of concerns architecture level? If its in the base |
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
I'm still confused 😅 because the removal of points IS done in the source like you're suggesting. Here are some diagrams to help you visualize it: 
I added 2 commits now for more polygon-related code sharing + tests. I actually think you could give it a medium-effort architectural review already |
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
I see, I misread on first glance, I agree and largely happy with this!
|
|
||
| const std::vector<std::string> zone_names = | ||
| node->declare_or_get_parameter<std::vector<std::string>>( | ||
| source_name_ + ".exclusion_zones", std::vector<std::string>()); |
There was a problem hiding this comment.
Should we make this a dynamic parameter? Seems useful actually for the dock or other cases
There was a problem hiding this comment.
Adding and removing exclusion_zones from a source seems a little complex in terms of creation and cleanup - we can already dynamically enable/disable exclusion_zones with the dynamic enabled param, is that not enough?
Preempting the question: "What if we use the same exclusion zone for multiple sources but want to specifically disable the zone for one source?"
The quick solution would be two define two zones with the same points:
scan:
exclusion_zones: ["dock_scan"]
pointcloud:
exclusion_zones: ["dock_cloud"]
dock_scan:
type: polygon
frame_id: dock_pose
points: "[[0.2,0.65],[0.2,-0.65],[-0.4,-0.65],[-0.4,0.65]]"
enabled: true
dock_cloud:
type: polygon
frame_id: dock_pose
points: "[[0.2,0.65],[0.2,-0.65],[-0.4,-0.65],[-0.4,0.65]]"
enabled: false # independently toggled
There was a problem hiding this comment.
What if the number of docks is very large, they move, or these are based on live detections and not pre-known coordinates?
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
+ void Source::activate()
+ {
+ for (const auto & zone : exclusion_zones_) {
+ zone->activate();
+ }
+ }
+
+ void Source::deactivate()
+ {
+ for (const auto & zone : exclusion_zones_) {
+ zone->deactivate();
+ }
+ }
+
+ void Source::publishExclusionZones() const
+ {
+ for (const auto & zone : exclusion_zones_) {
+ zone->publish();
+ }
+ }
These lines seem important but there's never a zone in the loop to publish/activate/deactivate in the tests. That seems important to validate / exercise.
| std::make_move_iterator(source_data.begin()), | ||
| std::make_move_iterator(source_data.end())); |
There was a problem hiding this comment.
Is this... really necessary to use make_move_iterator? If so, sure keep and close my issue, but I don't think for this Point class that's necessary.
There was a problem hiding this comment.
No real necessity but it is technically more efficient because of the std::string source in Point - it makes insert move-construct each Point into data instead of copy-constructing and that's safe because source_data is throwaway.
It becomes worthwhile if source_data has many points.
|
|
||
| #### Exclusion Zones | ||
|
|
||
| Any data source can optionally define one or more **exclusion zones**. An exclusion zone is a region that **removes (masks out) that source's points which fall inside it**, before the action/detection polygons are evaluated. Unlike the polygons above, an exclusion zone does **not** trigger a behavior — it is a per-source pre-filter, and works for both the Collision Monitor and the Collision Detector. |
There was a problem hiding this comment.
Remove -. I don't want AI looking language in the docs. I don't actually take issue with the writing though otherwise.
|
|
||
| Any data source can optionally define one or more **exclusion zones**. An exclusion zone is a region that **removes (masks out) that source's points which fall inside it**, before the action/detection polygons are evaluated. Unlike the polygons above, an exclusion zone does **not** trigger a behavior — it is a per-source pre-filter, and works for both the Collision Monitor and the Collision Detector. | ||
|
|
||
| A typical use case is ignoring known structure the robot deliberately approaches, such as a charging dock or a conveyor: the returns from that structure would otherwise trip the stop/slowdown zones. A zone can be a polygon or a circle anchored to an arbitrary `frame_id` (e.g. `dock_link`), so it tracks that frame as the robot moves, with an optional height band for 3D sources. Two safety-relevant behaviors are worth noting: the filter is **fail-safe** (if the zone transform is unavailable, no points are removed, so protection is never silently lost), and each zone **inherits its source's `base_shift_correction` policy** so the mask and the points are always transformed under the same assumptions. |
There was a problem hiding this comment.
I'd also add a remark about self-collision filtering when points can see the chassis of the robot.
There was a problem hiding this comment.
This is a good set of language to add to the configuration guide perhaps as well when you open that PR
| /// @brief Whether source is enabled | ||
| bool enabled_; | ||
| /// @brief Exclusion zones masking out points from this source | ||
| std::vector<std::shared_ptr<ExclusionZone>> exclusion_zones_; |
There was a problem hiding this comment.
If there's not a good reason for these to be shared pointers, can this not just be a vector of zones?
There was a problem hiding this comment.
When using vector of zones the build fails with:
error: use of deleted function ‘ExclusionZone::ExclusionZone(const ExclusionZone&)’
...implicitly deleted because the default definition would be ill-formed:
error: use of deleted function ‘std::mutex::mutex(const std::mutex&)’
I think it's necessary to use a pointer because of std::mutex that has a deleted copy constructor and no move constructor
| min_height: 0.1 | ||
| max_height: 0.5 | ||
| enabled: true | ||
| # dock: |
There was a problem hiding this comment.
Add a self collision example too
| max_height: 0.5 | ||
| use_global_height: false | ||
| enabled: true | ||
| # dock: |
There was a problem hiding this comment.
Add a self collision example too
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
… exclusion zones Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
There was a problem hiding this comment.
Pull request overview
This PR introduces Exclusion Zones to nav2_collision_monitor: per-Source geometric masks (polygon or circle) that remove a source’s collision points inside the zone before action/detection polygons are evaluated. This supports use cases like filtering a docking station (or self-filtering robot structure) without disabling the entire sensor source.
Changes:
- Adds a new
ExclusionZoneprimitive with TF-aware masking, optional height banding, and optional visualization publishing. - Integrates exclusion-zone masking into
Source::getData()and updates all Source implementations to provide data viagetSourceData(). - Extends Collision Monitor/Detector visualization publishing to include exclusion zones, and adds tests + parameter examples.
Reviewed changes
Copilot reviewed 27 out of 27 changed files in this pull request and generated 5 comments.
Show a summary per file
| File | Description |
|---|---|
| nav2_collision_monitor/src/exclusion_zone.cpp | Implements exclusion-zone masking logic, TF bridging/hold behavior, dynamic parameter handling, and visualization publishing. |
| nav2_collision_monitor/include/nav2_collision_monitor/exclusion_zone.hpp | Declares the ExclusionZone API and configuration/behavior contract. |
| nav2_collision_monitor/src/source.cpp | Configures exclusion zones per source, adds masking in Source::getData(), and forwards zone visualization lifecycle. |
| nav2_collision_monitor/include/nav2_collision_monitor/source.hpp | Updates Source API: getData() becomes non-virtual and introduces getSourceData() plus zone visualization hooks. |
| nav2_collision_monitor/src/scan.cpp | Renames data retrieval override to getSourceData() for new Source contract. |
| nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp | Updates method signature to getSourceData() override. |
| nav2_collision_monitor/src/range.cpp | Renames data retrieval override to getSourceData() for new Source contract. |
| nav2_collision_monitor/include/nav2_collision_monitor/range.hpp | Updates method signature to getSourceData() override. |
| nav2_collision_monitor/src/pointcloud.cpp | Renames data retrieval override to getSourceData() for new Source contract. |
| nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp | Updates method signature to getSourceData() override. |
| nav2_collision_monitor/src/polygon_source.cpp | Renames data retrieval override to getSourceData() for new Source contract. |
| nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp | Updates method signature to getSourceData() override. |
| nav2_collision_monitor/src/costmap.cpp | Renames data retrieval override to getSourceData() for new Source contract. |
| nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp | Updates method signature to getSourceData() override. |
| nav2_collision_monitor/src/collision_monitor_node.cpp | Activates source zone publishers on lifecycle activate/deactivate and publishes zones alongside polygons. |
| nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp | Adds publishVisualizations() API. |
| nav2_collision_monitor/src/collision_detector_node.cpp | Activates source zone publishers on lifecycle activate/deactivate and publishes zones alongside polygons. |
| nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp | Adds publishVisualizations() API. |
| nav2_collision_monitor/include/nav2_collision_monitor/polygon_utils.hpp | Adds shared free-functions for polygon parsing, point transforms, and circle polygonization. |
| nav2_collision_monitor/src/polygon.cpp | Switches polygon parsing to polygon_utils::parsePolygonPoints() helper. |
| nav2_collision_monitor/src/circle.cpp | Switches circle polygon generation to polygon_utils::circleToPolygon() helper. |
| nav2_collision_monitor/test/exclusion_zone_test.cpp | Adds unit/integration tests for parsing, masking behavior, TF-hold behavior, visualization, and Source integration. |
| nav2_collision_monitor/test/CMakeLists.txt | Registers and links the new exclusion-zone gtest target. |
| nav2_collision_monitor/README.md | Documents Exclusion Zones conceptually and highlights fail-safe and TF behavior. |
| nav2_collision_monitor/params/collision_monitor_params.yaml | Adds commented parameter examples for defining exclusion zones and wiring them to a source. |
| nav2_collision_monitor/params/collision_detector_params.yaml | Adds commented parameter examples for defining exclusion zones and wiring them to a source. |
| nav2_collision_monitor/CMakeLists.txt | Adds src/exclusion_zone.cpp to both collision monitor and detector libraries. |
Comments suppressed due to low confidence (1)
nav2_collision_monitor/src/source.cpp:86
Sourcestores only a singlepost_set_params_handler_/on_set_params_handler_. At least one derived class (nav2_collision_monitor/src/pointcloud.cpp) assigns to these same members again, which drops the handle fromSource::configure()without removing the registered callback from the node. That can leave dangling parameter callbacks after destruction (use-after-free) and makes it unclear which callbacks are actually active.
// Add callback for dynamic parameters
post_set_params_handler_ = node->add_post_set_parameters_callback(
std::bind(
&Source::updateParametersCallback,
this, std::placeholders::_1));
| if (param_name == zone_name_ + ".radius" && | ||
| parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE && | ||
| parameter.as_double() <= 0.0) | ||
| { | ||
| result.successful = false; | ||
| result.reason = "radius must be > 0"; |
| } else if (param_name == zone_name_ + ".points" && // NOLINT | ||
| parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) | ||
| { | ||
| // Reject a live polygon update that cannot be parsed into a valid polygon | ||
| // so the running zone is never left with a malformed shape. | ||
| std::vector<Point> parsed; | ||
| std::string error; | ||
| if (!parsePolygonPoints(parameter.as_string(), 3, parsed, error)) { | ||
| result.successful = false; | ||
| result.reason = error; | ||
| } | ||
| } |
| } else if (param_name == zone_name_ + ".radius" && // NOLINT | ||
| parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) | ||
| { | ||
| radius_ = parameter.as_double(); | ||
| radius_squared_ = radius_ * radius_; | ||
| } else if (param_name == zone_name_ + ".min_height" && // NOLINT |
| // Activating exclusion zone visualization publishers | ||
| for (std::shared_ptr<Source> source : sources_) { | ||
| source->activate(); | ||
| } | ||
|
|
||
| // Since polygons are being published when cmd_vel_in appears, | ||
| // we need to publish polygons first time to display them at startup | ||
| publishPolygons(); |
Signed-off-by: Tony Najjar <tony.najjar@dexory.com>
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
Pedantic changes. Rebase once #6281 merges for CI to turn over and this should be an easy merge tomorrow
| void CollisionMonitor::publishPolygons() const | ||
| { | ||
| for (std::shared_ptr<Polygon> polygon : polygons_) { | ||
| if (polygon->getEnabled() || !enabled_) { | ||
| polygon->publish(); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| void CollisionMonitor::publishVisualizations() const | ||
| { | ||
| publishPolygons(); | ||
| for (std::shared_ptr<Source> source : sources_) { | ||
| source->publishExclusionZones(); | ||
| } | ||
| } |
There was a problem hiding this comment.
| } | |
| void CollisionMonitor::publishVisualizations() const | |
| { | |
| for (std::shared_ptr<Polygon> polygon : polygons_) { | |
| if (polygon->getEnabled() || !enabled_) { | |
| polygon->publish(); | |
| } | |
| } | |
| for (std::shared_ptr<Source> source : sources_) { | |
| source->publishExclusionZones(); | |
| } | |
| } |
Then simply remove publishPolygons
Just a suggestion, not a requirement
|
|
||
| void CollisionDetector::publishVisualizations() const | ||
| { | ||
| publishPolygons(); |
| std::string zone_name_; | ||
|
|
||
| /// @brief TF buffer | ||
| std::shared_ptr<tf2_ros::Buffer> tf_buffer_; |
There was a problem hiding this comment.
Note: #6281 replaces this with a new nav2 version of abuffer/listener/etc that should be used + allows main to compile in Lyrical, Jazzy, and Rolling
| * The zone frame is looked up at the latest available time (never curr_time), | ||
| * so a slowly published or flaky frame is not extrapolated into the future. | ||
| * The pose is accepted only while its age stays within the hold window | ||
| * (transform tolerance, extended by frame_hold_timeout_); it is then evaluated | ||
| * at its own stamp and bridged to the base frame at curr_time through the | ||
| * smooth global frame, so the zone stays put in the world as the robot moves. |
| return true; | ||
| } | ||
|
|
||
| // Runs on the safety thread, so all lookups are NON-BLOCKING (zero timeout): |
There was a problem hiding this comment.
Never use the word "safety" plz. Collision, critical, etc are ok
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Basic Info
Description of contribution in a few bullet points
ExclusionZoneprimitive tonav2_collision_monitor. An exclusion zone is a region that masks out source points falling inside it before the action polygons are evaluated. Unlike aPolygon, it produces no robot Action — it is purely a pre-filter on aSource's collision points.For context, our use case is filtering out the charging station when docking - we currently disable the CM sources completely but this has the drawback of being blind to obstacles during the docking procedure.
As a first step don't review it in great detail , it's a draft PR to get your feeling on it and if it's something that would sit well in the collision_monitor. If yes, I'll add tests and docs and review it thoroughly (most of it is AI)
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:
backport-*.cc @doisyg