diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst index 5b9d5a1a19..d4e8b2d51b 100644 --- a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -57,6 +57,14 @@ The data may be obtained from different data sources: - Laser scanners (``sensor_msgs::msg::LaserScan`` messages) - PointClouds (``sensor_msgs::msg::PointCloud2`` messages) - IR/Sonars (``sensor_msgs::msg::Range`` messages) +- Costmap (``nav2_msgs::msg::Costmap`` messages) + +.. warning:: + + **⚠️ 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. + Parameters ********** @@ -520,7 +528,7 @@ Observation sources parameters ============== ============================= Description: - Type of polygon shape. Could be ``scan``, ``pointcloud``, ``range`` or ``polygon``. + Type of polygon shape. Could be ``scan``, ``pointcloud``, ``range``, ``polygon`` or ``costmap``. :````.transport_type: @@ -639,6 +647,32 @@ Observation sources parameters Description: Maximum time interval in which source data is considered as valid. If no new data is received within this interval, the robot will be stopped. Setting ``source_timeout: 0.0`` disables this blocking mechanism. Overrides node parameter for each source individually, if desired. +:````.cost_threshold: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 253 + ============== ============================= + + Description: + For ``costmap`` sources only. Minimum cell cost (0–255) to be treated as an + obstacle. By default this matches inscribed/lethal cells (253–254) and ignores + lower-cost cells. + +:````.treat_unknown_as_obstacle: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool true + ============== ============================= + + Description: + For ``costmap`` sources only. If ``true``, cells with cost ``255`` (``NO_INFORMATION``) + will also be turned into obstacle points. Set to ``false`` if your costmap has + large unknown areas you don’t want to trigger Collision Monitor. + :bond_heartbeat_period: ============== ============================= @@ -774,3 +808,9 @@ Here is an example of configuration YAML for the Collision Monitor. max_height: 0.5 min_range: 0.2 enabled: True + # costmap: + # type: "costmap" # relative, respects namespaces + # topic: "local_costmap/costmap" + # cost_threshold: 254 + # enabled: True + # treat_unknown_as_obstacle: True diff --git a/migration/Kilted.rst b/migration/Kilted.rst index ca5522e48e..a4f4de6ead 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -477,3 +477,11 @@ After plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" desired_linear_vel: 1.0 lookahead_dist: 0.6 + +New CostmapSource observation type for Collision Monitor and CollisionDetector +------------------------------------------------------------------------------ + +In `PR #5642 `_, a new +``CostmapSource`` observation type was added to both Collision Monitor and +Collision Detector, allowing them to subscribe directly to +``nav2_msgs/msg/Costmap`` messages as an observation source. This source should be used with caution. diff --git a/tutorials/docs/using_3laws_supervisor.rst b/tutorials/docs/using_3laws_supervisor.rst index d82468954f..9e06524051 100644 --- a/tutorials/docs/using_3laws_supervisor.rst +++ b/tutorials/docs/using_3laws_supervisor.rst @@ -55,7 +55,7 @@ However, it has the same limitations due to the slow-and-stop nature of the solu :alt: Supervisor Solution Infographic :align: center -The Supervisor ROS uses a dynamic approach to collision avoidance to enhance safety while addressing the efficiency challenges and lightening the system tuning load. +The Supervisor ROS uses a dynamic approach to collision avoidance to enhance safety while addressing the efficiency challenges and reducing the system tuning load. Rather than solely stopping or slowing down when an obstacle is detected in proximity to the robot, the Supervisor takes over and adjusts the robot's trajectory in real-time to avoid collisions while still making progress towards its goal - also considering lower-level safety constraints described above if present (Pro version). This allows the robot to dynamically react to the environment while not prematurely stopping or slowing when maneuvers are available which can prevent triggering safety system reactions - increasing throughput. It will however still stop the robot is required as any other safety system might.