Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] authored and Ahmed Ebrahim committed Apr 22, 2024
1 parent 7e08443 commit 44e7c51
Showing 1 changed file with 5 additions and 7 deletions.
12 changes: 5 additions & 7 deletions planning/route_handler/src/route_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2267,7 +2267,7 @@ double RouteHandler::getRemainingDistance(const Pose & current_pose, const Pose

lanelet::ConstLanelet current_lanelet;
getClosestLaneletWithinRoute(current_pose, &current_lanelet);

lanelet::ConstLanelet goal_lanelet;
getGoalLanelet(&goal_lanelet);

Expand All @@ -2278,20 +2278,18 @@ double RouteHandler::getRemainingDistance(const Pose & current_pose, const Pose
remaining_shortest_path = optional_route->shortestPath();

for (auto & llt : remaining_shortest_path) {

if (remaining_shortest_path.size() == 1) {
remaining_distance += tier4_autoware_utils::calcDistance2d(current_pose.position, goal_pose_.position);
remaining_distance +=
tier4_autoware_utils::calcDistance2d(current_pose.position, goal_pose_.position);
break;
}

if (index == 0) {
lanelet::ArcCoordinates arc_coord =
lanelet::utils::getArcCoordinates({llt}, current_pose);
lanelet::ArcCoordinates arc_coord = lanelet::utils::getArcCoordinates({llt}, current_pose);
double this_lanelet_length = lanelet::utils::getLaneletLength2d(llt);
remaining_distance += this_lanelet_length - arc_coord.length;
} else if (index == (remaining_shortest_path.size() - 1)) {
lanelet::ArcCoordinates arc_coord =
lanelet::utils::getArcCoordinates({llt}, goal_pose_);
lanelet::ArcCoordinates arc_coord = lanelet::utils::getArcCoordinates({llt}, goal_pose_);
remaining_distance += arc_coord.length;
} else {
remaining_distance += lanelet::utils::getLaneletLength2d(llt);
Expand Down

0 comments on commit 44e7c51

Please sign in to comment.