fix(start/goal_planner): fix addition of duplicate segments in calcBe…

…foreShiftedArcLength (#7902)

* fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update trajectory.hpp

Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

* Update trajectory.hpp

Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -991,34 +991,51 @@ calcCurvature<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
  * curvature calculation
  */
 template <class T>
-std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+std::vector<std::pair<double, std::pair<double, double>>> calcCurvatureAndSegmentLength(
+  const T & points)
 {
-  // Note that arclength is for the segment, not the sum.
-  std::vector<std::pair<double, double>> curvature_arc_length_vec;
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
+  // segment length is pair of segment length between {p1, p2} and {p2, p3}
+  std::vector<std::pair<double, std::pair<double, double>>> curvature_and_segment_length_vec;
+  curvature_and_segment_length_vec.reserve(points.size());
+  curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0));
   for (size_t i = 1; i < points.size() - 1; ++i) {
     const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1));
     const auto p2 = autoware::universe_utils::getPoint(points.at(i));
     const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1));
     const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3);
-    const double arc_length =
-      autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
-      autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1));
-    curvature_arc_length_vec.emplace_back(curvature, arc_length);
+
+    // The first point has only the next point, so put the distance to that point.
+    // In other words, assign the first segment length at the second point to the
+    // second_segment_length at the first point.
+    if (i == 1) {
+      curvature_and_segment_length_vec.at(0).second.second =
+        autoware::universe_utils::calcDistance2d(p1, p2);
+    }
+
+    // The second_segment_length of the previous point and the first segment length of the current
+    // point are equal.
+    const std::pair<double, double> arc_length{
+      curvature_and_segment_length_vec.back().second.second,
+      autoware::universe_utils::calcDistance2d(p2, p3)};
+
+    curvature_and_segment_length_vec.emplace_back(curvature, arc_length);
   }
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
 
-  return curvature_arc_length_vec;
+  // set to the last point
+  curvature_and_segment_length_vec.emplace_back(
+    0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0));
+
+  return curvature_and_segment_length_vec;
 }
 
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_planning_msgs::msg::PathPoint> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 /**

common/autoware_motion_utils/src/trajectory/trajectory.cpp

@@ -238,14 +238,14 @@ calcCurvature<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 //
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_planning_msgs::msg::PathPoint> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 //

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp

@@ -308,8 +308,19 @@ double ShiftPullOver::calcBeforeShiftedArcLength(
 
   double before_arc_length{0.0};
   double after_arc_length{0.0};
-  for (const auto & [k, segment_length] :
-       autoware::motion_utils::calcCurvatureAndArcLength(reversed_path.points)) {
+
+  const auto curvature_and_segment_length =
+    autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points);
+
+  for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) {
+    const auto & [k, segment_length_pair] = curvature_and_segment_length[i];
+
+    // If it is the last point, add the lengths of the previous and next segments.
+    // For other points, only add the length of the previous segment.
+    const double segment_length = i == curvature_and_segment_length.size() - 1
+                                    ? segment_length_pair.first
+                                    : segment_length_pair.first + segment_length_pair.second;
+
     // after shifted segment length
     const double after_segment_length =
       k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr);

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -423,8 +423,17 @@ double ShiftPullOut::calcBeforeShiftedArcLength(
   double before_arc_length{0.0};
   double after_arc_length{0.0};
 
-  for (const auto & [k, segment_length] :
-       autoware::motion_utils::calcCurvatureAndArcLength(path.points)) {
+  const auto curvatures_and_segment_lengths =
+    autoware::motion_utils::calcCurvatureAndSegmentLength(path.points);
+  for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) {
+    const auto & [k, segment_length_pair] = curvatures_and_segment_lengths.at(i);
+
+    // If it is the last point, add the lengths of the previous and next segments.
+    // For other points, only add the length of the previous segment.
+    const double segment_length = i == curvatures_and_segment_lengths.size() - 1
+                                    ? segment_length_pair.first + segment_length_pair.second
+                                    : segment_length_pair.first;
+
     // after shifted segment length
     const double after_segment_length =
       k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr);