feat(motion_utils): add calc interpolated point for path with lane id (…

…autowarefoundation#2897)

* feat(motion_utils): add calc interpolated point for path with lane id

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

* format

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

---------

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

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

@@ -18,6 +18,7 @@
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/math/constants.hpp"
 
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 
 #include <boost/optional.hpp>
@@ -44,6 +45,21 @@ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint(
   const double dist_threshold = std::numeric_limits<double>::max(),
   const double yaw_threshold = std::numeric_limits<double>::max());
 
+/**
+ * @brief An interpolation function that finds the closest interpolated point on the path from
+ * the given pose
+ * @param path input path
+ * @param target_pose target_pose
+ * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for
+ * twist information
+ * @return resampled path(poses)
+ */
+autoware_auto_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_

common/motion_utils/src/trajectory/interpolation.cpp

@@ -16,8 +16,11 @@
 
 #include "interpolation/linear_interpolation.hpp"
 #include "interpolation/zero_order_hold.hpp"
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 
@@ -89,4 +92,57 @@ TrajectoryPoint calcInterpolatedPoint(
 
   return interpolated_point;
 }
+
+PathPointWithLaneId calcInterpolatedPoint(
+  const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose,
+  const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold)
+{
+  if (path.points.empty()) {
+    PathPointWithLaneId interpolated_point{};
+    interpolated_point.point.pose = target_pose;
+    return interpolated_point;
+  } else if (path.points.size() == 1) {
+    return path.points.front();
+  }
+
+  const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    path.points, target_pose, dist_threshold, yaw_threshold);
+
+  // Calculate interpolation ratio
+  const auto & curr_pt = path.points.at(segment_idx);
+  const auto & next_pt = path.points.at(segment_idx + 1);
+  const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt.point, next_pt.point);
+  const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt.point, target_pose);
+  if (v1.length2() < 1e-3) {
+    return curr_pt;
+  }
+
+  const double ratio = v1.dot(v2) / v1.length2();
+  const double clamped_ratio = std::clamp(ratio, 0.0, 1.0);
+
+  // Interpolate
+  PathPointWithLaneId interpolated_point{};
+
+  // pose interpolation
+  interpolated_point.point.pose =
+    tier4_autoware_utils::calcInterpolatedPose(curr_pt.point, next_pt.point, clamped_ratio);
+
+  // twist interpolation
+  if (use_zero_order_hold_for_twist) {
+    interpolated_point.point.longitudinal_velocity_mps = curr_pt.point.longitudinal_velocity_mps;
+    interpolated_point.point.lateral_velocity_mps = curr_pt.point.lateral_velocity_mps;
+  } else {
+    interpolated_point.point.longitudinal_velocity_mps = interpolation::lerp(
+      curr_pt.point.longitudinal_velocity_mps, next_pt.point.longitudinal_velocity_mps,
+      clamped_ratio);
+    interpolated_point.point.lateral_velocity_mps = interpolation::lerp(
+      curr_pt.point.lateral_velocity_mps, next_pt.point.lateral_velocity_mps, clamped_ratio);
+  }
+
+  // heading rate interpolation
+  interpolated_point.point.heading_rate_rps = interpolation::lerp(
+    curr_pt.point.heading_rate_rps, next_pt.point.heading_rate_rps, clamped_ratio);
+
+  return interpolated_point;
+}
 }  // namespace motion_utils

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

@@ -27,6 +27,8 @@
 
 namespace
 {
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::createPoint;
@@ -82,6 +84,42 @@ T generateTestTrajectory(
   }
   return traj;
 }
