@@ -2267,7 +2267,7 @@ double RouteHandler::getRemainingDistance(const Pose & current_pose, const Pose
2267
2267
2268
2268
lanelet::ConstLanelet current_lanelet;
2269
2269
getClosestLaneletWithinRoute (current_pose, ¤t_lanelet);
2270
-
2270
+
2271
2271
lanelet::ConstLanelet goal_lanelet;
2272
2272
getGoalLanelet (&goal_lanelet);
2273
2273
@@ -2278,20 +2278,18 @@ double RouteHandler::getRemainingDistance(const Pose & current_pose, const Pose
2278
2278
remaining_shortest_path = optional_route->shortestPath ();
2279
2279
2280
2280
for (auto & llt : remaining_shortest_path) {
2281
-
2282
2281
if (remaining_shortest_path.size () == 1 ) {
2283
- remaining_distance += tier4_autoware_utils::calcDistance2d (current_pose.position , goal_pose_.position );
2282
+ remaining_distance +=
2283
+ tier4_autoware_utils::calcDistance2d (current_pose.position , goal_pose_.position );
2284
2284
break ;
2285
2285
}
2286
2286
2287
2287
if (index == 0 ) {
2288
- lanelet::ArcCoordinates arc_coord =
2289
- lanelet::utils::getArcCoordinates ({llt}, current_pose);
2288
+ lanelet::ArcCoordinates arc_coord = lanelet::utils::getArcCoordinates ({llt}, current_pose);
2290
2289
double this_lanelet_length = lanelet::utils::getLaneletLength2d (llt);
2291
2290
remaining_distance += this_lanelet_length - arc_coord.length ;
2292
2291
} else if (index == (remaining_shortest_path.size () - 1 )) {
2293
- lanelet::ArcCoordinates arc_coord =
2294
- lanelet::utils::getArcCoordinates ({llt}, goal_pose_);
2292
+ lanelet::ArcCoordinates arc_coord = lanelet::utils::getArcCoordinates ({llt}, goal_pose_);
2295
2293
remaining_distance += arc_coord.length ;
2296
2294
} else {
2297
2295
remaining_distance += lanelet::utils::getLaneletLength2d (llt);
0 commit comments