feat(motion_utils): add new calcSignedArcLength (tier4#1701)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Oct 3, 2022 · 9f98afd · 9f98afd
1 parent 9071470
commit 9f98afd
Show file tree

Hide file tree

Showing 2 changed files with 177 additions and 0 deletions.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -1115,6 +1115,34 @@ double calcSignedArcLength(
   return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
 }
 
+template <class T>
+double calcSignedArcLength(
+  const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx,
+  const size_t dst_idx)
+{
+  validateNonEmpty(points);
+
+  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_point);
+
+  return signed_length_on_traj - signed_length_src_offset;
+}
+
+template <class T>
+double calcSignedArcLength(
+  const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point,
+  const size_t dst_seg_idx)
+{
+  validateNonEmpty(points);
+
+  const double signed_length_on_traj = calcSignedArcLength(points, src_idx, dst_seg_idx);
+  const double signed_length_dst_offset =
+    calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point);
+
+  return signed_length_on_traj + signed_length_dst_offset;
+}
+
 template <class T>
 size_t findFirstNearestIndexWithSoftConstraints(
   const T & points, const geometry_msgs::msg::Pose & pose,

diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp
@@ -4185,3 +4185,152 @@ TEST(trajectory, findFirstNearestIndexWithSoftConstraints)
     }
   }
 }
+
+TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex)
+{
+  using motion_utils::calcSignedArcLength;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Empty
+  EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0);
+
+  // Same point
+  {
+    const auto p = createPoint(3.0, 0.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, p, 2), 0, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p, 3, p, 3), 0, epsilon);
+  }
+
+  // Forward
+  {
+    const auto p1 = createPoint(0.0, 0.0, 0.0);
+    const auto p2 = createPoint(3.0, 1.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 2), 3, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 3), 3, epsilon);
+  }
+
+  // Backward
+  {
+    const auto p1 = createPoint(9.0, 0.0, 0.0);
+    const auto p2 = createPoint(8.0, 0.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 7), -1, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 8), -1, epsilon);
+  }
+
+  // Point before start point
+  {
+    const auto p1 = createPoint(-3.9, 3.0, 0.0);
+    const auto p2 = createPoint(6.0, -10.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 5), 9.9, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 6), 9.9, epsilon);
+  }
+
+  // Point after end point
+  {
+    const auto p1 = createPoint(7.0, -5.0, 0.0);
+    const auto p2 = createPoint(13.3, -10.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 6, p2, 8), 6.3, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, p2, 8), 6.3, epsilon);
+  }
+
+  // Point before start point and after end point
+  {
+    const auto p1 = createPoint(-4.3, 10.0, 0.0);
+    const auto p2 = createPoint(13.8, -1.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 18.1, epsilon);
+  }
+
+  // Random cases
+  {
+    const auto p1 = createPoint(1.0, 3.0, 0.0);
+    const auto p2 = createPoint(9.0, -1.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 8, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, p2, 8), 8, epsilon);
+  }
+  {
+    const auto p1 = createPoint(4.3, 7.0, 0.0);
+    const auto p2 = createPoint(2.0, 3.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 2), -2.3, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 1), -2.3, epsilon);
+  }
+}
+
+TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex)
+{
+  using motion_utils::calcSignedArcLength;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Empty
+  EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0);
+
+  // Same point
+  {
+    const auto p = createPoint(3.0, 0.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, 3), 0, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 3, p, 3), 0, epsilon);
+  }
+
+  // Forward
+  {
+    const auto p1 = createPoint(0.0, 0.0, 0.0);
+    const auto p2 = createPoint(3.0, 1.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 3), 3, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 2), 3, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 3), 3, epsilon);
+  }
+
+  // Backward
+  {
+    const auto p1 = createPoint(9.0, 0.0, 0.0);
+    const auto p2 = createPoint(8.0, 0.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, 8), -1, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 7), 0, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 8), 0, epsilon);
+  }
+
+  // Point before start point
+  {
+    const auto p1 = createPoint(-3.9, 3.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 6), 9.9, epsilon);
+  }
+
+  // Point after end point
+  {
+    const auto p2 = createPoint(13.3, -10.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 6.3, epsilon);
+  }
+
+  // Start point
+  {
+    const auto p1 = createPoint(0.0, 3.0, 0.0);
+    const auto p2 = createPoint(5.3, -10.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 5), 5, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 5), 5.3, epsilon);
+  }
+
+  // Point after end point
+  {
+    const auto p1 = createPoint(7.3, -5.0, 0.0);
+    const auto p2 = createPoint(9.0, -10.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, 9), 1.7, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 2.0, epsilon);
+  }
+
+  // Random cases
+  {
+    const auto p1 = createPoint(1.0, 3.0, 0.0);
+    const auto p2 = createPoint(9.0, -1.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 9), 8, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, 9), 8, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 1, p2, 8), 8, epsilon);
+  }
+  {
+    const auto p1 = createPoint(4.3, 7.0, 0.0);
+    const auto p2 = createPoint(2.3, 3.0, 0.0);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, 2), -2.3, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 2), -1.7, epsilon);
+    EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 1), -1.7, epsilon);
+  }
+}