Collision Monitor Node

The Collision Monitor is a node providing an additional level of robot safety. It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level.

This is analogous to safety sensor and hardware features; take in laser scans from a real-time certified safety scanner, detect if there is to be an imminent collision in a configurable bounding box, and either emergency-stop the certified robot controller or slow the robot to avoid such collision. However, this node is done at the CPU level with any form of sensor. As such, this does not provide hard real-time safety certifications, but uses the same types of techniques with the same types of data for users that do not have safety-rated laser sensors, safety-rated controllers, or wish to use any type of data input (e.g. pointclouds from depth or stereo or range sensors).

This is a useful and integral part of large heavy industrial robots, or robots moving with high velocities, around people or other dynamic agents (e.g. other robots) as a safety mechanism for high-response emergency stopping. The costmaps / trajectory planners will handle most situations, but this is to handle obstacles that virtually appear out of no where (from the robot's perspective) or approach the robot at such high speed it needs to immediately stop to prevent collision.

See the package's README for more complete information. For more information how to bring-up your own Collision Monitor node, please refer to the Using Collision Monitor tutorial.

Also, the practical demonstration of Collision Monitor abilities presented at 6th ROS Developers Day 2023, could be found below:

Features

The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". Data that fall into these zones trigger an operation depending on the model being used. A given instance of the Collision Monitor can have many zones with different models at the same time. When multiple zones trigger at once, the most aggressive one is used (e.g. stop > slow 50% > slow 10%).

The following models of safety behaviors are employed by Collision Monitor:

Stop model: Define a zone and a point threshold. If min_points or more obstacle points appear inside this area, stop the robot until the obstacles will disappear.
Slowdown model: Define a zone around the robot and slow the maximum speed for a slowdown_ratio, if min_points or more points will appear inside the area.
Limit model: Define a zone around the robot and restricts the maximum linear and angular velocities to linear_limit and angular_limit values accordingly, if min_points or more points will appear inside the area.
Approach model: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than time_before_collision seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least time_before_collision seconds to collision. The effect here would be to keep the robot always time_before_collision seconds from any collision.

The zones around the robot can take the following shapes:

Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface.
Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time.
Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape.
VelocityPolygon: allow switching of polygons based on the command velocity. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s).

All shapes (Polygon, Circle and VelocityPolygon) are derived from base Polygon class, so without loss of generality they would be called as "polygons". Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.

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.

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 polygons are evaluated. Unlike the polygons above, an exclusion zone does not trigger a behavior, it is a per-source pre-filter. A typical use case is ignoring known structure the robot deliberately approaches, such as a charging dock or a conveyor, whose returns would otherwise trip the stop/slowdown zones. Another common use case is self-filtering: masking out returns from parts of the robot itself (e.g. arms, mast, bumpers, or trailers) that fall within a sensor's field of view, which would otherwise be mistaken for obstacles. Anchoring the zone to the relevant robot frame keeps the mask aligned with that structure as it moves. A zone can be a polygon or 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. The filter is fail-safe: if the zone transform is unavailable, no points are removed, so collision protection is never silently lost. Each zone inherits its owning source's base_shift_correction policy, so the mask and the source points are always transformed under the same assumptions. See YAML at the bottom for an example.

Parameters

`enabled`

Type: bool Default: true: Sets the initial state. This can come in handy when the robot is docked/inside any of the zones at startup and the node needs to be disabled then. Please note that is not a dynamic parameter, there's /toggle service interface and BT Node to update this state later at runtime.

`base_frame_id`

Type: string Default: "base_footprint": Robot base frame.

`odom_frame_id`

Type: string Default: "odom": Which frame to use for odometry.

`cmd_vel_in_topic`

Type: string Default: "cmd_vel_smoothed": Input cmd_vel topic with desired robot velocity.

`cmd_vel_out_topic`

Type: string Default: "cmd_vel": Output cmd_vel topic with output produced by Collision Monitor velocities.

`state_topic`

Type: string Default: "": Output the currently activated polygon action type and name. Optional parameter. No publisher will be created if it is unspecified.

`transform_tolerance`

Type: double Default: 0.1: Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.

`source_timeout`

