Collision monitor brakes for walls the robot is intentionally driving up to

[Jakubach]

Feature request

Feature description

We want to use the approach polygon as our main "slow down and stop before obstacles" mechanism, with a 2-3 s time_before_collision, so the robot brakes early and smoothly when a person or an object shows up on its path. What blocks this today: our robot maneuvers close to the walls, so a long projection triggers on the walls constantly. Currently the only possibility is shrinking time_before_collision, but it cancels stopping before a person or an object on the path. Controllers already handle much of this through the costmap, but their behavior near obstacles is emergent from tuning - and a stop-and-wait behavior with e.g. MPPI needs additional BT logic anyway - while the approach polygon gives an explicit time-to-collision guarantee.

We are aware of the nav_to_pose_and_pause_near_goal_obstacle pattern, but PathLongerOnApproach only fires within prox_len of the goal and requires the planner to return a >= length_factor longer path to the exact same goal pose (we want a case with navigation through multiple goals).

What do we think about making the approach action path- or map-aware?
With either in place, the full "slow down, stop, wait, continue" behavior could be easily added with a small BT node: if collision_monitor_state shows the approach polygon active for more than X seconds, the application replans or skips goals instead of waiting forever. Two possible solutions:

1. Project the approach polygon along the path. Optional per-polygon mode: sweep the footprint along the upcoming poses of the path, up to time_before_collision * current speed. CM subscribes to transformed_global_plan; if the path is stale or missing (e.g. a behavior is driving instead of the controller), the current projection with the full time_before_collision applies.

FootprintApproach:
  type: "polygon"
  action_type: "approach"
  footprint_topic: "/local_costmap/published_footprint"
  time_before_collision: 2.5
  simulation_time_step: 0.02
  project_along_path: true        # new; false (default) = current straight-line projection
path_topic: "/transformed_global_plan"   # new, node-level
path_timeout: 0.5   # same semantics as source_timeout; 0.0 disables the staleness check

or

2. Extend ExclusionZone (Add exclusion zone functionality to collision monitor #6233) to take the static map as input. Today zones are hand-declared polygons. Optionally feeding it with a map layer (e.g. the static layer) would mask source points coinciding with its occupied cells, so approach only reacts to obstacles that are not in the map. Localization drift could mask a real wall - so it has to be opt-in with a tolerance parameter.

Implementation considerations

For 1: not sure if this should be a parameter of approach or a new action type next to it (e.g. path_approach).

P.S. happy to write the PR - which of the two would you prefer, or is there a better way?

[SteveMacenski]

What blocks this today: our robot maneuvers close to the walls, so a long projection triggers on the walls constantly. Currently the only possibility is shrinking time_before_collision, but it cancels stopping before a person or an object on the path.

How do you propose to identify between wall and human to better handle this case? I see 3 potential strategies:

(1) you detect wall vs non-wall and you only pass in non-wall detections some X distance in the future for use in the collision monitor to in-effect filter out future irrelevant sensor data you do not care about, or

(2) consider a trajectory into the future rather than a single cmd_vel command if using something like MPPI that generates a non-trivial future trajectory (DWB/RPP will both be constant curves). We have an optimal trajectory topic we can listen to to use rather than assuming a constant application of the current cmd_vel. Do you use MPPI? I suppose similar could apply to a global path, but that is not as 'safe' since that is not a future prediction necessarily of what the robot actually plans to do - just what the guide is, or,

(3) as you mention, reduce the time before collision.

Project the approach polygon along the path. Optional per-polygon mode: sweep the footprint along the upcoming poses of the path, up to time_before_collision * current speed. CM subscribes to transformed_global_plan; if the path is stale or missing (e.g. a behavior is driving instead of the controller), the current projection with the full time_before_collision applies.

That doesn't seem entirely a bad idea, but was not really the design intent of this software for the reasons above. I would ask you to do a little leg work and look at the docs for the safety lidar systems like from SICK or PILZ and check what they do for this kind of thing. I would ideally like to mimic those as closely as reasonably possible to provide a similar non-certified technique.

Extend ExclusionZone to take the static map as input. Today zones are hand-declared polygons. Optionally feeding it with a map layer (e.g. the static layer) would mask source points coinciding with its occupied cells, so approach only reacts to obstacles that are not in the map. Localization drift could mask a real wall - so it has to be opt-in with a tolerance parameter.

That's kind of simila to my (1) but instead of pre-filtering sensor data, you're pre-annotating zones / using the static map. I'd ask you consider the pre-filtering idea first since I think its better and independent of localization quality.