diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp new file mode 100644 index 0000000000000..2221bd0147104 --- /dev/null +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -0,0 +1,117 @@ +// 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__RESAMPLE__RESAMPLE_UTILS_HPP_ +#define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ + +#include +#include +#include +#include + +#include + +namespace resample_utils +{ +constexpr double CLOSE_S_THRESHOLD = 1e-6; + +template +bool validate_size(const T & points) +{ + if (points.size() < 2) { + return false; + } + return true; +} + +template +bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) +{ + const double points_length = motion_utils::calcArcLength(points); + if (points_length < resampling_intervals.back()) { + return false; + } + + return true; +} + +template +bool validate_points_duplication(const T & points) +{ + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto & curr_pt = tier4_autoware_utils::getPoint(points.at(i)); + const auto & next_pt = tier4_autoware_utils::getPoint(points.at(i + 1)); + const double ds = tier4_autoware_utils::calcDistance2d(curr_pt, next_pt); + if (ds < CLOSE_S_THRESHOLD) { + return false; + } + } + + return true; +} + +template +bool validate_arguments(const T & input_points, const std::vector & resampling_intervals) +{ + // Check size of the arguments + if (!validate_size(input_points)) { + std::cerr << "The number of input points is less than 2" << std::endl; + return false; + } else if (!validate_size(resampling_intervals)) { + std::cerr << "The number of resampling intervals is less than 2" << std::endl; + return false; + } + + // Check resampling range + if (!validate_resampling_range(input_points, resampling_intervals)) { + std::cerr << "resampling interval is longer than input points" << std::endl; + return false; + } + + // Check duplication + if (!validate_points_duplication(input_points)) { + std::cerr << "input points has some duplicated points" << std::endl; + return false; + } + + return true; +} + +template +bool validate_arguments(const T & input_points, const double resampling_interval) +{ + // Check size of the arguments + if (!validate_size(input_points)) { + std::cerr << "The number of input points is less than 2" << std::endl; + return false; + } + + // check resampling interval + if (resampling_interval < motion_utils::overlap_threshold) { + std::cerr << "resampling interval is less than " << motion_utils::overlap_threshold + << std::endl; + return false; + } + + // Check duplication + if (!validate_points_duplication(input_points)) { + std::cerr << "input points has some duplicated points" << std::endl; + return false; + } + + return true; +} +} // namespace resample_utils + +#endif // MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 569739882f63a..fd9e8d46e9e39 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,9 +14,9 @@ #include "motion_utils/resample/resample.hpp" +#include "motion_utils/resample/resample_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -constexpr double CLOSE_S_THRESHOLD = 1e-6; namespace motion_utils { std::vector resamplePath( @@ -24,14 +24,8 @@ std::vector resamplePath( const std::vector & resampled_arclength, const bool use_lerp_for_xy, const bool use_lerp_for_z) { - // Check vector size and if out_arclength have the end point of the path - const double input_path_len = motion_utils::calcArcLength(points); - if ( - points.size() < 2 || resampled_arclength.size() < 2 || - input_path_len < resampled_arclength.back()) { - std::cerr - << "[motion_utils]: input points size, input points length or resampled arclength is wrong" - << std::endl; + // validate arguments + if (!resample_utils::validate_arguments(points, resampled_arclength)) { return points; } @@ -53,9 +47,6 @@ std::vector resamplePath( const auto & prev_pt = points.at(i - 1); const auto & curr_pt = points.at(i); const double ds = tier4_autoware_utils::calcDistance2d(prev_pt.position, curr_pt.position); - if (ds < CLOSE_S_THRESHOLD) { - continue; - } input_arclength.push_back(ds + input_arclength.back()); x.push_back(curr_pt.position.x); y.push_back(curr_pt.position.y); @@ -104,9 +95,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const std::vector & resampled_arclength, const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { - // Check vector size and if out_arclength have the end point of the path - if (input_path.points.size() < 2 || resampled_arclength.size() < 2) { - std::cerr << "[motion_utils]: input path size or input path length is wrong" << std::endl; + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { return input_path; } @@ -150,9 +140,6 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const auto & curr_pt = input_path.points.at(i).point; const double ds = tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); - if (ds < CLOSE_S_THRESHOLD) { - continue; - } input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -213,11 +200,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { - // Check vector size and if out_arclength have the end point of the trajectory - if (input_path.points.size() < 2 || resample_interval < 1e-3) { - std::cerr << "[motion_utils]: input trajectory size or resample " - "interval is invalid" - << std::endl; + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { return input_path; } @@ -240,7 +224,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } // Insert terminal point - if (input_path_len - resampling_arclength.back() < 1e-3) { + if (input_path_len - resampling_arclength.back() < motion_utils::overlap_threshold) { resampling_arclength.back() = input_path_len; } else { resampling_arclength.push_back(input_path_len); @@ -259,9 +243,9 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < 1e-3) { + if (dist_to_prev_point < motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < 1e-3) { + } else if (dist_to_following_point < motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); @@ -281,14 +265,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( const std::vector & resampled_arclength, const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { - // Check vector size and if out_arclength have the end point of the path - const double input_path_len = motion_utils::calcArcLength(input_path.points); - if ( - input_path.points.size() < 2 || resampled_arclength.size() < 2 || - input_path_len < resampled_arclength.back()) { - std::cerr - << "[motion_utils]: input path size, input path length or resampled arclength is wrong" - << std::endl; + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { return input_path; } @@ -314,9 +292,6 @@ autoware_auto_planning_msgs::msg::Path resamplePath( const auto & curr_pt = input_path.points.at(i); const double ds = tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); - if (ds < CLOSE_S_THRESHOLD) { - continue; - } input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -365,14 +340,8 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const std::vector & resampled_arclength, const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { - // Check vector size and if out_arclength have the end point of the trajectory - const double input_trajectory_len = motion_utils::calcArcLength(input_trajectory.points); - if ( - input_trajectory.points.size() < 2 || resampled_arclength.size() < 2 || - input_trajectory_len < resampled_arclength.back()) { - std::cerr << "[motion_utils]: input trajectory size, input trajectory length or resampled " - "arclength is wrong" - << std::endl; + // validate arguments + if (!resample_utils::validate_arguments(input_trajectory.points, resampled_arclength)) { return input_trajectory; } @@ -411,9 +380,6 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const auto & curr_pt = input_trajectory.points.at(i); const double ds = tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position); - if (ds < CLOSE_S_THRESHOLD) { - continue; - } input_arclength.push_back(ds + input_arclength.back()); input_pose.push_back(curr_pt.pose); v_lon.push_back(curr_pt.longitudinal_velocity_mps); @@ -474,16 +440,13 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { - const double input_trajectory_len = motion_utils::calcArcLength(input_trajectory.points); - // Check vector size and if out_arclength have the end point of the trajectory - if ( - input_trajectory.points.size() < 2 || input_trajectory_len < 1e-3 || resample_interval < 1e-3) { - std::cerr << "[motion_utils]: input trajectory size, input_trajectory length or resample " - "interval is invalid" - << std::endl; + // validate arguments + if (!resample_utils::validate_arguments(input_trajectory.points, resample_interval)) { return input_trajectory; } + const double input_trajectory_len = motion_utils::calcArcLength(input_trajectory.points); + std::vector resampling_arclength; for (double s = 0.0; s < input_trajectory_len; s += resample_interval) { resampling_arclength.push_back(s); @@ -494,7 +457,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( } // Insert terminal point - if (input_trajectory_len - resampling_arclength.back() < 1e-3) { + if (input_trajectory_len - resampling_arclength.back() < motion_utils::overlap_threshold) { resampling_arclength.back() = input_trajectory_len; } else { resampling_arclength.push_back(input_trajectory_len); @@ -513,9 +476,9 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); const double dist_to_following_point = std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); - if (dist_to_prev_point < 1e-3) { + if (dist_to_prev_point < motion_utils::overlap_threshold) { resampling_arclength.at(i - 1) = *distance_to_stop_point; - } else if (dist_to_following_point < 1e-3) { + } else if (dist_to_following_point < motion_utils::overlap_threshold) { resampling_arclength.at(i) = *distance_to_stop_point; } else { resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index 62cab7cc35ba8..798a4dc8356cb 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/constants.hpp" #include "motion_utils/resample/resample.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" @@ -364,103 +365,27 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) path.points.at(i) = generateTestPathPointWithLaneId( i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); } - path.points.at(10) = path.points.at(9); - path.points.at(9).point.is_final = true; - path.points.at(10).point.is_final = true; + path.points.at(6) = path.points.at(5); std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; const auto resampled_path = resamplePath(path, resampled_arclength); - { - const auto p = resampled_path.points.at(0); - EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 0); - } - } - - { - const auto p = resampled_path.points.at(1); - EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(2); - EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 1); - } - } - - { - const auto p = resampled_path.points.at(3); - EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 5); - } - } - - { - const auto p = resampled_path.points.at(4); - EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); - EXPECT_EQ(p.point.is_final, false); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 7); - } - } - - { - const auto p = resampled_path.points.at(5); - EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); - EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); - EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); - EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); - EXPECT_EQ(p.point.is_final, true); - for (size_t i = 0; i < p.lane_ids.size(); ++i) { - EXPECT_EQ(p.lane_ids.at(i), 9); - } - } - - for (size_t i = 0; i < resampled_path.points.size(); ++i) { - const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); - EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + EXPECT_EQ(path.points.size(), resampled_path.points.size()); + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + const auto resampled_p = resampled_path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, resampled_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, resampled_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, resampled_p.point.pose.position.z, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, resampled_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, resampled_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, resampled_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, resampled_p.point.is_final); + EXPECT_NEAR(p.point.pose.orientation.x, resampled_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, resampled_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, resampled_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, resampled_p.point.pose.orientation.w, epsilon); } } @@ -1225,10 +1150,11 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) path.points.at(0).point.longitudinal_velocity_mps = 5.0; path.points.back().point.is_final = true; - const auto resampled_path = resamplePath(path, 0.999); + const double ds = 1.0 - motion_utils::overlap_threshold; + const auto resampled_path = resamplePath(path, ds); for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { const auto p = resampled_path.points.at(i); - EXPECT_NEAR(p.point.pose.position.x, 0.999 * i, epsilon); + EXPECT_NEAR(p.point.pose.position.x, ds * i, epsilon); EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); @@ -1242,7 +1168,7 @@ TEST(resample_path_with_lane_id, resample_path_by_same_interval) epsilon); EXPECT_NEAR( p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.point.heading_rate_rps, 0.0999 * i, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ds / 10 * i, epsilon); EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); for (size_t j = 0; j < p.lane_ids.size(); ++j) { EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); @@ -2818,10 +2744,11 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) } traj.points.at(0).longitudinal_velocity_mps = 5.0; - const auto resampled_traj = resampleTrajectory(traj, 0.999); + const double ds = 1.0 - motion_utils::overlap_threshold; + const auto resampled_traj = resampleTrajectory(traj, ds); for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { const auto p = resampled_traj.points.at(i); - EXPECT_NEAR(p.pose.position.x, 0.999 * i, epsilon); + EXPECT_NEAR(p.pose.position.x, ds * i, epsilon); EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); @@ -2833,7 +2760,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) EXPECT_NEAR( p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon); EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(idx).lateral_velocity_mps, epsilon); - EXPECT_NEAR(p.heading_rate_rps, 0.0999 * i, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ds / 10.0 * i, epsilon); EXPECT_NEAR(p.acceleration_mps2, traj.points.at(idx).acceleration_mps2, epsilon); }