Type: double Default: 2.0: 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. This parameter can be overridden per observation source.

`base_shift_correction`

Type: bool Default: true: Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time. If enabled, produces more accurate sources positioning in the robot base frame, at the cost of slower performance. This will cause average delays for ~1/(2*odom_rate) per each cmd_vel calculation cycle. However, disabling this option for better performance is not recommended for the fast moving robots, where during the typical rate of data sources, robot could move unacceptably far. Thus reasonable odometry rates are recommended (~100 hz).

`stop_pub_timeout`

Type: double Default: 1.0: Timeout, after which zero-velocity ceases to be published. It could be used for other overrode systems outside Nav2 are trying to bring the robot out of a state close to a collision, or to allow a standing robot to go into sleep mode.

`polygons`

Type: vector<string> Default: N/A: List of zones (stop/slowdown/limit bounding boxes, footprint, approach circle, etc...). Causes an error, if not specialized.

`observation_sources`

Type: vector<string> Default: N/A: List of data sources (laser scanners, pointclouds, etc...). Causes an error, if not specialized.

`use_realtime_priority`

Type: bool Default: false: Adds soft real-time prioritization to the controller server to better ensure resources to time sensitive portions of the codebase. This will set the controller's execution thread to a higher priority than the rest of the system (90) to meet scheduling deadlines to have less missed loop rates. To use this feature, you use set the following inside of /etc/security/limits.conf to give userspace access to elevated prioritization permissions: <username> soft rtprio 99 <username> hard rtprio 99

`enable_stamped_cmd_vel`

Type: bool Default: true: Whether to use geometry_msgs::msg::Twist or geometry_msgs::msg::TwistStamped velocity data. true uses TwistStamped, false uses Twist.

Polygons parameters

<polygon name> is the corresponding polygon name ID selected for this type.

`<polygon_name>.type`

Type: string Default: N/A: Type of polygon shape. Available values are "polygon", "circle". Causes an error, if not specialized.

`<polygon_name>.points`

Type: string Default: N/A: Polygon vertices, listed in "[[p1.x, p1.y], [p2.x, p2.y], [p3.x, p3.y], ...]" format (e.g. "[[0.5, 0.25], [0.5, -0.25], [0.0, -0.25], [0.0, 0.25]]" for the square in the front). Used for "polygon" type. Minimum 3 points for a triangle polygon. If not specified, the collision monitor will use dynamic polygon subscription to polygon_sub_topic for points in the "stop"/"slowdown"/"limit" action types, or footprint subscriber to footprint_topic for "approach" action type.

`<polygon_name>.polygon_sub_topic`

Type: string Default: N/A: For "polygon" type, topic to listen the polygon points from. For "circle" type, topic to listen the circle radius from. Applicable for "stop"/"slowdown"/"limit" action types. Causes an error if not specified and static polygon geometry (using parameter points for "polygon" type or radius for "circle" type) is also not specified. If both static polygon geometry and polygon_sub_topic are specified, the static parameter takes priority.

`<polygon_name>.footprint_topic`

Type: string Default: "local_costmap/published_footprint": Topic to listen the robot footprint from. Applicable only for "polygon" type and "approach" action type. If both points and footprint_topic are specified, the static points takes priority.

`<polygon_name>.polygon_subscribe_transient_local`

Type: bool Default: false: QoS durability setting for the incoming polygon or footprint topic subscription.

`<polygon_name>.radius`

Type: double Default: N/A: Circle radius. Used for "circle" type. If not specified, the collision monitor will use dynamic polygon subscription to polygon_sub_topic for circle radius in the "stop"/"slowdown"/"limit" action types.

`<polygon_name>.action_type`

Type: string Default: N/A: Zone behavior model. Available values are "stop", "slowdown", "limit", "approach". Causes an error, if not specialized.

`<polygon_name>.min_points`

Type: int Default: 4: Minimum number of data readings within a zone to trigger the action.

`<polygon_name>.trigger_consecutive_points`

Type: int Default: 1: Number of consecutive processing cycles with points_inside >= min_points required to enter the triggered state. A value of 1 means trigger in a single processing cycle.

`<polygon_name>.release_consecutive_points`

