File tree Expand file tree Collapse file tree 1 file changed +7
-5
lines changed
nav2_regulated_pure_pursuit_controller/src Expand file tree Collapse file tree 1 file changed +7
-5
lines changed Original file line number Diff line number Diff line change @@ -426,14 +426,16 @@ double RegulatedPurePursuitController::findVelocitySignChange(
426426 return hypot (
427427 transformed_plan.poses [pose_id].pose .position .x ,
428428 transformed_plan.poses [pose_id].pose .position .y );
429- } else if (
429+ }
430+
431+ if (
430432 (hypot (oa_x, oa_y) == 0.0 &&
431- transformed_plan.poses [pose_id - 1 ].pose .orientation !=
432- transformed_plan.poses [pose_id].pose .orientation )
433+ transformed_plan.poses [pose_id - 1 ].pose .orientation !=
434+ transformed_plan.poses [pose_id].pose .orientation )
433435 ||
434436 (hypot (ab_x, ab_y) == 0.0 &&
435- transformed_plan.poses [pose_id].pose .orientation !=
436- transformed_plan.poses [pose_id + 1 ].pose .orientation ))
437+ transformed_plan.poses [pose_id].pose .orientation !=
438+ transformed_plan.poses [pose_id + 1 ].pose .orientation ))
437439 {
438440 // returning the distance since the points overlap
439441 // but are not simply duplicate points (e.g. in place rotation)
You can’t perform that action at this time.
0 commit comments