From 72dc6b3872687804327a4d2d877919a498144626 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 26 Jul 2022 01:52:42 +0900 Subject: [PATCH] feat(perception_utils): add prediction resampling function (#1425) * feat(perception_utils): add predicted_path utils * add no interpolation case Signed-off-by: yutaka * fix tests Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka --- .../perception_utils/predicted_path_utils.hpp | 172 +++++++++++ common/perception_utils/package.xml | 1 + .../test/src/test_predicted_path_utils.cpp | 272 ++++++++++++++++++ 3 files changed, 445 insertions(+) create mode 100644 common/perception_utils/include/perception_utils/predicted_path_utils.hpp create mode 100644 common/perception_utils/test/src/test_predicted_path_utils.cpp diff --git a/common/perception_utils/include/perception_utils/predicted_path_utils.hpp b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp new file mode 100644 index 0000000000000..b129be06288a1 --- /dev/null +++ b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp @@ -0,0 +1,172 @@ +// 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 PERCEPTION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#define PERCEPTION_UTILS__PREDICTED_PATH_UTILS_HPP_ + +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "perception_utils/geometry.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include + +#include + +#include +#include + +namespace perception_utils +{ +/** + * @brief Calculate Interpolated Pose from predicted path by time + * @param path Input predicted path + * @param relative_time time at interpolated point. This should be within [0.0, + * time_step*(num_of_path_points)] + * @return interpolated pose + */ +inline boost::optional calcInterpolatedPose( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) +{ + // Check if relative time is in the valid range + if (path.path.empty() || relative_time < 0.0) { + return boost::none; + } + + constexpr double epsilon = 1e-6; + const double & time_step = rclcpp::Duration(path.time_step).seconds(); + for (size_t path_idx = 1; path_idx < path.path.size(); ++path_idx) { + const auto & pt = path.path.at(path_idx); + const auto & prev_pt = path.path.at(path_idx - 1); + if (relative_time - epsilon < time_step * path_idx) { + const double offset = relative_time - time_step * (path_idx - 1); + const double ratio = std::clamp(offset / time_step, 0.0, 1.0); + return tier4_autoware_utils::calcInterpolatedPose(prev_pt, pt, ratio); + } + } + + return boost::none; +} + +/** + * @brief Resampling predicted path by time step vector. Note this function does not substitute + * original time step + * @param path Input predicted path + * @param resampled_time resampled time at each resampling point. Each time should be within [0.0, + * time_step*(num_of_path_points)] + * @return resampled path + */ +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const std::vector & resampled_time, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false) +{ + if (path.path.empty() || resampled_time.empty()) { + throw std::invalid_argument("input path or resampled_time is empty"); + } + + const double & time_step = rclcpp::Duration(path.time_step).seconds(); + std::vector input_time(path.path.size()); + std::vector x(path.path.size()); + std::vector y(path.path.size()); + std::vector z(path.path.size()); + for (size_t i = 0; i < path.path.size(); ++i) { + input_time.at(i) = time_step * i; + x.at(i) = path.path.at(i).position.x; + y.at(i) = path.path.at(i).position.y; + z.at(i) = path.path.at(i).position.z; + } + + const auto lerp = [&](const auto & input) { + return interpolation::lerp(input_time, input, resampled_time); + }; + const auto slerp = [&](const auto & input) { + return interpolation::slerp(input_time, input, resampled_time); + }; + + const auto interpolated_x = use_spline_for_xy ? slerp(x) : lerp(x); + const auto interpolated_y = use_spline_for_xy ? slerp(y) : lerp(y); + const auto interpolated_z = use_spline_for_z ? slerp(z) : lerp(z); + + autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + resampled_path.confidence = path.confidence; + resampled_path.path.resize(resampled_time.size()); + + // Set Position + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + const auto p = tier4_autoware_utils::createPoint( + interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); + resampled_path.path.at(i).position = p; + } + + // Set Quaternion + for (size_t i = 0; i < resampled_path.path.size() - 1; ++i) { + const auto & src_point = resampled_path.path.at(i).position; + const auto & dst_point = resampled_path.path.at(i + 1).position; + const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); + resampled_path.path.at(i).orientation = + tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); + } + resampled_path.path.back().orientation = + resampled_path.path.at(resampled_path.path.size() - 2).orientation; + + return resampled_path; +} + +/** + * @brief Resampling predicted path by sampling time interval. Note that this function samples + * terminal point as well as points by sampling time interval + * @param path Input predicted path + * @param sampling_time_interval sampling time interval for each point + * @param sampling_horizon sampling time horizon + * @return resampled path + */ +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const double sampling_time_interval, const double sampling_horizon, + const bool use_spline_for_xy = true, const bool use_spline_for_z = false) +{ + if (path.path.empty()) { + throw std::invalid_argument("Predicted Path is empty"); + } + + if (sampling_time_interval <= 0.0 || sampling_horizon <= 0.0) { + throw std::invalid_argument("sampling time interval or sampling time horizon is negative"); + } + + // Calculate Horizon + const double predicted_horizon = + rclcpp::Duration(path.time_step).seconds() * static_cast(path.path.size() - 1); + const double horizon = std::min(predicted_horizon, sampling_horizon); + + // Get sampling time vector + constexpr double epsilon = 1e-6; + std::vector sampling_time_vector; + for (double t = 0.0; t < horizon + epsilon; t += sampling_time_interval) { + sampling_time_vector.push_back(t); + } + + // Resample and substitute time interval + auto resampled_path = + resamplePredictedPath(path, sampling_time_vector, use_spline_for_xy, use_spline_for_z); + resampled_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval); + return resampled_path; +} +} // namespace perception_utils + +#endif // PERCEPTION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 2abadbcd2fa84..c338601d9ec46 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -13,6 +13,7 @@ autoware_auto_perception_msgs geometry_msgs + interpolation libboost-dev rclcpp tier4_autoware_utils diff --git a/common/perception_utils/test/src/test_predicted_path_utils.cpp b/common/perception_utils/test/src/test_predicted_path_utils.cpp new file mode 100644 index 0000000000000..ee467448f3d83 --- /dev/null +++ b/common/perception_utils/test/src/test_predicted_path_utils.cpp @@ -0,0 +1,272 @@ +// Copyright 2020 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 "perception_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" + +#include + +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Point3d; + +constexpr double epsilon = 1e-06; + +namespace +{ +using autoware_auto_perception_msgs::msg::PredictedPath; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; +using tier4_autoware_utils::transformPoint; + +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; +} + +PredictedPath createTestPredictedPath( + const size_t num_points, const double point_time_interval, const double vel, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + PredictedPath path; + path.confidence = 1.0; + path.time_step = rclcpp::Duration::from_seconds(point_time_interval); + + const double point_interval = vel * point_time_interval; + 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); + + const auto p = createPose(x, y, 0.0, 0.0, 0.0, theta); + path.path.push_back(p); + } + return path; +} +} // namespace + +TEST(predicted_path_utils, testCalcInterpolatedPose) +{ + using perception_utils::calcInterpolatedPose; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::createQuaternionFromYaw; + using tier4_autoware_utils::deg2rad; + + const auto path = createTestPredictedPath(100, 0.1, 1.0); + + // Normal Case (same point as the original point) + { + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + for (double t = 0.0; t < 9.0 + 1e-6; t += 1.0) { + const auto p = calcInterpolatedPose(path, t); + + EXPECT_NE(p, boost::none); + EXPECT_NEAR(p->position.x, t * 1.0, epsilon); + EXPECT_NEAR(p->position.y, 0.0, epsilon); + EXPECT_NEAR(p->position.z, 0.0, epsilon); + EXPECT_NEAR(p->orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p->orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p->orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p->orientation.w, ans_quat.w, epsilon); + } + } + + // Normal Case (random case) + { + const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + for (double t = 0.0; t < 9.0; t += 0.3) { + const auto p = calcInterpolatedPose(path, t); + + EXPECT_NE(p, boost::none); + EXPECT_NEAR(p->position.x, t * 1.0, epsilon); + EXPECT_NEAR(p->position.y, 0.0, epsilon); + EXPECT_NEAR(p->position.z, 0.0, epsilon); + EXPECT_NEAR(p->orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p->orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p->orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p->orientation.w, ans_quat.w, epsilon); + } + } + + // No Interpolation + { + // Negative time + { + const auto p = calcInterpolatedPose(path, -1.0); + EXPECT_EQ(p, boost::none); + } + + // Over the time horizon + { + const auto p = calcInterpolatedPose(path, 11.0); + EXPECT_EQ(p, boost::none); + } + + // Empty Path + { + PredictedPath empty_path; + const auto p = calcInterpolatedPose(empty_path, 5.0); + EXPECT_EQ(p, boost::none); + } + } +} + +TEST(predicted_path_utils, resamplePredictedPath_by_vector) +{ + using perception_utils::resamplePredictedPath; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::createQuaternionFromYaw; + using tier4_autoware_utils::deg2rad; + + const auto path = createTestPredictedPath(10, 1.0, 1.0); + + // Resample Same Points + { + const std::vector resampling_vec = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; + const auto resampled_path = resamplePredictedPath(path, resampling_vec); + + EXPECT_EQ(resampled_path.path.size(), path.path.size()); + EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); + + for (size_t i = 0; i < path.path.size(); ++i) { + EXPECT_NEAR(path.path.at(i).position.x, resampled_path.path.at(i).position.x, epsilon); + EXPECT_NEAR(path.path.at(i).position.y, resampled_path.path.at(i).position.y, epsilon); + EXPECT_NEAR(path.path.at(i).position.z, resampled_path.path.at(i).position.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.x, resampled_path.path.at(i).orientation.x, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.y, resampled_path.path.at(i).orientation.y, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.z, resampled_path.path.at(i).orientation.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.w, resampled_path.path.at(i).orientation.w, epsilon); + } + } + + // Resample random case + { + const std::vector resampling_vec = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 1.3, 2.8, + 3.7, 4.2, 5.1, 6.9, 7.3, 8.5, 9.0}; + const auto resampled_path = resamplePredictedPath(path, resampling_vec); + + EXPECT_EQ(resampled_path.path.size(), resampling_vec.size()); + EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); + + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i) * 1.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Some points are out of range + { + const std::vector resampling_vec = {-1.0, 0.0, 5.0, 9.0, 9.1}; + EXPECT_THROW(resamplePredictedPath(path, resampling_vec), std::invalid_argument); + } +} + +TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) +{ + using perception_utils::resamplePredictedPath; + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::createQuaternionFromYaw; + using tier4_autoware_utils::deg2rad; + + const auto path = createTestPredictedPath(10, 1.0, 1.0); + + // Sample same points + { + const auto resampled_path = resamplePredictedPath(path, 1.0, 9.0); + + EXPECT_EQ(resampled_path.path.size(), path.path.size()); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 1.0, epsilon); + for (size_t i = 0; i < path.path.size(); ++i) { + EXPECT_NEAR(path.path.at(i).position.x, resampled_path.path.at(i).position.x, epsilon); + EXPECT_NEAR(path.path.at(i).position.y, resampled_path.path.at(i).position.y, epsilon); + EXPECT_NEAR(path.path.at(i).position.z, resampled_path.path.at(i).position.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.x, resampled_path.path.at(i).orientation.x, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.y, resampled_path.path.at(i).orientation.y, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.z, resampled_path.path.at(i).orientation.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.w, resampled_path.path.at(i).orientation.w, epsilon); + } + } + + // Fine sampling + { + const auto resampled_path = resamplePredictedPath(path, 0.1, 9.0); + + EXPECT_EQ(resampled_path.path.size(), static_cast(91)); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 0.1, epsilon); + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, 0.1 * i, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Coarse Sampling + { + const auto resampled_path = resamplePredictedPath(path, 2.0, 9.0); + + EXPECT_EQ(resampled_path.path.size(), static_cast(5)); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 2.0, epsilon); + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, 2.0 * i, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Shorter horizon + { + const auto resampled_path = resamplePredictedPath(path, 1.5, 7.0); + + EXPECT_EQ(resampled_path.path.size(), static_cast(5)); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 1.5, epsilon); + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, 1.5 * i, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // No Sampling + { + // Negative resampling time or resampling time horizon + EXPECT_THROW(resamplePredictedPath(path, 0.0, 9.0), std::invalid_argument); + EXPECT_THROW(resamplePredictedPath(path, -1.0, 9.0), std::invalid_argument); + EXPECT_THROW(resamplePredictedPath(path, 1.0, 0.0), std::invalid_argument); + EXPECT_THROW(resamplePredictedPath(path, 1.0, -9.0), std::invalid_argument); + + PredictedPath empty_path; + EXPECT_THROW(resamplePredictedPath(empty_path, 1.0, 10.0), std::invalid_argument); + } +}