+
+PathPointWithLaneId generateTestPathPoint(
+  const double x, const double y, const double z, const double theta = 0.0,
+  const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0)
+{
+  PathPointWithLaneId p;
+  p.point.pose = createPose(x, y, z, 0.0, 0.0, theta);
+  p.point.longitudinal_velocity_mps = vel_lon;
+  p.point.lateral_velocity_mps = vel_lat;
+  p.point.heading_rate_rps = heading_rate;
+  return p;
+}
+
+template <class T>
+T generateTestPath(
+  const size_t num_points, const double point_interval, const double vel_lon = 0.0,
+  const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0,
+  const double delta_theta = 0.0)
+{
+  using Point = typename T::_points_type::value_type;
+
+  T traj;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double theta = init_theta + i * delta_theta;
+    const double x = i * point_interval * std::cos(theta);
+    const double y = i * point_interval * std::sin(theta);
+
+    Point p;
+    p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta);
+    p.point.longitudinal_velocity_mps = vel_lon;
+    p.point.lateral_velocity_mps = vel_lat;
+    p.point.heading_rate_rps = heading_rate_rps;
+    traj.points.push_back(p);
+  }
+  return traj;
+}
 }  // namespace
 
 TEST(Interpolation, interpolate_path_for_trajectory)
@@ -307,3 +345,195 @@ TEST(Interpolation, interpolate_path_for_trajectory)
     EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon);
   }
 }
+
+TEST(Interpolation, interpolate_path_for_path)
+{
+  using motion_utils::calcInterpolatedPoint;
+
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1);
+    }
+
+    // Same points as the path point
+    {
+      const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose);
+
+      EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 3.0, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 1.5, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.3, epsilon);
+    }
+
+    // Random Point
+    {
+      const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose);
+
+      EXPECT_NEAR(result.point.pose.position.x, 2.5, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 2.5, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 1.25, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.25, epsilon);
+    }
+
+    // Random Point with zero order hold
+    {
+      const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose, true);
+
+      EXPECT_NEAR(result.point.pose.position.x, 2.5, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 2.0, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.25, epsilon);
+    }
+
+    // Initial Point
+    {
+      const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose);
+
+      EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon);
+    }
+
+    // Terminal Point
+    {
+      const auto target_pose = createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose);
+
+      EXPECT_NEAR(result.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.9, epsilon);
+    }
+
+    // Outside of initial point
+    {
+      const auto target_pose = createPose(-2.0, -9.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose);
+
+      EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon);
+    }
+
+    // Outside of terminal point
+    {
+      const auto target_pose = createPose(10.0, 5.0, 0.0, 0.0, 0.0, 0.0);
+      const auto result = calcInterpolatedPoint(path, target_pose);
+
+      EXPECT_NEAR(result.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+      EXPECT_NEAR(result.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(result.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(result.point.heading_rate_rps, 0.9, epsilon);
+    }
+  }
+
+  // Empty Point
+  {
+    const PathWithLaneId path;
+    const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0);
+    const auto result = calcInterpolatedPoint(path, target_pose);
+
+    EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon);
+    EXPECT_NEAR(result.point.pose.position.y, 5.0, epsilon);
+    EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+    EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon);
+    EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon);
+    EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon);
+  }
+
+  // One point
+  {
+    const auto path = generateTestPath<PathWithLaneId>(1, 1.0, 1.0, 0.5, 0.1);
+    const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0);
+    const auto result = calcInterpolatedPoint(path, target_pose);
+
+    EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+    EXPECT_NEAR(result.point.longitudinal_velocity_mps, 1.0, epsilon);
+    EXPECT_NEAR(result.point.lateral_velocity_mps, 0.5, epsilon);
+    EXPECT_NEAR(result.point.heading_rate_rps, 0.1, epsilon);
+  }
+
+  // Duplicated Points
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1);
+    }
+    path.points.at(4) = path.points.at(3);
+
+    const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0);
+    const auto result = calcInterpolatedPoint(path, target_pose);
+
+    EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon);
+    EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon);
+    EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon);
+    EXPECT_NEAR(result.point.longitudinal_velocity_mps, 3.0, epsilon);
+    EXPECT_NEAR(result.point.lateral_velocity_mps, 1.5, epsilon);
+    EXPECT_NEAR(result.point.heading_rate_rps, 0.3, epsilon);
+  }
+}