feat(tier4_autoware_utils): add new insert point function (#1222)

* add new length function Signed-off-by: yutaka <purewater0901@gmail.com> * add test Signed-off-by: yutaka <purewater0901@gmail.com> * fix test Signed-off-by: yutaka <purewater0901@gmail.com> * add path with lane id funciton Signed-off-by: yutaka <purewater0901@gmail.com>
autowarefoundation · Jul 6, 2022 · 15bacdd · 15bacdd
1 parent 96acb49
commit 15bacdd
Show file tree

Hide file tree

Showing 4 changed files with 561 additions and 4 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
@@ -259,6 +259,42 @@ inline boost::optional<size_t> insertTargetPoint(
 
   return seg_idx;
 }
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @param insert_point_length length to insert point from the beginning of the points
+ * @param p_target point to be inserted
+ * @param points output points of trajectory, path, ...
+ * @return index of insert point
+ */
+template <>
+inline boost::optional<size_t> insertTargetPoint(
+  const double insert_point_length, const geometry_msgs::msg::Point & p_target,
+  std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
+  const double overlap_threshold)
+{
+  validateNonEmpty(points);
+
+  if (insert_point_length < 0.0) {
+    return boost::none;
+  }
+
+  // Get Nearest segment index
+  boost::optional<size_t> segment_idx = boost::none;
+  for (size_t i = 1; i < points.size(); ++i) {
+    const double length = calcSignedArcLength(points, 0, i);
+    if (insert_point_length <= length) {
+      segment_idx = i - 1;
+      break;
+    }
+  }
+
+  if (!segment_idx) {
+    return boost::none;
+  }
+
+  return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold);
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
@@ -667,6 +667,41 @@ inline boost::optional<size_t> insertTargetPoint(
 
   return seg_idx;
 }
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @param insert_point_length length to insert point from the beginning of the points
+ * @param p_target point to be inserted
+ * @param points output points of trajectory, path, ...
+ * @return index of insert point
+ */
+template <class T>
+inline boost::optional<size_t> insertTargetPoint(
+  const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points,
+  const double overlap_threshold = 1e-3)
+{
+  validateNonEmpty(points);
+
+  if (insert_point_length < 0.0) {
+    return boost::none;
+  }
+
+  // Get Nearest segment index
+  boost::optional<size_t> segment_idx = boost::none;
+  for (size_t i = 1; i < points.size(); ++i) {
+    const double length = calcSignedArcLength(points, 0, i);
+    if (insert_point_length <= length) {
+      segment_idx = i - 1;
+      break;
+    }
+  }
+
+  if (!segment_idx) {
+    return boost::none;
+  }
+
+  return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold);
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__TRAJECTORY__TRAJECTORY_HPP_
diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp
@@ -594,16 +594,19 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     auto traj_out = traj;
 
     const auto p_target = createPoint(10.0, 0.0, 0.0);
-    const auto insert_idx = insertTargetPoint(9U, p_target, traj_out.points);
+    const size_t segment_idx = 9U;
+    const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points);
 
     EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Empty
   {
     auto empty_traj = generateTestTrajectory(0, 1.0);
+    const size_t segment_idx = 0;
     EXPECT_THROW(
-      insertTargetPoint({}, geometry_msgs::msg::Point{}, empty_traj.points), std::invalid_argument);
+      insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points),
+      std::invalid_argument);
   }
 }
 
@@ -699,3 +702,243 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
     }
   }
 }
+
+TEST(trajectory, insertTargetPoint_PathWithLaneId_Length)
+{
+  using tier4_autoware_utils::calcArcLength;
+  using tier4_autoware_utils::calcDistance2d;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::findNearestSegmentIndex;
+  using tier4_autoware_utils::getPose;
+  using tier4_autoware_utils::insertTargetPoint;
+
+  const auto traj = generateTestTrajectory(10, 1.0);
+  const auto total_length = calcArcLength(traj.points);
+
+  // Insert
+  for (double x_start = 0.5; x_start < total_length; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Insert(Boundary condition)
+  for (double x_start = 0.0; x_start < total_length; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start + 1.1e-3, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Right on the terminal point
+  {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(9.0, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size());
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Insert(Quaternion interpolation)
+  for (double x_start = 0.25; x_start < total_length; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Not insert(Overlap base_idx point)
+  for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start + 1e-4, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size());
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+  }
+
+  // Not insert(Overlap base_idx + 1 point)
+  for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start - 1e-4, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size());
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
+    }
+  }
+
+  // Invalid target point(In front of the beginning point)
+  {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(-1.0, 0.0, 0.0);
+    const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points);
+
+    EXPECT_EQ(insert_idx, boost::none);
+  }
+
+  // Invalid target point(Behind the end point)
+  {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(10.0, 0.0, 0.0);
+    const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points);
+
+    EXPECT_EQ(insert_idx, boost::none);
+  }
+
+  // Invalid target point(Huge lateral offset)
+  {
+    testing::internal::CaptureStderr();
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(4.0, 10.0, 0.0);
+    const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points);
+
+    EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
+    EXPECT_EQ(insert_idx, boost::none);
+  }
+
+  // Empty
+  {
+    auto empty_traj = generateTestTrajectory(0, 1.0);
+    EXPECT_THROW(
+      insertTargetPoint(0.0, geometry_msgs::msg::Point{}, empty_traj.points),
+      std::invalid_argument);
+  }
+}