Skip to content

Commit 62ebc54

Browse files
Revert "[RPP] Prevent collision check premature termination (#5598)" (#5620)
This reverts commit 6bc74e5.
1 parent 01580bd commit 62ebc54

File tree

1 file changed

+2
-7
lines changed

1 file changed

+2
-7
lines changed

nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)