From 2a50825fafdf1c26d3cc52f7d79afb9a4db6f7f6 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Tue, 3 Dec 2024 14:57:37 +0900 Subject: [PATCH] test(lane_departure_checker): add tests for calcTrajectoryDeviation(), calcMaxSearchLengthForBoundaries() (#9029) * move calcTrajectoryDeviation() to separate files Signed-off-by: mitukou1109 * move calcMaxSearchLengthForBoundaries() to separate files Signed-off-by: mitukou1109 * add tests for calcTrajectoryDeviation() Signed-off-by: mitukou1109 * add tests for calcMaxSearchLengthForBoundaries() Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 Co-authored-by: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> --- .../lane_departure_checker.hpp | 6 - .../autoware/lane_departure_checker/utils.hpp | 23 ++++ .../lane_departure_checker.cpp | 27 +---- .../src/lane_departure_checker_node/utils.cpp | 22 ++++ ..._calc_max_search_length_for_boundaries.cpp | 105 ++++++++++++++++++ .../test/test_calc_trajectory_deviation.cpp | 98 ++++++++++++++++ 6 files changed, 250 insertions(+), 31 deletions(-) create mode 100644 control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp create mode 100644 control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 6c83f8780c93c..da4784bb2458a 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -157,10 +157,6 @@ class LaneDepartureChecker Param param_; std::shared_ptr vehicle_info_ptr_; - static PoseDeviation calcTrajectoryDeviation( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold); - static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); @@ -168,8 +164,6 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints) const; - double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const; - static SegmentRtree extractUncrossableBoundaries( const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, const double max_search_length, const std::vector & boundary_types_to_detect); diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp index 45a651339cc12..944c57f66b8f1 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/utils.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ #include +#include #include #include @@ -28,6 +29,7 @@ namespace autoware::lane_departure_checker::utils { using autoware::universe_utils::LinearRing2d; +using autoware::universe_utils::PoseDeviation; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; @@ -73,6 +75,27 @@ std::vector createVehicleFootprints( std::vector createVehicleFootprints( const PathWithLaneId & path, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double footprint_extra_margin); + +/** + * @brief calculate the deviation of the given pose from the nearest pose on the trajectory + * @param trajectory target trajectory + * @param pose vehicle pose + * @param dist_threshold distance threshold used for searching for first nearest index to given pose + * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose + * @return deviation of the given pose from the trajectory + */ +PoseDeviation calcTrajectoryDeviation( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold); + +/** + * @brief calculate the maximum search length for boundaries considering the vehicle dimensions + * @param trajectory target trajectory + * @param vehicle_info vehicle information + * @return maximum search length for boundaries + */ +double calcMaxSearchLengthForBoundaries( + const Trajectory & trajectory, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); } // namespace autoware::lane_departure_checker::utils #endif // AUTOWARE__LANE_DEPARTURE_CHECKER__UTILS_HPP_ diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f823988c77e4d..a1ef24e32543f 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -16,7 +16,6 @@ #include "autoware/lane_departure_checker/utils.hpp" -#include #include #include #include @@ -29,7 +28,6 @@ #include #include -using autoware::motion_utils::calcArcLength; using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::LineString2d; using autoware::universe_utils::MultiPoint2d; @@ -103,7 +101,7 @@ Output LaneDepartureChecker::update(const Input & input) autoware::universe_utils::StopWatch stop_watch; - output.trajectory_deviation = calcTrajectoryDeviation( + output.trajectory_deviation = utils::calcTrajectoryDeviation( *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, param_.ego_nearest_yaw_threshold); output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true); @@ -148,7 +146,7 @@ Output LaneDepartureChecker::update(const Input & input) output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); const double max_search_length_for_boundaries = - calcMaxSearchLengthForBoundaries(*input.predicted_trajectory); + utils::calcMaxSearchLengthForBoundaries(*input.predicted_trajectory, *vehicle_info_ptr_); const auto uncrossable_boundaries = extractUncrossableBoundaries( *input.lanelet_map, input.predicted_trajectory->points.front().pose.position, max_search_length_for_boundaries, input.boundary_types_to_detect); @@ -169,15 +167,6 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( return willLeaveLane(candidate_lanelets, vehicle_footprints); } -PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation( - const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) -{ - const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( - trajectory.points, pose, dist_threshold, yaw_threshold); - return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); -} - std::vector LaneDepartureChecker::createVehiclePassingAreas( const std::vector & vehicle_footprints) { @@ -388,18 +377,6 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -double LaneDepartureChecker::calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const -{ - const double max_ego_lon_length = std::max( - std::abs(vehicle_info_ptr_->max_longitudinal_offset_m), - std::abs(vehicle_info_ptr_->min_longitudinal_offset_m)); - const double max_ego_lat_length = std::max( - std::abs(vehicle_info_ptr_->max_lateral_offset_m), - std::abs(vehicle_info_ptr_->min_lateral_offset_m)); - const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length); - return calcArcLength(trajectory.points) + max_ego_search_length; -} - SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries( const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, const double max_search_length, const std::vector & boundary_types_to_detect) diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp index 324936c633158..19ea23ccd19da 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp @@ -14,6 +14,7 @@ #include "autoware/lane_departure_checker/utils.hpp" +#include #include #include @@ -161,4 +162,25 @@ std::vector createVehicleFootprints( return vehicle_footprints; } + +PoseDeviation calcTrajectoryDeviation( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + trajectory.points, pose, dist_threshold, yaw_threshold); + return autoware::universe_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose); +} + +double calcMaxSearchLengthForBoundaries( + const Trajectory & trajectory, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + const double max_ego_lon_length = std::max( + std::abs(vehicle_info.max_longitudinal_offset_m), + std::abs(vehicle_info.min_longitudinal_offset_m)); + const double max_ego_lat_length = std::max( + std::abs(vehicle_info.max_lateral_offset_m), std::abs(vehicle_info.min_lateral_offset_m)); + const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length); + return autoware::motion_utils::calcArcLength(trajectory.points) + max_ego_search_length; +} } // namespace autoware::lane_departure_checker::utils diff --git a/control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp b/control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp new file mode 100644 index 0000000000000..1632f41f35153 --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_calc_max_search_length_for_boundaries.cpp @@ -0,0 +1,105 @@ +// Copyright 2024 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 "autoware/lane_departure_checker/utils.hpp" + +#include + +#include + +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace +{ +Trajectory create_trajectory(const std::vector & points) +{ + Trajectory trajectory; + for (const auto & point : points) { + TrajectoryPoint p; + p.pose.position.x = point.x(); + p.pose.position.y = point.y(); + trajectory.points.push_back(p); + } + return trajectory; +} + +// reference: +// https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml +constexpr double wheel_radius_m = 0.383; +constexpr double wheel_width_m = 0.235; +constexpr double wheel_base_m = 2.79; +constexpr double wheel_tread_m = 1.64; +constexpr double front_overhang_m = 1.0; +constexpr double rear_overhang_m = 1.1; +constexpr double left_overhang_m = 0.128; +constexpr double right_overhang_m = 0.128; +constexpr double vehicle_height_m = 2.5; +constexpr double max_steer_angle_rad = 0.70; +} // namespace + +struct CalcMaxSearchLengthForBoundariesParam +{ + std::string description; + std::vector trajectory_points; + double expected_max_search_length; +}; + +std::ostream & operator<<(std::ostream & os, const CalcMaxSearchLengthForBoundariesParam & p) +{ + return os << p.description; +} + +class CalcMaxSearchLengthForBoundariesTest +: public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_info = autoware::vehicle_info_utils::createVehicleInfo( + wheel_radius_m, wheel_width_m, wheel_base_m, wheel_tread_m, front_overhang_m, rear_overhang_m, + left_overhang_m, right_overhang_m, vehicle_height_m, max_steer_angle_rad); + } + + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +TEST_P(CalcMaxSearchLengthForBoundariesTest, test_calc_max_search_length_for_boundaries) +{ + const auto p = GetParam(); + const auto trajectory = create_trajectory(p.trajectory_points); + + const auto max_search_length = + autoware::lane_departure_checker::utils::calcMaxSearchLengthForBoundaries( + trajectory, vehicle_info); + + EXPECT_DOUBLE_EQ(max_search_length, p.expected_max_search_length); +} + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CalcMaxSearchLengthForBoundariesTest, + ::testing::Values( + CalcMaxSearchLengthForBoundariesParam{ + "EmptyTrajectory", + {}, + std::hypot(front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m)}, + CalcMaxSearchLengthForBoundariesParam{ + "SinglePointTrajectory", + {{0.0, 0.0}}, + std::hypot(front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m)}, + CalcMaxSearchLengthForBoundariesParam{ + "MultiPointTrajectory", + {{0.0, 0.0}, {1.0, 0.0}}, + 1.0 + std::hypot(front_overhang_m + wheel_base_m, wheel_tread_m / 2.0 + left_overhang_m)}), + ::testing::PrintToStringParamName()); diff --git a/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp b/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp new file mode 100644 index 0000000000000..a0f141e9639a9 --- /dev/null +++ b/control/autoware_lane_departure_checker/test/test_calc_trajectory_deviation.cpp @@ -0,0 +1,98 @@ +// Copyright 2024 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 "autoware/lane_departure_checker/utils.hpp" + +#include + +#include + +using autoware::universe_utils::PoseDeviation; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace +{ +Trajectory create_trajectory(const std::vector & points) +{ + Trajectory trajectory; + for (const auto & point : points) { + TrajectoryPoint p; + p.pose.position.x = point.x(); + p.pose.position.y = point.y(); + trajectory.points.push_back(p); + } + return trajectory; +} + +geometry_msgs::msg::Pose create_pose(const Eigen::Vector3d & x_y_yaw) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = x_y_yaw[0]; + pose.position.y = x_y_yaw[1]; + pose.orientation.z = std::sin(x_y_yaw[2] / 2); + pose.orientation.w = std::cos(x_y_yaw[2] / 2); + return pose; +} + +constexpr double ego_nearest_dist_threshold = 3.0; +constexpr double ego_nearest_yaw_threshold = 1.046; +} // namespace + +struct CalcTrajectoryDeviationTestParam +{ + std::string description; + std::vector trajectory_points; + Eigen::Vector3d x_y_yaw; + bool exception_expected; + PoseDeviation expected_deviation; +}; + +std::ostream & operator<<(std::ostream & os, const CalcTrajectoryDeviationTestParam & p) +{ + return os << p.description; +} + +class CalcTrajectoryDeviationTest +: public ::testing::TestWithParam +{ +}; + +TEST_P(CalcTrajectoryDeviationTest, test_calc_trajectory_deviation) +{ + const auto p = GetParam(); + const auto trajectory = create_trajectory(p.trajectory_points); + const auto pose = create_pose(p.x_y_yaw); + if (p.exception_expected) { + EXPECT_ANY_THROW({ + autoware::lane_departure_checker::utils::calcTrajectoryDeviation( + trajectory, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + }); + } else { + const auto deviation = autoware::lane_departure_checker::utils::calcTrajectoryDeviation( + trajectory, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + + EXPECT_DOUBLE_EQ(deviation.lateral, p.expected_deviation.lateral); + EXPECT_DOUBLE_EQ(deviation.longitudinal, p.expected_deviation.longitudinal); + EXPECT_DOUBLE_EQ(deviation.yaw, p.expected_deviation.yaw); + } +} + +INSTANTIATE_TEST_SUITE_P( + LaneDepartureCheckerTest, CalcTrajectoryDeviationTest, + ::testing::Values( + CalcTrajectoryDeviationTestParam{"EmptyTrajectory", {}, {}, true, {}}, + CalcTrajectoryDeviationTestParam{ + "SinglePointTrajectory", {{0.0, 0.0}}, {0.0, 0.0, 0.0}, false, {0.0, 0.0, 0.0}}), + ::testing::PrintToStringParamName());