fix: change remaning distance when the size of route is 1 (autowarefo…

…undation#8852) Signed-off-by: shulanbushangshu <[email protected]>
tudelft-iv · Oct 30, 2024 · f3886e8 · f3886e8
1 parent e58a158
commit f3886e8
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/...toware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/...toware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp
@@ -143,8 +143,9 @@ void RemainingDistanceTimeCalculatorNode::calculate_remaining_distance()
 
   for (auto & llt : remaining_shortest_path) {
     if (remaining_shortest_path.size() == 1) {
-      remaining_distance_ += autoware::universe_utils::calcDistance2d(
-        current_vehicle_pose_.position, goal_pose_.position);
+      auto arc_coord_cur = lanelet::utils::getArcCoordinates({llt}, current_vehicle_pose_);
+      auto arc_coord_goal = lanelet::utils::getArcCoordinates({llt}, goal_pose_);
+      remaining_distance_ += arc_coord_goal.length - arc_coord_cur.length;
       break;
     }