From b4d8e7c75d21a5ec492ed1f9ac7f4e70ca7ab1d6 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 21 Oct 2024 16:32:17 +0900 Subject: [PATCH] test(bpp_common): add test for path utils (#9122) * add test file for path utils Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori * add tests for map irrelevant function Signed-off-by: Go Sakayori * add test for getUnshiftedEgoPose Signed-off-by: Go Sakayori * add docstring and remove unneccesary function Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- .../CMakeLists.txt | 3 +- .../utils/path_utils.hpp | 56 +++- .../src/utils/path_utils.cpp | 15 +- .../test/test_path_utils.cpp | 250 ++++++++++++++++++ .../src/scene.cpp | 8 +- 5 files changed, 314 insertions(+), 18 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index d3f76ed904244..9380bce784cc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -34,7 +34,8 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities - test/test_utils.cpp + test/test_utils.cpp + test/test_path_utils.cpp ) target_link_libraries(test_${PROJECT_NAME}_utilities diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp index dc371e3063822..3e4f180035b20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp @@ -58,34 +58,76 @@ PathWithLaneId resamplePathWithSpline( const PathWithLaneId & path, const double interval, const bool keep_input_points = false, const std::pair target_section = {0.0, std::numeric_limits::max()}); +/** + * @brief Gets index based on target index and arc length. + * @param [in] path Path. + * @param [in] target_idx Target index. + * @param [in] signed_arc Signed arc length. (Positive is forward) + * @return Index + */ size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc); +/** + * @brief Clips path according to the target index and length. + * @param [in] path Path. + * @param [in] target_idx Target index. + * @param [in] forward Forward distance from target index. + * @param [in] backward Backward distance from target index. + * @return Index + */ void clipPathLength( PathWithLaneId & path, const size_t target_idx, const double forward, const double backward); -void clipPathLength( - PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params); - PathWithLaneId convertWayPointsToPathWithLaneId( const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, const lanelet::ConstLanelets & lanelets); +/** + * @brief Get indices where the driving direction would reverse. + * @param [in] path Original path. + * @return Indices. + */ std::vector getReversingIndices(const PathWithLaneId & path); +/** + * @brief Divide path by given indices. + * @param [in] path Original path. + * @param [in] indices Where to split the path. + * @return Divided paths. + */ std::vector dividePath( const PathWithLaneId & path, const std::vector & indices); +/** + * @brief Corrects the velocity implemented in the trajectory by judging the vehicle driving + * direction. + * @param [in] divided_paths Multiple path with lane ID. + * @return + */ void correctDividedPathVelocity(std::vector & divided_paths); // only two points is supported -std::vector splineTwoPoints( +std::vector spline_two_points( const std::vector & base_s, const std::vector & base_x, const double begin_diff, const double end_diff, const std::vector & new_s); +/** + * @brief Interpolates pose between 2 pose. + * @param [in] start_pose Start pose. + * @param [in] end_pose End pose. + * @param [in] resample_interval Resampling interval. + * @return Array of pose + */ std::vector interpolatePose( const Pose & start_pose, const Pose & end_pose, const double resample_interval); +/** + * @brief Get ego pose which is not shifted. + * @param [in] ego_pose Ego pose. + * @param [in] prev_path Previous path with lane ID. + * @return Unshifted pose. + */ geometry_msgs::msg::Pose getUnshiftedEgoPose( const geometry_msgs::msg::Pose & ego_pose, const ShiftedPath & prev_path); @@ -94,6 +136,12 @@ PathWithLaneId calcCenterLinePath( const double longest_dist_to_shift_line, const std::optional & prev_module_path = std::nullopt); +/** + * @brief Combines 2 path which do not overlap. + * @param [in] path1 First path. + * @param [in] path2 Second path. + * @return Combined path. + */ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); BehaviorModuleOutput getReferencePath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 3b06e084690e3..d6f56f9c90168 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include #include @@ -102,6 +101,7 @@ PathWithLaneId resamplePathWithSpline( transformed_path, 0, transformed_path.size()); for (size_t i = 0; i < path.points.size(); ++i) { const double s = s_vec.at(i); + for (const auto & lane_id : path.points.at(i).lane_ids) { if (!keep_input_points && (unique_lane_ids.find(lane_id) != unique_lane_ids.end())) { continue; @@ -202,13 +202,6 @@ void clipPathLength( path.points = clipped_points; } -// TODO(murooka) This function should be replaced with autoware::motion_utils::cropPoints -void clipPathLength( - PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params) -{ - clipPathLength(path, target_idx, params.forward_path_length, params.backward_path_length); -} - PathWithLaneId convertWayPointsToPathWithLaneId( const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, const lanelet::ConstLanelets & lanelets) @@ -311,7 +304,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) } // only two points is supported -std::vector splineTwoPoints( +std::vector spline_two_points( const std::vector & base_s, const std::vector & base_x, const double begin_diff, const double end_diff, const std::vector & new_s) { @@ -350,10 +343,10 @@ std::vector interpolatePose( new_s.push_back(s); } - const std::vector interpolated_x = splineTwoPoints( + const std::vector interpolated_x = spline_two_points( base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), std::cos(tf2::getYaw(end_pose.orientation)), new_s); - const std::vector interpolated_y = splineTwoPoints( + const std::vector interpolated_y = spline_two_points( base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), std::sin(tf2::getYaw(end_pose.orientation)), new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp new file mode 100644 index 0000000000000..d96b30cccdfaa --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_path_utils.cpp @@ -0,0 +1,250 @@ +// 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/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" + +#include + +#include + +#include + +#include + +using tier4_planning_msgs::msg::PathWithLaneId; + +using autoware::test_utils::createPose; +using autoware::test_utils::generateTrajectory; + +TEST(BehaviorPathPlanningPathUtilTest, calcPathArcLengthArray) +{ + using autoware::behavior_path_planner::utils::calcPathArcLengthArray; + + auto path = generateTrajectory(10, 1.0); + auto arc_length_array = calcPathArcLengthArray(path); + + ASSERT_EQ(arc_length_array.size(), path.points.size()); + + size_t i = 0; + for (const auto & arc_length : arc_length_array) { + EXPECT_DOUBLE_EQ(arc_length, 1.0 * i); + i++; + } +} + +TEST(BehaviorPathPlanningPathUtilTest, resamplePathWithSpline) +{ + using autoware::behavior_path_planner::utils::resamplePathWithSpline; + + // Condition: path point less than 2 + auto path = generateTrajectory(1, 1.0); + EXPECT_EQ(resamplePathWithSpline(path, 1.0).points.size(), path.points.size()); + + // Condition: not enough point for spline interpolation + path = generateTrajectory(10, 0.01); + EXPECT_EQ(resamplePathWithSpline(path, 1.0).points.size(), path.points.size()); + + // Condition: spline interpolation + path = generateTrajectory(10, 0.1); + auto resampled_path = resamplePathWithSpline(path, 1.0); + EXPECT_EQ(resampled_path.points.size(), 5); +} + +TEST(BehaviorPathPlanningPathUtilTest, getIdxByArclength) +{ + using autoware::behavior_path_planner::utils::getIdxByArclength; + + tier4_planning_msgs::msg::PathWithLaneId path; + + // Condition: empty points + EXPECT_ANY_THROW(getIdxByArclength(path, 5, 1.0)); + + // Condition: negative arc with idx 0 + path = generateTrajectory(10, 1.0); + EXPECT_EQ(getIdxByArclength(path, 0, -1.0), 0); + + // Condition: negative arc + EXPECT_EQ(getIdxByArclength(path, 5, -2.0), 2); + + // Condition: positive arc + EXPECT_EQ(getIdxByArclength(path, 3, 4.0), 8); +} + +TEST(BehaviorPathPlanningPathUtilTest, clipPathLength) +{ + using autoware::behavior_path_planner::utils::clipPathLength; + + // Condition: path point less than 3 + auto path = generateTrajectory(2, 1.0); + clipPathLength(path, 5, 10.0, 1.0); + EXPECT_EQ(path.points.size(), 2); + + // Condition: path to be cropped + path = generateTrajectory(10, 1.0); + clipPathLength(path, 5, 10.0, 1.0); + EXPECT_EQ(path.points.size(), 7); + + size_t i = 0; + for (const auto & point : path.points) { + EXPECT_DOUBLE_EQ(point.point.pose.position.x, 3.0 + i); + i++; + } +} + +TEST(BehaviorPathPlanningPathUtilTest, getReversingIndices) +{ + using autoware::behavior_path_planner::utils::getReversingIndices; + + size_t target_index = 7; + auto path = generateTrajectory(10, 1.0, 1.0); + path.points.at(target_index).point.longitudinal_velocity_mps = -1.0; + auto reversing_indices = getReversingIndices(path); + EXPECT_EQ(reversing_indices.size(), 2); + + size_t i = 0; + for (const auto & index : reversing_indices) { + EXPECT_EQ(index, target_index - 1 + i); + i++; + } +} + +TEST(BehaviorPathPlanningPathUtilTest, dividePath) +{ + using autoware::behavior_path_planner::utils::dividePath; + auto path = generateTrajectory(10, 1.0); + + // Condition: empty indices + std::vector indices; + auto divided_path = dividePath(path, indices); + EXPECT_EQ(divided_path.size(), 1); + + // Condition: divide path + indices = {3, 5, 8}; + divided_path = dividePath(path, indices); + ASSERT_EQ(divided_path.size(), 4); + EXPECT_EQ(divided_path.at(0).points.size(), 4); + EXPECT_EQ(divided_path.at(1).points.size(), 3); + EXPECT_EQ(divided_path.at(2).points.size(), 4); + EXPECT_EQ(divided_path.at(3).points.size(), 2); +} + +TEST(BehaviorPathPlanningPathUtilTest, correctDividedPathVelocity) +{ + using autoware::behavior_path_planner::utils::correctDividedPathVelocity; + + double velocity = 1.0; + std::vector divided_paths; + // forward driving + divided_paths.push_back(generateTrajectory(10, 1.0, velocity)); + // reverse driving + divided_paths.push_back(generateTrajectory(10, -1.0, velocity, M_PI)); + correctDividedPathVelocity(divided_paths); + + for (const auto & point : divided_paths.at(0).points) { + if (point == divided_paths.at(0).points.back()) { + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + } else { + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity); + } + } + + for (const auto & point : divided_paths.at(1).points) { + if (point == divided_paths.at(1).points.back()) { + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0); + } else { + EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, -velocity); + } + } +} + +TEST(BehaviorPathPlanningPathUtilTest, interpolatePose) +{ + using autoware::behavior_path_planner::utils::interpolatePose; + + auto start_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + auto end_pose = createPose(10.0, 2.0, 0.0, 0.0, 0.0, M_PI_2); + double resample_interval = 1.0; + + auto resampled_pose = interpolatePose(start_pose, end_pose, resample_interval); + EXPECT_EQ(resampled_pose.size(), 10); +} + +TEST(BehaviorPathPlanningPathUtilTest, getUnshiftedEgoPose) +{ + using autoware::behavior_path_planner::utils::getUnshiftedEgoPose; + + auto ego_pose = createPose(2.0, 4.0, 0.0, 0.0, 0.0, 0.0); + autoware::behavior_path_planner::ShiftedPath shifted_path; + + // Condition: empty path + auto unshifted_ego_pose = getUnshiftedEgoPose(ego_pose, shifted_path); + EXPECT_DOUBLE_EQ(unshifted_ego_pose.position.x, ego_pose.position.x); + EXPECT_DOUBLE_EQ(unshifted_ego_pose.position.y, ego_pose.position.y); + + shifted_path.path = generateTrajectory(10, 1.0); + for (size_t i = 0; i < shifted_path.path.points.size(); i++) { + shifted_path.shift_length.push_back(static_cast(i) * 0.1); + } + + // Condition: path with increasing offset + unshifted_ego_pose = getUnshiftedEgoPose(ego_pose, shifted_path); + EXPECT_DOUBLE_EQ(unshifted_ego_pose.position.x, ego_pose.position.x); + EXPECT_DOUBLE_EQ(unshifted_ego_pose.position.y, -0.2); +} + +TEST(BehaviorPathPlanningPathUtilTest, combinePath) +{ + using autoware::behavior_path_planner::utils::combinePath; + + PathWithLaneId first_path; + auto second_path = generateTrajectory(10, 1.0); + + // Condition: first path empty + auto combined_path = combinePath(first_path, second_path); + EXPECT_EQ(combined_path.points.size(), 10); + size_t i = 0; + for (const auto & point : combined_path.points) { + EXPECT_DOUBLE_EQ(point.point.pose.position.x, static_cast(i)); + i++; + } + + // Condition: second path empty + first_path = generateTrajectory(4, 0.5); + second_path.points.clear(); + combined_path = combinePath(first_path, second_path); + EXPECT_EQ(combined_path.points.size(), 4); + i = 0; + for (const auto & point : combined_path.points) { + EXPECT_DOUBLE_EQ(point.point.pose.position.x, static_cast(i) * 0.5); + i++; + } + + // Condition: combine path + second_path = generateTrajectory(20, 0.25); + for (auto & point : second_path.points) { + point.point.pose.position.x += 1.5; + } + combined_path = combinePath(first_path, second_path); + EXPECT_EQ(combined_path.points.size(), 23); + i = 0; + for (const auto & point : combined_path.points) { + if (i < 4) + EXPECT_DOUBLE_EQ(point.point.pose.position.x, static_cast(i) * 0.5); + else + EXPECT_DOUBLE_EQ(point.point.pose.position.x, static_cast(i - 3) * 0.25 + 1.5); + + i++; + } +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index fb63180be7ac4..9b04847bd59b0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1125,7 +1125,9 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); const size_t ego_idx = planner_data_->findEgoIndex(output.path.points); - utils::clipPathLength(output.path, ego_idx, planner_data_->parameters); + utils::clipPathLength( + output.path, ego_idx, planner_data_->parameters.forward_path_length, + planner_data_->parameters.backward_path_length); // Drivable area generation. { @@ -1176,7 +1178,9 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const if (data.safe_shift_line.empty()) { const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); + utils::clipPathLength( + shifted_path.path, ego_idx, planner_data_->parameters.forward_path_length, + planner_data_->parameters.backward_path_length); output.path_candidate = shifted_path.path; return output;