[Collision_monitor] Approach polygon time=0 step is not processed

[BriceRenaudeau]

Bug report

In the approach polygon process, the algorithm loops over simulation_time_step to find the collision time.
time start at 0.0 but the projectState(simulation_time_step_, pose, vel); is call directly therefor the time 0.0 polygon is not checked.

navigation2/nav2_collision_monitor/src/polygon.cpp

Lines 243 to 256 in bf291d7

	// Robot movement simulation
	for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
	// Shift the robot pose towards to the vel during simulation_time_step_ time interval
	// NOTE: vel is changing during the simulation
	projectState(simulation_time_step_, pose, vel);
	// Transform collision_points to the frame concerned with current robot pose
	points_transformed = collision_points;
	transformPoints(pose, points_transformed);
	// If the collision occurred on this stage, return the actual time before a collision
	// as if robot was moved with given velocity
	if (getPointsInside(points_transformed) >= min_points_) {
	return time;
	}
	}

This leads to a possible flaw in the safety. If the simulation_time_step or the velocity is too big, close obstacles in the polygon are not detected and the robot is not stopped.

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Iron
• Version or commit hash:
  • sources
• DDS implementation:
  • Cyclonne

Steps to reproduce issue

• Put a huge value in simulation_time_step like 1s
• Start a simulation with collision_monitor enabled
• Put an obstacle close to the robot
• Move toward the obstacle at full speed (teleop)

Expected behavior

The robot shouldn't move if there is an obstacle in its polygon

Actual behavior

The robot moves even if there is an obstacle in its polygon

Additional information

• We can add a check before the simulation loop.
• The time in the collision check actually starts at simulation_time_step_ and not 0.0.

[tonynajjar]

This makes sense to me, I think the most straightforward fix would be to add something like

    if (getPointsInside(collision_points) >= min_points_) {
      return 0.0;
    }

at the beginning of getCollisionTime. What do you think?

Adding a test for this would be great

[BriceRenaudeau]

That's what I was thinking about. (plus init time at simulation_time_step_ in the loop).

My only concern is that people may have used this bug as a feature to evade when the robot is touching a wall for example.

[tonynajjar]

My only concern is that people may have used this bug as a feature to evade when the robot is touching a wall for example.

This should have been implemented with #3313 afaik so I don't see the reason to keep this "feature" in that way

(plus init time at simulation_time_step_ in the loop

hmm, I'm not sure but I think this is not necessary since projectState first projection is at Time=0 + dt, no?
Alternatively to having an explicit check at time=0, we could initialize double time = -simulation_time_step_, although that looks a bit weird

[tonynajjar]

Actually, thinking about this again, I think there is no avoiding the collision monitor misbehaving if simulation_time_step is too big. Implementing what we just discussed would avoid moving if the robot is already in collision, but it would not avoid the collision if the robot is close to collision, simulation_time_step is big and the velocity sent is big enough.

It might still be worth it to check for collision at time=0 but essentially simulation_time_step still need to be small enough to avoid collisions reliably.

[BriceRenaudeau]

Yes, but at least, when the robot is in collision it is not pushing anymore.

hmm, I'm not sure but I think this is not necessary since projectState first projection is at Time=0 + dt, no?

The issue is that the value returned is time (0.0) not time+dt

Alternatively to having an explicit check at time=0, we could initialize double time = -simulation_time_step_, although that looks a bit weird

Yes it will look weird and it will return -simulation_time_step_ when already in collision.

A better solution is to make the projectState() AFTER the getPointsInside check:

• the time 0.0 is tested
• The return time is valid

[tonynajjar]

A better solution is to make the projectState() AFTER the getPointsInside

Agree! Feel free to open a PR

[SteveMacenski]

Seems rational, thanks for the thread on the European timezone. I have nothing further to add :-)