Type: int Default: 1: Number of consecutive processing cycles with points_inside < min_points required to leave the triggered state. A value of 1 means release in a single processing cycle. In practice, values greater than 1 can reduce sensor noise flicker while remaining responsive.

`<polygon_name>.slowdown_ratio`

Type: double Default: 0.5: Robot slowdown (share of its actual speed). Applicable for "slowdown" action type.

`<polygon_name>.linear_limit`

Type: double Default: 0.5: Robot linear speed limit. Applicable for "limit" action type.

`<polygon_name>.angular_limit`

Type: double Default: 0.5: Robot angular speed limit. Applicable for "limit" action type.

`<polygon_name>.time_before_collision`

Type: double Default: 2.0: Time before collision in seconds. Maximum simulation time used in collision prediction. Higher values mean lower performance. Applicable for "approach" action type.

`<polygon_name>.simulation_time_step`

Type: double Default: 0.1: Time iteration step for robot movement simulation during collision prediction. Higher values mean lower prediction accuracy but better performance. Applicable for "approach" action type.

`<polygon_name>.visualize`

Type: bool Default: false: Whether to publish the polygon in a separate topic.

`<polygon_name>.polygon_pub_topic`

Type: string Default: <polygon_name>: Topic name to publish a polygon to. Used only if visualize is true.

`<polygon_name name>.enabled`

Type: bool Default: true: Whether to use this polygon for collision monitoring. (Can be dynamically set)

VelocityPolygon parameters

All previous Polygon parameters apply, in addition to the following unique parameters for VelocityPolygon.

`<vel_poly>.holonomic`

Type: bool Default: false: Whether to use holonomic or non-holonomic robot model for collision prediction. For holonomic robot model, the resultant velocity will be used to compare the linear velocity range. Additionally, there will be 2 more parameters, direction_start_angle and direction_end_angle, to specify the resultant velocity direction.

`<vel_poly>.velocity_polygons`

Type: vector<string> Default: N/A: List of sub polygons for switching based on the robot's current velocity. When velocity is covered by multiple sub polygons, the first sub polygon in the list will be used. Causes an error, if not specified.

`<vel_poly>.<subpoly>.points`

Type: vector<string> Default: N/A: Polygon vertices, listed in "[[p1.x, p1.y], [p2.x, p2.y], [p3.x, p3.y], ...]" format (e.g. "[[0.5, 0.25], [0.5, -0.25], [0.0, -0.25], [0.0, 0.25]]" for the square in the front). Used for "polygon" type. Minimum 3 points for a triangle polygon. Causes an error, if not specified.

`<vel_poly>.<subpoly>.linear_min`

Type: double Default: N/A

Minimum linear velocity for the sub-polygon. Causes an error, if not specified.

Non-holonomic: This is the minimum signed velocity along the x-axis (allows negative values for reverse motion).
Holonomic: This is the minimum magnitude of the resultant velocity, which must be >= 0.0.

`<vel_poly>.<subpoly>.linear_max`

Type: double Default: N/A

Maximum linear velocity for the sub polygon. Causes an error, if not specified.

Non-holonomic: This is the maximum signed velocity along the x-axis. (allows negative values for reverse motion).
Holonomic: This is the maximum magnitude of the resultant velocity, which must be >= 0.0.

`<vel_poly>.<subpoly>.theta_min`

Type: double Default: N/A: Minimum angular velocity for the sub polygon. Causes an error, if not specified.

`<vel_poly>.<subpoly>.theta_max`

Type: double Default: N/A: Maximum angular velocity for the sub polygon. Causes an error, if not specified.

`<vel_poly>.<subpoly>.direction_start_angle`

Type: double Default: -PI: Start angle of the movement direction (for holonomic robot only). Refer to the Example section for the common configurations. Applicable for holonomic mode only.

`<vel_poly>.<subpoly>.direction_end_angle`

Type: double Default: PI: End angle of the movement direction (for holonomic robot only). Refer to the Example section for the common configurations. Applicable for holonomic mode only.

Observation sources parameters

<source name> is the corresponding data source name ID selected for this type.

`<source name>.type`

