test(autoware_behavior_path_start_planner_module): add test helper an…

…d 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 <kyoichi.sugahara@tier4.jp> * refactor(autoware_behavior_path_start_planner_module): remove TimeKeeper parameter from pull-out planners Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor(planner): add planner_data parameter to plan methods in pull out planners Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor(autoware_behavior_path_start_planner_module): remove LaneDepartureChecker dependency from pull-out planners Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
autowarefoundation · Jan 10, 2025 · 9546c0e · 9546c0e
1 parent 7d63242
commit 9546c0e
Show file tree

Hide file tree

Showing 26 changed files with 385 additions and 277 deletions.
diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...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 <autoware/lane_departure_checker/lane_departure_checker.hpp>
-
 #include <autoware_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -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/...include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/...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<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,

diff --git a/...lude/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/...lude/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/...lude/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/...lude/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/.../include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/.../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<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,

diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_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<FreespacePullOver>(node, *parameters, vehicle_info);
+    auto freespace_planner = std::make_shared<FreespacePullOver>(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<ShiftPullOver>(node, parameters, lane_departure_checker));
+      pull_over_planners_.push_back(std::make_shared<ShiftPullOver>(node, parameters));
     } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) {
-      pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
-        node, parameters, lane_departure_checker, /*is_forward*/ true));
+      pull_over_planners_.push_back(
+        std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ true));
     } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) {
-      pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
-        node, parameters, lane_departure_checker, /*is_forward*/ false));
+      pull_over_planners_.push_back(
+        std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ false));
     }
   }
 

diff --git a/...ner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/...ner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp
@@ -31,15 +31,13 @@
 #include <vector>
 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<PullOverPath> BezierPullOver::plan(
   [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id,
   [[maybe_unused]] const std::shared_ptr<const PlannerData> planner_data,

diff --git a/.../autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/.../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<AstarSearch>(
       parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters,

diff --git a/.../autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/.../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/...nner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/...nner/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
@@ -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/...planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/...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<PullOutPath> 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<const PlannerData> & planner_data,
+    PlannerDebugData & planner_debug_data) override;
+
+  friend class TestFreespacePullOut;
 
 protected:
   std::unique_ptr<AbstractPlanningAlgorithm> planner_;

diff --git a/...planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/...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<autoware::lane_departure_checker::LaneDepartureChecker>
-      lane_departure_checker,
     std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
       std::make_shared<universe_utils::TimeKeeper>());
 
   PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; };
   std::optional<PullOutPath> plan(
     const Pose & start_pose, const Pose & goal_pose,
+    const std::shared_ptr<const PlannerData> & planner_data,
     PlannerDebugData & planner_debug_data) override;
 
   GeometricParallelParking planner_;

diff --git a/...nner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/...nner_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<universe_utils::TimeKeeper> time_keeper =
       std::make_shared<universe_utils::TimeKeeper>())
-  : 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<const PlannerData> & 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<PullOutPath> 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<const PlannerData> & planner_data,
+    PlannerDebugData & planner_debug_data) = 0;
 
 protected:
   bool isPullOutPathCollided(
     autoware::behavior_path_planner::PullOutPath & pull_out_path,
+    const std::shared_ptr<const PlannerData> & planner_data,
     double collision_check_distance_from_end) const;
 
-  std::shared_ptr<const PlannerData> planner_data_;
   StartPlannerParameters parameters_;
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
   LinearRing2d vehicle_footprint_;

diff --git a/...art_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/...art_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<LaneDepartureChecker> & lane_departure_checker,
     std::shared_ptr<universe_utils::TimeKeeper> time_keeper =
       std::make_shared<universe_utils::TimeKeeper>());
 
   PlannerType getPlannerType() const override { return PlannerType::SHIFT; };
   std::optional<PullOutPath> plan(
     const Pose & start_pose, const Pose & goal_pose,
+    const std::shared_ptr<const PlannerData> & planner_data,
     PlannerDebugData & planner_debug_data) override;
 
   std::vector<PullOutPath> 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/...anner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/...anner_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 <autoware/lane_departure_checker/lane_departure_checker.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
@@ -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<std::pair<size_t, std::shared_ptr<PullOutPlannerBase>>>;
 
@@ -310,8 +308,6 @@ ego pose.
   std::vector<Pose> searchPullOutStartPoseCandidates(
     const PathWithLaneId & back_path_from_start_pose) const;
 
-  std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
-
   // turn signal
   TurnSignalInfo calcTurnSignalInfo();
 

diff --git a/...avior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/...avior_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<PullOutPath> 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<const PlannerData> & 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<PullOutPath> FreespacePullOut::plan(
   }
 
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    planner_data, backward_path_length, std::numeric_limits<double>::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 =