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.

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.