From e66973bd582845f7fbbfbaf98cb1d3384afe6c3d Mon Sep 17 00:00:00 2001 From: yutaka Date: Tue, 30 Aug 2022 13:14:25 +0900 Subject: [PATCH 1/3] feat(tier4_autoware_utils): add point to tfvector function Signed-off-by: yutaka --- .../geometry/geometry.hpp | 12 ++++ .../test/src/geometry/test_geometry.cpp | 67 +++++++++++++++++++ 2 files changed, 79 insertions(+) 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 115b544726bdf..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 @@ -430,6 +430,18 @@ inline geometry_msgs::msg::TransformStamped pose2transform( return transform; } +template +tf2::Vector3 point2tfVector(const Point1 & src, const Point2 & dst) +{ + const auto src_p = getPoint(src); + const auto dst_p = getPoint(dst); + + double dx = dst_p.x - src_p.x; + double dy = dst_p.y - src_p.y; + double dz = dst_p.z - src_p.z; + return tf2::Vector3(dx, dy, dz); +} + inline Point3d transformPoint( const Point3d & point, const geometry_msgs::msg::Transform & transform) { diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index 6b8d886b5196a..70ebb8ac72dca 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -752,6 +752,73 @@ TEST(geometry, pose2transform) } } +TEST(geometry, point2tfVector) +{ + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::point2tfVector; + + // Point + { + geometry_msgs::msg::Point src; + src.x = 1.0; + src.y = 2.0; + src.z = 3.0; + + geometry_msgs::msg::Point dst; + dst.x = 10.0; + dst.y = 5.0; + dst.z = -5.0; + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } + + // Pose + { + geometry_msgs::msg::Pose src; + src.position.x = 1.0; + src.position.y = 2.0; + src.position.z = 3.0; + src.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + geometry_msgs::msg::Pose dst; + dst.position.x = 10.0; + dst.position.y = 5.0; + dst.position.z = -5.0; + dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } + + // Point and Pose + { + geometry_msgs::msg::Point src; + src.x = 1.0; + src.y = 2.0; + src.z = 3.0; + + geometry_msgs::msg::Pose dst; + dst.position.x = 10.0; + dst.position.y = 5.0; + dst.position.z = -5.0; + dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } +} + TEST(geometry, transformPoint) { using tier4_autoware_utils::createQuaternionFromRPY; From 7d6b6178465eb862f308317edef4a57c3c592d11 Mon Sep 17 00:00:00 2001 From: yutaka Date: Tue, 30 Aug 2022 15:04:26 +0900 Subject: [PATCH 2/3] add interpolation function Signed-off-by: yutaka --- common/motion_utils/CMakeLists.txt | 1 + .../motion_utils/trajectory/interpolation.hpp | 38 +++++++++ .../src/trajectory/interpolation.cpp | 81 +++++++++++++++++++ .../geometry/geometry.hpp | 17 ++++ 4 files changed, 137 insertions(+) create mode 100644 common/motion_utils/include/motion_utils/trajectory/interpolation.hpp create mode 100644 common/motion_utils/src/trajectory/interpolation.cpp 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..b9b11a9b706de --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -0,0 +1,38 @@ +// 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 +{ +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); + +} // 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..fe983597f077c --- /dev/null +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -0,0 +1,81 @@ +// 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) +{ + 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; + } + + const size_t segment_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, target_pose.position); + + // 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); + + TrajectoryPoint interpolated_point{}; + interpolated_point.pose = + tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + 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); + } + interpolated_point.heading_rate_rps = + interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + 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); + 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/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 2ba61fe321cbe..54d44ef2eed41 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,6 +15,7 @@ #ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ +#include #include #include #include @@ -718,6 +719,22 @@ 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_ From a572a2ba6b5e473e342cd2ef52840de6d845679b Mon Sep 17 00:00:00 2001 From: yutaka Date: Wed, 31 Aug 2022 15:28:27 +0900 Subject: [PATCH 3/3] 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_