@@ -91,14 +91,12 @@ bool CollisionChecker::isCollisionImminent(
9191
9292 // only forward simulate within time requested
9393 double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot ;
94- double simulation_distance_limit = carrot_dist;
9594 if (params_->min_distance_to_obstacle > 0.0 ) {
9695 max_allowed_time_to_collision_check = std::max (
9796 params_->max_allowed_time_to_collision_up_to_carrot ,
9897 params_->min_distance_to_obstacle / std::max (std::abs (linear_vel),
9998 params_->min_approach_linear_velocity )
10099 );
101- simulation_distance_limit = std::max (carrot_dist, params_->min_distance_to_obstacle );
102100 }
103101 int i = 1 ;
104102 while (i * projection_time < max_allowed_time_to_collision_check) {
@@ -112,11 +110,8 @@ bool CollisionChecker::isCollisionImminent(
112110 theta += projection_time * angular_vel;
113111 curr_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis (theta);
114112
115- // Stop simulating if we've gone past the simulation distance limit
116- // (max: carrot or min distance)
117- if (hypot (curr_pose.position .x - robot_xy.x ,
118- curr_pose.position .y - robot_xy.y ) > simulation_distance_limit)
119- {
113+ // check if past carrot pose, where no longer a thoughtfully valid command
114+ if (hypot (curr_pose.position .x - robot_xy.x , curr_pose.position .y - robot_xy.y ) > carrot_dist) {
120115 break ;
121116 }
122117
0 commit comments