diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 59e43d2ec0347..941855eb62528 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(motion_utils SHARED src/motion_utils.cpp src/marker/marker_helper.cpp src/resample/resample.cpp + src/trajectory/interpolation.cpp src/vehicle/vehicle_state_checker.cpp ) diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp new file mode 100644 index 0000000000000..b05a5334b7328 --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -0,0 +1,49 @@ +// 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. + +#ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/constants.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include + +#include +#include +#include +#include + +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 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 new file mode 100644 index 0000000000000..cd603676ddd78 --- /dev/null +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -0,0 +1,92 @@ +// 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 "interpolation/linear_interpolation.hpp" +#include "interpolation/zero_order_hold.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +namespace motion_utils +{ +TrajectoryPoint calcInterpolatedPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, + 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) { + return trajectory.points.front(); + } + + const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory.points, target_pose, dist_threshold, yaw_threshold); + + // Calculate interpolation ratio + 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; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + 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); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + 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 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); + } +}