From cc3feac0967a0d317cff28fc6fddcbc443186234 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 16 Feb 2023 17:00:18 +0900 Subject: [PATCH 1/2] feat(motion_utils): add calc interpolated point for path with lane id Signed-off-by: Takayuki Murooka --- .../motion_utils/trajectory/interpolation.hpp | 16 ++ .../src/trajectory/interpolation.cpp | 55 +++++ .../src/trajectory/test_interpolation.cpp | 232 ++++++++++++++++++ 3 files changed, 303 insertions(+) diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index b05a5334b7328..6062337506a62 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -19,6 +19,7 @@ #include "tier4_autoware_utils/math/constants.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -44,6 +45,21 @@ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::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::max(), + const double yaw_threshold = std::numeric_limits::max()); + } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index cd603676ddd78..d9d2cad9f0c3b 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -17,9 +17,12 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; namespace motion_utils { @@ -89,4 +92,56 @@ 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 diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index f0e45179d05b6..5f83352acc048 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -29,6 +29,8 @@ namespace { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -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 +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,197 @@ 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(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); + } +} From 4a395f4e0067c140d2497bb624cec59e62abd472 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 16 Feb 2023 17:06:24 +0900 Subject: [PATCH 2/2] format Signed-off-by: Takayuki Murooka --- .../motion_utils/trajectory/interpolation.hpp | 2 +- .../motion_utils/src/trajectory/interpolation.cpp | 13 +++++++------ .../test/src/trajectory/test_interpolation.cpp | 14 ++++++-------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index 6062337506a62..815703ffbe354 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -18,8 +18,8 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index d9d2cad9f0c3b..4077202aba8a4 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -16,13 +16,13 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" -#include "motion_utils/trajectory/trajectory.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; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; namespace motion_utils { @@ -133,14 +133,15 @@ PathPointWithLaneId calcInterpolatedPoint( 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); + 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); + 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; } diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 5f83352acc048..73dc0a54ca689 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -27,10 +27,10 @@ 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 autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -100,8 +100,8 @@ PathPointWithLaneId generateTestPathPoint( template 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) + 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; @@ -354,8 +354,7 @@ TEST(Interpolation, interpolate_path_for_path) 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(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 @@ -519,8 +518,7 @@ TEST(Interpolation, interpolate_path_for_path) 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(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);