Move projectState after getPointsInside

Signed-off-by: Brice <[email protected]>

nav2_collision_monitor/src/polygon.cpp

@@ -242,17 +242,17 @@ double Polygon::getCollisionTime(
 
   // 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
+    // If the collision occurred on this stage, return the actual collision time 
     // as if robot was moved with given velocity
     if (getPointsInside(points_transformed) >= min_points_) {
       return time;
     }
+    // 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);
   }
 
   // There is no collision