[RPP] Prevent collision check premature termination (#5598)

* prevent collision check premature termination

Signed-off-by: EricoMeger <ericomeger9@gmail.com>

* improve comment wording

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Érico Meger <86668447+EricoMeger@users.noreply.github.com>

* fix linting error

Signed-off-by: EricoMeger <ericomeger9@gmail.com>

---------

Signed-off-by: EricoMeger <ericomeger9@gmail.com>
Signed-off-by: Érico Meger <86668447+EricoMeger@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: EricoMeger

nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp

@@ -91,12 +91,14 @@ bool CollisionChecker::isCollisionImminent(
 
   // only forward simulate within time requested
   double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot;
+  double simulation_distance_limit = carrot_dist;
   if (params_->min_distance_to_obstacle > 0.0) {
     max_allowed_time_to_collision_check = std::max(
         params_->max_allowed_time_to_collision_up_to_carrot,
         params_->min_distance_to_obstacle / std::max(std::abs(linear_vel),
         params_->min_approach_linear_velocity)
     );
+    simulation_distance_limit = std::max(carrot_dist, params_->min_distance_to_obstacle);
   }
   int i = 1;
   while (i * projection_time < max_allowed_time_to_collision_check) {
@@ -110,8 +112,11 @@ bool CollisionChecker::isCollisionImminent(
     theta += projection_time * angular_vel;
     curr_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
 
-    // check if past carrot pose, where no longer a thoughtfully valid command
-    if (hypot(curr_pose.position.x - robot_xy.x, curr_pose.position.y - robot_xy.y) > carrot_dist) {
+    // Stop simulating if we've gone past the simulation distance limit
+    // (max: carrot or min distance)
+    if (hypot(curr_pose.position.x - robot_xy.x,
+        curr_pose.position.y - robot_xy.y) > simulation_distance_limit)
+    {
       break;
     }