Type: string Default: "scan": Type of polygon shape. Could be "scan", "pointcloud", "range", "polygon" or "costmap".

`<source name>.transport_type`

Type: string Default: "raw"

For pointcloud data, specify the transport plugin to use:

"raw": No compression. Default; highest bandwidth usage.
"draco": Lossy compression via Google.
"zlib": Lossless compression via Zlib compression.
"zstd": Lossless compression via Zstd compression.

See the known transports for more details.

`<source name>.topic`

Type: string Default: "scan": Topic to listen the source data from.

`<source name>.min_height`

Type: double Default: 0.05: Minimum height the PointCloud projection to 2D space started from. Applicable for "pointcloud" type.

`<source name>.max_height`

Type: double Default: 0.5: Maximum height the PointCloud projection to 2D space ended with. Applicable for "pointcloud" type.

`<source name>.use_global_height`

Type: bool Default: false: Set true for pointcloud sources containing a "height" field relative to a real world ground contour. The "height" field will be used for the min and max height checks instead of the "z" field and will not be transformed as it is assumed that height is already global frame referenced. Applicable for "pointcloud" type.

`<source name>.min_range`

Type: double Default: 0.0: Minimum range threshold for PointCloud points. Points closer than this distance (measured as Euclidean distance from sensor origin) will be filtered out before processing. Useful for eliminating noise and invalid readings very close to the sensor. Applicable for "pointcloud" type.

`<source name>.obstacles_angle`

Type: double Default: PI / 180 (1 degree): Angle increment (in radians) between nearby obstacle points at the range arc. Two outermost points from the field of view are not taken into account (they will always exist regardless of this value). Applicable for "range" type.

`<source name>.sampling_distance`

Type: double Default: 0.1: Internally the polygon is sampled for collision detection. sampling_distance is the distance between sampled points of the polygon. Applicable for "polygon" source type.

`<source name>.enabled`

Type: bool Default: true: Whether to use this source for collision monitoring. (Can be dynamically set)

`<source name>.source_timeout`

Type: double Default: (node parameter source_timeout value): 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.

`<source name>.cost_threshold`

Type: int Default: 253: 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.

`<source name>.treat_unknown_as_obstacle`

Type: bool Default: true: 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.

`<source name>.exclusion_zones`

Type: vector<string> Default: [""]: List of exclusion zone name IDs defined for this source. Each name refers to a zone parameter block (see Exclusion zones parameters). Points from this source that fall inside an enabled zone are removed before the action polygons are evaluated.

Exclusion zones parameters

<zone name> is a parameter block referenced by name from a source's exclusion_zones list. Zone names are global across the node. Exclusion zones remove (mask out) a source's points and never trigger an action. Each zone inherits the owning source's base_shift_correction policy.

`<zone name>.type`

Type: string Default: "polygon": Type of zone shape. Available values are "polygon" and "circle".

`<zone name>.points`

Type: string Default: "": Zone polygon vertices, listed in "[[p1.x, p1.y], [p2.x, p2.y], [p3.x, p3.y], ...]" format, expressed in frame_id. Used for "polygon" type. Minimum 3 points. Causes an error, if invalid for a "polygon" zone.

`<zone name>.radius`

Type: double Default: N/A: Circle radius. Used for "circle" type. Must be greater than 0. Causes an error, if not specified for a "circle" zone.

`<zone name>.frame_id`

Type: string Default: (node parameter base_frame_id): Frame the zone shape is anchored to and tracked via TF (e.g. dock_link). Leaving it empty, or equal to the base frame, makes a static, robot-relative zone.

`<zone name>.frame_hold_timeout`

Type: double Default: 0.0: Extra time (in seconds) beyond transform_tolerance that the last known pose of a stale zone frame_id keeps being used before the zone fails safe and stops masking points. While held, the zone is frozen at its last valid pose in the odom_frame_id frame, so it stays world-fixed even if the robot moves. Useful to ride out brief detection dropouts of a marker-based zone frame. 0.0 means only the transform tolerance applies.

`<zone name>.min_height`

Type: double Default: -inf: Lower bound (in the base frame z) of the height band a point must be within to be masked. Unbounded by default so 2D sources are fully covered.

