feat(tier4_autoware_utils): add new calcSignedArclength Function (#1092)

* feat(tier4_autoware_utils): add new calcSignedArclength Function

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix argument

Signed-off-by: yutaka <purewater0901@gmail.com>

common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp

@@ -256,7 +256,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t
  */
 template <class T>
 double calcSignedArcLength(
-  const T & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx)
+  const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx)
 {
   validateNonEmpty(points);
 
@@ -269,6 +269,29 @@ double calcSignedArcLength(
   return signed_length_on_traj - signed_length_src_offset;
 }
 
+/**
+ * @brief calcSignedArcLength from point to index with maximum distance and yaw threshold
+ */
+template <class T>
+boost::optional<double> calcSignedArcLength(
+  const T & points, const geometry_msgs::msg::Pose & src_pose, const size_t dst_idx,
+  const double max_dist = std::numeric_limits<double>::max(),
+  const double max_yaw = std::numeric_limits<double>::max())
+{
+  validateNonEmpty(points);
+
+  const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
+  if (!src_seg_idx) {
+    return boost::none;
+  }
+
+  const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_idx);
+  const double signed_length_src_offset =
+    calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position);
+
+  return signed_length_on_traj - signed_length_src_offset;
+}
+
 /**
  * @brief calcSignedArcLength from index to point
  */

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

@@ -497,6 +497,48 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex)
   EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(4.3, 7.0, 0.0), 2), -2.3, epsilon);
 }
 
+TEST(trajectory, calcSignedArcLengthFromPoseToIndex_DistThreshold)
+{
+  using tier4_autoware_utils::calcSignedArcLength;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Out of threshold
+  EXPECT_FALSE(calcSignedArcLength(traj.points, createPose(0.0, 0.6, 0.0, 0.0, 0.0, 0.0), 3, 0.5));
+
+  // On threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0,
+    epsilon);
+
+  // Within threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.4, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0,
+    epsilon);
+}
+
+TEST(trajectory, calcSignedArcLengthFromPoseToIndex_YawThreshold)
+{
+  using tier4_autoware_utils::calcSignedArcLength;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  const auto max_d = std::numeric_limits<double>::max();
+
+  // Out of threshold
+  EXPECT_FALSE(
+    calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.1), 3, max_d, 1.0));
+
+  // On threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.0), 3, max_d, 1.0), 3.0,
+    epsilon);
+
+  // Within threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.9), 3, max_d, 1.0), 3.0,
+    epsilon);
+}
+
 TEST(trajectory, calcSignedArcLengthFromIndexToPoint)
 {
   using tier4_autoware_utils::calcSignedArcLength;