From 9546c0efa9e6541587321eb5ec7a44bbeec19356 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Fri, 10 Jan 2025 19:47:29 +0900 Subject: [PATCH] test(autoware_behavior_path_start_planner_module): add test helper and implement unit tests for FreespacePullOut (#9832) * refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners Signed-off-by: Kyoichi Sugahara * refactor(autoware_behavior_path_start_planner_module): remove TimeKeeper parameter from pull-out planners Signed-off-by: Kyoichi Sugahara * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling Signed-off-by: kyoichi-sugahara * refactor(planner): add planner_data parameter to plan methods in pull out planners Signed-off-by: kyoichi-sugahara * refactor(autoware_behavior_path_start_planner_module): remove LaneDepartureChecker dependency from pull-out planners Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../goal_planner_module.hpp | 3 - .../pull_over_planner/bezier_pull_over.hpp | 4 +- .../pull_over_planner/freespace_pull_over.hpp | 4 +- .../pull_over_planner/geometric_pull_over.hpp | 3 +- .../pull_over_planner/shift_pull_over.hpp | 4 +- .../src/goal_planner_module.cpp | 19 +-- .../pull_over_planner/bezier_pull_over.cpp | 12 +- .../pull_over_planner/freespace_pull_over.cpp | 6 +- .../pull_over_planner/geometric_pull_over.cpp | 7 +- .../src/pull_over_planner/shift_pull_over.cpp | 7 +- .../CMakeLists.txt | 14 ++- .../freespace_pull_out.hpp | 6 +- .../geometric_pull_out.hpp | 3 +- .../pull_out_planner_base.hpp | 17 ++- .../shift_pull_out.hpp | 5 +- .../start_planner_module.hpp | 4 - .../src/freespace_pull_out.cpp | 15 +-- .../src/geometric_pull_out.cpp | 25 ++-- .../src/pull_out_planner_base.cpp | 7 +- .../src/shift_pull_out.cpp | 36 +++--- .../src/start_planner_module.cpp | 21 +--- .../include/start_planner_test_helper.hpp | 39 ++++++ .../test/src/start_planner_test_helper.cpp | 98 +++++++++++++++ .../test/test_freespace_pull_out.cpp | 113 ++++++++++++++++++ .../test/test_geometric_pull_out.cpp | 96 +++------------ .../test/test_shift_pull_out.cpp | 94 +++------------ 26 files changed, 385 insertions(+), 277 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index cabbeb3435087..41524b1b8193c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -23,8 +23,6 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include @@ -40,7 +38,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using nav_msgs::msg::OccupancyGrid; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp index fabbd667298bb..9b97f87c933a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -29,9 +29,7 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class BezierPullOver : public PullOverPlannerBase { public: - BezierPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::BEZIER; } std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 37c82ea904a55..06ff47224dd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -30,9 +30,7 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; class FreespacePullOver : public PullOverPlannerBase { public: - FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index d15c1e796bbe7..e12a759eb2137 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -32,8 +32,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward); + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward); PullOverPlannerType getPlannerType() const override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 2c6aec919bfbb..994d8283fe36c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -31,9 +31,7 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { public: - ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index d4f3953c557b4..0a6185127fcf0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -101,7 +101,7 @@ GoalPlannerModule::GoalPlannerModule( // freespace parking if (parameters_->enable_freespace_parking) { - auto freespace_planner = std::make_shared(node, *parameters, vehicle_info); + auto freespace_planner = std::make_shared(node, *parameters); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -280,22 +280,15 @@ LaneParkingPlanner::LaneParkingPlanner( is_lane_parking_cb_running_(is_lane_parking_cb_running), logger_(logger) { - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - lane_departure_checker::Param lane_departure_checker_params; - lane_departure_checker_params.footprint_extra_margin = - parameters.lane_departure_check_expansion_margin; - LaneDepartureChecker lane_departure_checker(lane_departure_checker_params, vehicle_info); - for (const std::string & planner_type : parameters.efficient_path_order) { if (planner_type == "SHIFT" && parameters.enable_shift_parking) { - pull_over_planners_.push_back( - std::make_shared(node, parameters, lane_departure_checker)); + pull_over_planners_.push_back(std::make_shared(node, parameters)); } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, parameters, lane_departure_checker, /*is_forward*/ true)); + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ true)); } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, parameters, lane_departure_checker, /*is_forward*/ false)); + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ false)); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index ad42ccf1582ab..ec3984a6d9443 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -31,15 +31,13 @@ #include namespace autoware::behavior_path_planner { -BezierPullOver::BezierPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) -: PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, - left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} +BezierPullOver::BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) +: PullOverPlannerBase(node, parameters), + lane_departure_checker_( + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_), + left_side_parking_(parameters.parking_policy == ParkingPolicy::LEFT_SIDE) { } - std::optional BezierPullOver::plan( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 473f76b5904d6..6605d258a8d5e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -32,9 +32,7 @@ namespace autoware::behavior_path_planner using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::RRTStar; -FreespacePullOver::FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +FreespacePullOver::FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}, @@ -45,7 +43,7 @@ FreespacePullOver::FreespacePullOver( } { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { planner_ = std::make_unique( parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 74b3fe149fd1d..bf2ce86b49bab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -26,11 +26,10 @@ namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward) + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, - parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{ + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 9a3e2d6b13db9..8350252369cc6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -30,11 +30,10 @@ namespace autoware::behavior_path_planner { -ShiftPullOver::ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) +ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{ + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt index e28339fb51a6c..5961b98bdda08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -19,12 +19,22 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + # Add test helper library + ament_auto_add_library(start_planner_test_helper SHARED + test/src/start_planner_test_helper.cpp + ) + target_include_directories(start_planner_test_helper PUBLIC + test/include + ) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES} ) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} + start_planner_test_helper + ) endif() - ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index 364d2d31a2577..e1a7ae455e374 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -40,7 +40,11 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } std::optional plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) override; + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) override; + + friend class TestFreespacePullOut; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 78eb72183cdf5..5d36c996cca3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -33,14 +33,13 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, std::shared_ptr time_keeper = std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; GeometricParallelParking planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index dfd972aff9be0..0316d6a14a4f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -42,33 +42,30 @@ class PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr time_keeper = std::make_shared()) - : parameters_(parameters), - vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()), - vehicle_footprint_(vehicle_info_.createFootprint()), + : parameters_{parameters}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, time_keeper_(time_keeper) { } virtual ~PullOutPlannerBase() = default; - void setPlannerData(const std::shared_ptr & planner_data) - { - planner_data_ = planner_data; - } - void setCollisionCheckMargin(const double collision_check_margin) { collision_check_margin_ = collision_check_margin; }; virtual PlannerType getPlannerType() const = 0; virtual std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) = 0; + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) = 0; protected: bool isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const; - std::shared_ptr planner_data_; StartPlannerParameters parameters_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 8da104940d327..1000437bdea56 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -35,18 +35,19 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, std::shared_ptr time_keeper = std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose); + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 47eecb54e2b2a..cf9227c2387f8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -27,7 +27,6 @@ #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp" -#include #include #include @@ -51,7 +50,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using autoware::lane_departure_checker::LaneDepartureChecker; using geometry_msgs::msg::PoseArray; using PriorityOrder = std::vector>>; @@ -310,8 +308,6 @@ ego pose. std::vector searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const; - std::shared_ptr lane_departure_checker_; - // turn signal TurnSignalInfo calcTurnSignalInfo(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index a089f6a8a83fc..6893c7d11d09d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -47,13 +47,14 @@ FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParame } std::optional FreespacePullOut::plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const double backward_path_length = planner_data_->parameters.backward_path_length; - const double forward_path_length = planner_data_->parameters.forward_path_length; + const auto & route_handler = planner_data->route_handler; + const double backward_path_length = planner_data->parameters.backward_path_length; + const double forward_path_length = planner_data->parameters.forward_path_length; - planner_->setMap(*planner_data_->costmap); + planner_->setMap(*planner_data->costmap); try { if (!planner_->makePlan(start_pose, end_pose)) { @@ -65,11 +66,11 @@ std::optional FreespacePullOut::plan( } const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + start_planner_utils::getPullOutLanes(planner_data, backward_path_length); const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); const PathWithLaneId path = diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index bbc6c05725434..69c5c41405932 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -36,35 +36,37 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, - parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_(lane_departure_checker) + parallel_parking_parameters_{parameters.parallel_parking_parameters} { + lane_departure_checker_ = + std::make_shared( + autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, + vehicle_info_, time_keeper_); planner_.setParameters(parallel_parking_parameters_); } std::optional GeometricPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { PullOutPath output; // combine road lane and pull out lane const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); + const auto pull_out_lanes = getPullOutLanes(planner_data, backward_path_length); // check if the ego is at left or right side of road lane center const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance; planner_.setTurningRadius( - planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); - planner_.setPlannerData(planner_data_); + planner_data->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); + planner_.setPlannerData(planner_data); const bool found_valid_path = planner_.planPullOut( start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); if (!found_valid_path) { @@ -122,7 +124,8 @@ std::optional GeometricPullOut::plan( output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; - if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + output, planner_data, parameters_.geometric_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp index 42c5bead33604..a475b3ad582cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp @@ -14,21 +14,24 @@ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include + namespace autoware::behavior_path_planner { bool PullOutPlannerBase::isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check for collisions - const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & dynamic_objects = planner_data->dynamic_object; if (!dynamic_objects) { return false; } const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_.max_back_distance); + planner_data, planner_data->parameters.backward_path_length + parameters_.max_back_distance); // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_.th_moving_object_velocity); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 4759559619870..8f4de1d3636de 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -41,31 +41,36 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker} +: PullOutPlannerBase{node, parameters, time_keeper} { + lane_departure_checker_ = + std::make_shared( + autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, + vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & common_parameters = planner_data->parameters; const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); + auto pull_out_paths = + calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose, common_parameters); if (pull_out_paths.empty()) { planner_debug_data.conditions_evaluation.emplace_back("no path found"); return std::nullopt; } - const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = planner_data->route_handler->getLaneletMapPtr(); std::vector fused_id_start_to_end{}; std::optional fused_polygon_start_to_end = std::nullopt; @@ -159,9 +164,10 @@ std::optional ShiftPullOut::plan( continue; } shift_path.points = cropped_path.points; - shift_path.header = planner_data_->route_handler->getRouteHeader(); + shift_path.header = planner_data->route_handler->getRouteHeader(); - if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + pull_out_path, planner_data, parameters_.shift_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); continue; } @@ -227,7 +233,8 @@ bool ShiftPullOut::refineShiftedPathToStartPose( std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose) + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -238,9 +245,8 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const auto & common_parameters = planner_data_->parameters; - const double forward_path_length = common_parameters.forward_path_length; - const double backward_path_length = common_parameters.backward_path_length; + const double forward_path_length = behavior_path_parameters.forward_path_length; + const double backward_path_length = behavior_path_parameters.backward_path_length; const double lateral_jerk = parameters_.lateral_jerk; const double minimum_lateral_acc = parameters_.minimum_lateral_acc; const double maximum_lateral_acc = parameters_.maximum_lateral_acc; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 83c1d55c7d022..0bac50a37fdbf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -68,20 +68,12 @@ StartPlannerModule::StartPlannerModule( vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = - parameters->lane_departure_check_expansion_margin; - lane_departure_checker_ = std::make_shared( - lane_departure_checker_params, vehicle_info_, time_keeper_); - // set enabled planner if (parameters_->enable_shift_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -983,17 +975,16 @@ bool StartPlannerModule::findPullOutPath( const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); - planner->setPlannerData(planner_data_); PlannerDebugData debug_data{ planner->getPlannerType(), {}, collision_check_margin, backwards_distance}; - const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose, debug_data); + const auto pull_out_path = + planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data); debug_data_vector.push_back(debug_data); // If no path is found, return false if (!pull_out_path) { return false; } - if (backward_is_unnecessary) { updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); return true; @@ -1592,9 +1583,9 @@ std::optional StartPlannerModule::planFreespacePath( for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - freespace_planner_->setPlannerData(planner_data); PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0}; - auto freespace_path = freespace_planner_->plan(current_pose, end_pose, debug_data); + auto freespace_path = + freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data); DEBUG_PRINT( "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(), debug_data.str().c_str()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp new file mode 100644 index 0000000000000..a1d294b279ff6 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp @@ -0,0 +1,39 @@ +// 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. +#pragma once + +#include +#include + +#include + +namespace autoware::behavior_path_planner::testing +{ + +class StartPlannerTestHelper +{ +public: + static rclcpp::NodeOptions make_node_options(); + + static void set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose); + static void set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id); + static void set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y); +}; + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp new file mode 100644 index 0000000000000..0f0720edc4a15 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.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. +// pull_out_test_utils.cpp +#include "start_planner_test_helper.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::testing +{ +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +rclcpp::NodeOptions StartPlannerTestHelper::make_node_options() +{ + // Load common configuration files + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); + const auto nearest_search_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); + const auto vehicle_info_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); + const auto behavior_path_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); + const auto start_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, + behavior_path_planner_param_path, drivable_area_expansion_param_path, + scene_module_manager_param_path, start_planner_param_path}); + + return node_options; +} + +void StartPlannerTestHelper::set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose) +{ + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data->self_odometry = odometry; +} + +void StartPlannerTestHelper::set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id) +{ + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + auto route_handler = std::make_shared(map_bin_msg); + + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + planner_data->route_handler = route_handler; +} + +void StartPlannerTestHelper::set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.header.frame_id = "map"; + costmap.info.width = static_cast(grid_length_x / grid_resolution); + costmap.info.height = static_cast(grid_length_y / grid_resolution); + costmap.info.resolution = grid_resolution; + + costmap.info.origin.position.x = start_pose.position.x - grid_length_x / 2; + costmap.info.origin.position.y = start_pose.position.y - grid_length_y / 2; + costmap.data = std::vector(costmap.info.width * costmap.info.height, 0); + + planner_data->costmap = std::make_shared(costmap); +} + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp new file mode 100644 index 0000000000000..79c58c37f0dcf --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp @@ -0,0 +1,113 @@ +// 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 "start_planner_test_helper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::FreespacePullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestFreespacePullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) + { + return freespace_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = + rclcpp::Node::make_shared("freespace_pull_out", StartPlannerTestHelper::make_node_options()); + + planner_data_ = std::make_shared(); + planner_data_->init_parameters(*node_); + + initialize_freespace_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + // Member variables + std::shared_ptr node_; + std::shared_ptr freespace_pull_out_; + std::shared_ptr planner_data_; + +private: + void initialize_freespace_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + freespace_pull_out_ = std::make_shared(*node_, parameters); + } +}; + +TEST_F(TestFreespacePullOut, GenerateValidFreespacePullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(299.796).y(303.529).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(-0.748629).w( + 0.662990)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(280.721).y(301.025).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.991718).w( + 0.128435)); + + StartPlannerTestHelper::set_odometry(planner_data_, start_pose); + StartPlannerTestHelper::set_route(planner_data_, 508, 720); + StartPlannerTestHelper::set_costmap(planner_data_, start_pose, 0.3, 70.0, 70.0); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, planner_data_, debug_data); + + // Assert that a valid Freespace pull out path is generated + ASSERT_TRUE(result.has_value()) << "Freespace pull out path generation failed."; + // EXPECT_EQ(result->partial_paths.size(), 2UL) + // << "Freespace pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "Freespace pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index 726c9ccc4c5d7..4548c058b871d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "start_planner_test_helper.hpp" + #include #include #include #include -#include #include #include #include @@ -30,10 +31,10 @@ using autoware::behavior_path_planner::GeometricPullOut; using autoware::behavior_path_planner::StartPlannerParameters; -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::test_utils::get_absolute_path_to_config; using autoware_planning_msgs::msg::LaneletRoute; using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; namespace autoware::behavior_path_planner @@ -43,102 +44,33 @@ class TestGeometricPullOut : public ::testing::Test { public: std::optional call_plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - return geometric_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + return geometric_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); } protected: void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("geometric_pull_out", make_node_options()); + node_ = + rclcpp::Node::make_shared("geometric_pull_out", StartPlannerTestHelper::make_node_options()); - initialize_lane_departure_checker(); initialize_geometric_pull_out_planner(); } - void TearDown() override { rclcpp::shutdown(); } - PlannerData make_planner_data( - const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) - { - PlannerData planner_data; - planner_data.init_parameters(*node_); - - // Load a sample lanelet map and create a route handler - const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( - "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); - auto route_handler = std::make_shared(map_bin_msg); - - // Set up current odometry at start pose - auto odometry = std::make_shared(); - odometry->pose.pose = start_pose; - odometry->header.frame_id = "map"; - planner_data.self_odometry = odometry; - - // Setup route - const auto route = makeBehaviorRouteFromLaneId( - route_start_lane_id, route_goal_lane_id, "autoware_test_utils", - "road_shoulder/lanelet2_map.osm"); - route_handler->setRoute(route); - - // Update planner data with the route handler - planner_data.route_handler = route_handler; - - return planner_data; - } - // Member variables std::shared_ptr node_; std::shared_ptr geometric_pull_out_; - std::shared_ptr lane_departure_checker_; private: - rclcpp::NodeOptions make_node_options() const - { - // Load common configuration files - auto node_options = rclcpp::NodeOptions{}; - - const auto common_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); - const auto nearest_search_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); - const auto vehicle_info_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); - const auto behavior_path_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); - const auto drivable_area_expansion_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); - const auto scene_module_manager_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); - const auto start_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); - - autoware::test_utils::updateNodeOptions( - node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, - behavior_path_planner_param_path, drivable_area_expansion_param_path, - scene_module_manager_param_path, start_planner_param_path}); - - return node_options; - } - - void initialize_lane_departure_checker() - { - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - const auto vehicle_info = - autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - lane_departure_checker_ = - std::make_shared(lane_departure_checker_params, vehicle_info); - } - void initialize_geometric_pull_out_planner() { auto parameters = StartPlannerParameters::init(*node_); - geometric_pull_out_ = - std::make_shared(*node_, parameters, lane_departure_checker_); + geometric_pull_out_ = std::make_shared(*node_, parameters); } }; @@ -157,15 +89,17 @@ TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) .orientation( geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - const auto planner_data = make_planner_data(start_pose, 4619, 4635); - geometric_pull_out_->setPlannerData(std::make_shared(planner_data)); + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = call_plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); - // Assert that a valid geometric geometric pull out path is generated + // Assert that a valid geometric pull out path is generated ASSERT_TRUE(result.has_value()) << "Geometric pull out path generation failed."; EXPECT_EQ(result->partial_paths.size(), 2UL) << "Generated geometric pull out path does not have the expected number of partial paths."; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp index 05a9201dad41d..d2c7c60e1a4ca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "start_planner_test_helper.hpp" + #include #include #include #include -#include #include #include #include @@ -25,15 +26,16 @@ #include #include +#include #include #include using autoware::behavior_path_planner::ShiftPullOut; using autoware::behavior_path_planner::StartPlannerParameters; -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::test_utils::get_absolute_path_to_config; using autoware_planning_msgs::msg::LaneletRoute; using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; namespace autoware::behavior_path_planner @@ -43,101 +45,34 @@ class TestShiftPullOut : public ::testing::Test { public: std::optional call_plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - return shift_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + return shift_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); } protected: void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("shift_pull_out", make_node_options()); + node_ = + rclcpp::Node::make_shared("shift_pull_out", StartPlannerTestHelper::make_node_options()); - initialize_lane_departure_checker(); initialize_shift_pull_out_planner(); } void TearDown() override { rclcpp::shutdown(); } - PlannerData make_planner_data( - const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) - { - PlannerData planner_data; - planner_data.init_parameters(*node_); - - // Load a sample lanelet map and create a route handler - const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( - "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); - auto route_handler = std::make_shared(map_bin_msg); - - // Set up current odometry at start pose - auto odometry = std::make_shared(); - odometry->pose.pose = start_pose; - odometry->header.frame_id = "map"; - planner_data.self_odometry = odometry; - - // Setup route - const auto route = makeBehaviorRouteFromLaneId( - route_start_lane_id, route_goal_lane_id, "autoware_test_utils", - "road_shoulder/lanelet2_map.osm"); - route_handler->setRoute(route); - - // Update planner data with the route handler - planner_data.route_handler = route_handler; - - return planner_data; - } - // Member variables std::shared_ptr node_; std::shared_ptr shift_pull_out_; - std::shared_ptr lane_departure_checker_; private: - rclcpp::NodeOptions make_node_options() const - { - // Load common configuration files - auto node_options = rclcpp::NodeOptions{}; - - const auto common_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); - const auto nearest_search_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); - const auto vehicle_info_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); - const auto behavior_path_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); - const auto drivable_area_expansion_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); - const auto scene_module_manager_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); - const auto start_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); - - autoware::test_utils::updateNodeOptions( - node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, - behavior_path_planner_param_path, drivable_area_expansion_param_path, - scene_module_manager_param_path, start_planner_param_path}); - - return node_options; - } - - void initialize_lane_departure_checker() - { - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - const auto vehicle_info = - autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - lane_departure_checker_ = - std::make_shared(lane_departure_checker_params, vehicle_info); - } - void initialize_shift_pull_out_planner() { auto parameters = StartPlannerParameters::init(*node_); - shift_pull_out_ = std::make_shared(*node_, parameters, lane_departure_checker_); + shift_pull_out_ = std::make_shared(*node_, parameters); } }; @@ -156,13 +91,14 @@ TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) .orientation( geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - const auto planner_data = make_planner_data(start_pose, 4619, 4635); - - shift_pull_out_->setPlannerData(std::make_shared(planner_data)); + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = call_plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); // Assert that a valid shift pull out path is generated ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed.";