fix(behavior_path_planner): fix bug of turn signal with overlap lane (a…

…utowarefoundation#1886)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -347,7 +347,8 @@ boost::optional<size_t> findNearestSegmentIndex(
  */
 template <class T>
 double calcLateralOffset(
-  const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false)
+  const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx,
+  const bool throw_exception = false)
 {
   const auto overlap_removed_points = removeOverlapPoints(points, 0);
 
@@ -371,8 +372,6 @@ double calcLateralOffset(
     return std::nan("");
   }
 
-  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target);
-
   const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
   const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
 
@@ -383,6 +382,36 @@ double calcLateralOffset(
   return cross_vec(2) / segment_vec.norm();
 }
 
+template <class T>
+double calcLateralOffset(
+  const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false)
+{
+  const auto overlap_removed_points = removeOverlapPoints(points, 0);
+
+  if (throw_exception) {
+    validateNonEmpty(overlap_removed_points);
+  } else {
+    try {
+      validateNonEmpty(overlap_removed_points);
+    } catch (const std::exception & e) {
+      std::cerr << e.what() << std::endl;
+      return std::nan("");
+    }
+  }
+
+  if (overlap_removed_points.size() == 1) {
+    const std::runtime_error e("Same points are given.");
+    if (throw_exception) {
+      throw e;
+    }
+    std::cerr << e.what() << std::endl;
+    return std::nan("");
+  }
+
+  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target);
+  return calcLateralOffset(points, p_target, seg_idx, throw_exception);
+}
+
 /**
  * @brief calcSignedArcLength from index to index
  */

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -564,6 +564,67 @@ TEST(trajectory, calcLateralOffset)
   EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon);
 }
 
+TEST(trajectory, calcLateralOffset_without_segment_idx)
+{
+  using motion_utils::calcLateralOffset;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  const bool throw_exception = true;
+
+  // Empty
+  EXPECT_THROW(
+    calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception),
+    std::invalid_argument);
+
+  // Trajectory size is 1
+  {
+    const auto one_point_traj = generateTestTrajectory<Trajectory>(1, 1.0);
+    EXPECT_THROW(
+      calcLateralOffset(
+        one_point_traj.points, geometry_msgs::msg::Point{}, static_cast<size_t>(0),
+        throw_exception),
+      std::runtime_error);
+  }
+
+  // Same close points in trajectory
+  {
+    const auto invalid_traj = generateTestTrajectory<Trajectory>(10, 0.0);
+    const auto p = createPoint(3.0, 0.0, 0.0);
+    EXPECT_THROW(
+      calcLateralOffset(invalid_traj.points, p, static_cast<size_t>(2), throw_exception),
+      std::runtime_error);
+    EXPECT_THROW(
+      calcLateralOffset(invalid_traj.points, p, static_cast<size_t>(3), throw_exception),
+      std::runtime_error);
+  }
+
+  // Point on trajectory
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0), static_cast<size_t>(3)), 0.0,
+    epsilon);
+
+  // Point before start point
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0), static_cast<size_t>(0)), 3.0,
+    epsilon);
+
+  // Point after start point
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0), static_cast<size_t>(8)), -10.0,
+    epsilon);
+
+  // Random cases
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0), static_cast<size_t>(4)), 7.0,
+    epsilon);
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast<size_t>(0)), -3.0,
+    epsilon);
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast<size_t>(1)), -3.0,
+    epsilon);
+}
+
 TEST(trajectory, calcLateralOffset_CurveTrajectory)
 {
   using motion_utils::calcLateralOffset;

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -119,7 +119,8 @@ FrenetCoordinate3d convertToFrenetCoordinate3d(
     motion_utils::calcLongitudinalOffsetToSegment(pose_array, seg_idx, search_point_geom);
   frenet_coordinate.length =
     motion_utils::calcSignedArcLength(pose_array, 0, seg_idx) + longitudinal_length;
-  frenet_coordinate.distance = motion_utils::calcLateralOffset(pose_array, search_point_geom);
+  frenet_coordinate.distance =
+    motion_utils::calcLateralOffset(pose_array, search_point_geom, seg_idx);
 
   return frenet_coordinate;
 }