`<zone name>.max_height`

Type: double Default: +inf: Upper bound (in the base frame z) of the height band a point must be within to be masked. Unbounded by default so 2D sources are fully covered.

`<zone name>.enabled`

Type: bool Default: false: Whether this zone actively masks points. (Can be dynamically set)

`<zone name>.visualize`

Type: bool Default: false: Whether to publish the zone footprint as a geometry_msgs/PolygonStamped for visualization.

`bond_heartbeat_period`

Type: double Default: 0.25: The lifecycle node bond mechanism publishing period (on the /bond topic). Disabled if inferior or equal to 0.0.

`allow_parameter_qos_overrides`

Type: bool Default: true: Whether to allow QoS profiles to be overwritten with parameterized values.

Example

Here is an example illustrating the common configurations for holonomic robots that cover multiple directions of the resultant velocity:

Here is an example of configuration YAML for the Collision Monitor.

collision_monitor:
  ros__parameters:
    enabled: True
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.5
    source_timeout: 5.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    enable_stamped_cmd_vel: True
    use_realtime_priority: false
    polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"]
    PolygonStop:
      type: "circle"
      radius: 0.3
      action_type: "stop"
      min_points: 4
      visualize: True
      polygon_pub_topic: "polygon_stop"
      enabled: True
    PolygonSlow:
      type: "polygon"
      points: "[[1.0, 1.0], [1.0, -1.0], [-0.5, -1.0], [-0.5, 1.0]]"
      action_type: "slowdown"
      min_points: 4
      slowdown_ratio: 0.3
      visualize: True
      polygon_pub_topic: "polygon_slowdown"
      enabled: True
    PolygonLimit:
      type: "polygon"
      points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"
      action_type: "limit"
      min_points: 4
      linear_limit: 0.4
      angular_limit: 0.5
      visualize: True
      polygon_pub_topic: "polygon_limit"
      enabled: True
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 2.0
      simulation_time_step: 0.02
      min_points: 6
      visualize: False
      enabled: True
    VelocityPolygonStop:
      type: "velocity_polygon"
      action_type: "stop"
      min_points: 6
      visualize: True
      enabled: True
      polygon_pub_topic: "velocity_polygon_stop"
      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
      holonomic: false
      rotation:
        points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
        linear_min: 0.0
        linear_max: 0.05
        theta_min: -1.0
        theta_max: 1.0
      translation_forward:
        points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
        linear_min: 0.0
        linear_max: 1.0
        theta_min: -1.0
        theta_max: 1.0
      translation_backward:
        points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
        linear_min: -1.0
        linear_max: 0.0
        theta_min: -1.0
        theta_max: 1.0
      # This is the last polygon to be checked, it should cover the entire range of robot's velocities
      # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
      # is not covered by any of the other sub-polygons
      stopped:
        points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
        linear_min: -1.0
        linear_max: 1.0
        theta_min: -1.0
        theta_max: 1.0
    observation_sources: ["scan", "pointcloud"]
    scan:
      source_timeout: 0.2
      type: "scan"
      topic: "/scan"
      enabled: True
    pointcloud:
      type: "pointcloud"
      topic: "/intel_realsense_r200_depth/points"
      transport_type: "raw"  # raw or/ with compression (zlib, draco, zstd)
      min_height: 0.1
      max_height: 0.5
      min_range: 0.2
      enabled: True
      exclusion_zones: ["dock"]   # references the "dock" zone block below
    # Exclusion zone blocks are referenced by name from a source's "exclusion_zones" list.
    dock:
      enabled: True
      type: "polygon"          # "polygon" or "circle"
      frame_id: "dock_link"    # frame the zone is anchored to; empty -> robot base frame (static)
      points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"  # polygon type only
      # radius: 0.5            # circle type only (must be > 0)
      min_height: -1.0         # base-frame z band a point must be within to be masked
      max_height: 1.0
      visualize: True          # publish the zone footprint as a PolygonStamped
    # costmap:
    #   type: "costmap"   # relative, respects namespaces
    #   topic: "local_costmap/costmap"
    #   cost_threshold: 254
    #   enabled: True
    #   treat_unknown_as_obstacle: True