From a572a2ba6b5e473e342cd2ef52840de6d845679b Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 31 Aug 2022 15:28:27 +0900 Subject: [PATCH] add tests Signed-off-by: yutaka --- .../motion_utils/trajectory/interpolation.hpp | 13 +- .../src/trajectory/interpolation.cpp | 35 +- .../src/trajectory/test_interpolation.cpp | 309 ++++++++++++++++++ .../geometry/geometry.hpp | 17 - 4 files changed, 344 insertions(+), 30 deletions(-) create mode 100644 common/motion_utils/test/src/trajectory/test_interpolation.cpp diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index b9b11a9b706de..b05a5334b7328 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -29,9 +29,20 @@ namespace motion_utils { +/** + * @brief An interpolation function that finds the closest interpolated point on the trajectory from + * the given pose + * @param trajectory input trajectory + * @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::TrajectoryPoint calcInterpolatedPoint( const autoware_auto_planning_msgs::msg::Trajectory & trajectory, - const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false); + 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 diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index fe983597f077c..cd603676ddd78 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -25,34 +25,39 @@ namespace motion_utils { TrajectoryPoint calcInterpolatedPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, - const bool use_zero_order_hold_for_twist) + const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold) { if (trajectory.points.empty()) { TrajectoryPoint interpolated_point{}; interpolated_point.pose = target_pose; return interpolated_point; } else if (trajectory.points.size() == 1) { - TrajectoryPoint interpolated_point = trajectory.points.front(); - interpolated_point.pose = target_pose; - return interpolated_point; + return trajectory.points.front(); } - const size_t segment_idx = - motion_utils::findNearestSegmentIndex(trajectory.points, target_pose.position); + const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory.points, target_pose, dist_threshold, yaw_threshold); // Calculate interpolation ratio - const double ratio = tier4_autoware_utils::calcRatio( - trajectory.points.at(segment_idx).pose, trajectory.points.at(segment_idx + 1).pose, - target_pose); - const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); - - // Interpolate const auto & curr_pt = trajectory.points.at(segment_idx); const auto & next_pt = trajectory.points.at(segment_idx + 1); + const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, 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 TrajectoryPoint interpolated_point{}; + + // pose interpolation interpolated_point.pose = tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation if (use_zero_order_hold_for_twist) { interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; @@ -65,12 +70,18 @@ TrajectoryPoint calcInterpolatedPoint( interpolated_point.acceleration_mps2 = interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); } + + // heading rate interpolation interpolated_point.heading_rate_rps = interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation interpolated_point.front_wheel_angle_rad = interpolation::lerp( curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); interpolated_point.rear_wheel_angle_rad = interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation const double interpolated_time = interpolation::lerp( rclcpp::Duration(curr_pt.time_from_start).seconds(), rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp new file mode 100644 index 0000000000000..f0e45179d05b6 --- /dev/null +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -0,0 +1,309 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "motion_utils/trajectory/interpolation.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" + +#include +#include +#include + +#include +#include + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; +using tier4_autoware_utils::transformPoint; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +TrajectoryPoint generateTestTrajectoryPoint( + 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, + const double acc = 0.0) +{ + TrajectoryPoint p; + p.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate; + p.acceleration_mps2 = acc; + return p; +} + +template +T generateTestTrajectory( + 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 acc = 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.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate_rps; + p.acceleration_mps2 = acc; + traj.points.push_back(p); + } + return traj; +} +} // namespace + +TEST(Interpolation, interpolate_path_for_trajectory) +{ + using motion_utils::calcInterpolatedPoint; + + { + autoware_auto_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + // Same points as the trajectory point + { + const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Random Point + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.25, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.125, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, 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(traj, target_pose, true); + + EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 2.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.1, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Initial Point + { + const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 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(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, 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(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 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(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + } + + // Empty Point + { + const Trajectory traj; + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 5.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // One point + { + const auto traj = generateTestTrajectory(1, 1.0, 1.0, 0.5, 0.1, 0.05); + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.1, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.05, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Duplicated Points + { + autoware_auto_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(4) = traj.points.at(3); + + const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } +} diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 54d44ef2eed41..2ba61fe321cbe 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -15,7 +15,6 @@ #ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#include #include #include #include @@ -719,22 +718,6 @@ geometry_msgs::msg::Pose calcInterpolatedPose( return output_pose; } - -/** - * @brief Calculate Ratio of the target point - * @param src source point - * @param dst destination point - * @param target target point - * @return ratio of the target - */ -template -double calcRatio(const Point1 & src, const Point2 & dst, const Point3 & target) -{ - const auto v1 = point2tfVector(src, dst); - const auto v2 = point2tfVector(src, target); - - return v1.dot(v2) / std::max(v1.length2(), 1e-6); -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_