From 3d94c850e55f7f44cd817ced9721f08b811e2fe6 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Wed, 25 Dec 2024 18:31:32 +0900 Subject: [PATCH] refactor(planner): add planner_data parameter to plan methods in pull out planners Signed-off-by: kyoichi-sugahara --- .../freespace_pull_out.hpp | 4 +- .../geometric_pull_out.hpp | 1 + .../pull_out_planner_base.hpp | 19 +-- .../shift_pull_out.hpp | 4 +- .../src/freespace_pull_out.cpp | 15 +- .../src/geometric_pull_out.cpp | 16 +- .../src/pull_out_planner_base.cpp | 7 +- .../src/shift_pull_out.cpp | 29 ++-- .../src/start_planner_module.cpp | 9 +- .../test/test_geometric_pull_out.cpp | 140 ++++++------------ .../test/test_shift_pull_out.cpp | 19 ++- 11 files changed, 112 insertions(+), 151 deletions(-) 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 e358e35ed7794..a637d630faf03 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 @@ -43,7 +43,9 @@ 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; 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 18d1f3c3b9b81..35b4925c9f3b4 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 @@ -40,6 +40,7 @@ class GeometricPullOut : public PullOutPlannerBase 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 f606679e7f2da..b7b42a395fa94 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 @@ -41,33 +41,30 @@ class PullOutPlannerBase explicit PullOutPlannerBase( rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr time_keeper) - : time_keeper_(time_keeper) + : vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, + parameters_{parameters}, + time_keeper_(time_keeper) { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info_.createFootprint(); - parameters_ = parameters; } 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_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; 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 5b4f78b252d22..d91f04da46058 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 @@ -41,11 +41,13 @@ class ShiftPullOut : public PullOutPlannerBase 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/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 42698f799c6b3..38aba0da890c8 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 @@ -51,13 +51,14 @@ FreespacePullOut::FreespacePullOut( } 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)) { @@ -69,11 +70,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..0927f27bf6d87 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 @@ -47,24 +47,25 @@ GeometricPullOut::GeometricPullOut( } 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 +123,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..05d26aa925f16 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 @@ -48,24 +48,26 @@ ShiftPullOut::ShiftPullOut( } 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 +161,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 +230,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 +242,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 c223390e454d1..5ff84d498f49a 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 @@ -986,17 +986,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; @@ -1595,9 +1594,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/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index fb39e186afd4e..0d33dbb13df4a 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 @@ -37,6 +37,7 @@ using autoware::test_utils::get_absolute_path_to_config; using autoware_planning_msgs::msg::LaneletRoute; using RouteSections = std::vector; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; +using geometry_msgs::msg::Pose; namespace autoware::behavior_path_planner { @@ -44,37 +45,61 @@ namespace autoware::behavior_path_planner class TestGeometricPullOut : public ::testing::Test { public: - std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + std::optional call_plan( + 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", get_node_options()); + node_ = rclcpp::Node::make_shared("geometric_pull_out", make_node_options()); - load_parameters(); - initialize_vehicle_info(); initialize_lane_departure_checker(); - initialize_routeHandler(); initialize_geometric_pull_out_planner(); - initialize_planner_data(); } - void TearDown() override { rclcpp::shutdown(); } + + std::shared_ptr make_planner_data( + const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) + { + auto planner_data = std::make_shared(); + 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 route_handler_; - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::shared_ptr geometric_pull_out_; std::shared_ptr lane_departure_checker_; - PlannerData planner_data_; private: - rclcpp::NodeOptions get_node_options() const + rclcpp::NodeOptions make_node_options() const { // Load common configuration files auto node_options = rclcpp::NodeOptions{}; @@ -102,85 +127,25 @@ class TestGeometricPullOut : public ::testing::Test return node_options; } - void load_parameters() - { - const auto dp_double = [&](const std::string & s) { - return node_->declare_parameter(s); - }; - const auto dp_bool = [&](const std::string & s) { return node_->declare_parameter(s); }; - // Load parameters required for planning - const std::string ns = "start_planner."; - lane_departure_check_expansion_margin_ = - dp_double(ns + "lane_departure_check_expansion_margin"); - pull_out_max_steer_angle_ = dp_double(ns + "pull_out_max_steer_angle"); - pull_out_arc_path_interval_ = dp_double(ns + "arc_path_interval"); - center_line_path_interval_ = dp_double(ns + "center_line_path_interval"); - th_moving_object_velocity_ = dp_double(ns + "th_moving_object_velocity"); - divide_pull_out_path_ = dp_bool(ns + "divide_pull_out_path"); - backward_path_length_ = dp_double("backward_path_length"); - forward_path_length_ = dp_double("forward_path_length"); - } - - void initialize_vehicle_info() - { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - } - void initialize_lane_departure_checker() { + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); lane_departure_checker_ = std::make_shared(); - lane_departure_checker_->setVehicleInfo(vehicle_info_); + lane_departure_checker_->setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = lane_departure_check_expansion_margin_; lane_departure_checker_->setParam(lane_departure_checker_params); } - void initialize_routeHandler() - { - // 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); - - route_handler_ = std::make_shared(map_bin_msg); - } - void initialize_geometric_pull_out_planner() { - auto parameters = std::make_shared(); - parameters->parallel_parking_parameters.pull_out_max_steer_angle = pull_out_max_steer_angle_; - parameters->parallel_parking_parameters.pull_out_arc_path_interval = - pull_out_arc_path_interval_; - parameters->parallel_parking_parameters.center_line_path_interval = center_line_path_interval_; - parameters->th_moving_object_velocity = th_moving_object_velocity_; - parameters->divide_pull_out_path = divide_pull_out_path_; + auto parameters = StartPlannerParameters::init(*node_); auto time_keeper = std::make_shared(); geometric_pull_out_ = - std::make_shared(*node_, *parameters, lane_departure_checker_, time_keeper); - } - - void initialize_planner_data() - { - planner_data_.parameters.backward_path_length = backward_path_length_; - planner_data_.parameters.forward_path_length = forward_path_length_; - planner_data_.parameters.wheel_base = vehicle_info_.wheel_base_m; - planner_data_.parameters.wheel_tread = vehicle_info_.wheel_tread_m; - planner_data_.parameters.front_overhang = vehicle_info_.front_overhang_m; - planner_data_.parameters.left_over_hang = vehicle_info_.left_overhang_m; - planner_data_.parameters.right_over_hang = vehicle_info_.right_overhang_m; + std::make_shared(*node_, parameters, lane_departure_checker_, time_keeper); } - - // Parameter variables - double lane_departure_check_expansion_margin_{0.0}; - double pull_out_max_steer_angle_{0.0}; - double pull_out_arc_path_interval_{0.0}; - double center_line_path_interval_{0.0}; - double th_moving_object_velocity_{0.0}; - double backward_path_length_{0.0}; - double forward_path_length_{0.0}; - bool divide_pull_out_path_{false}; }; TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) @@ -199,26 +164,13 @@ TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - // 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( - 4619, 4635, "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_; - geometric_pull_out_->setPlannerData(std::make_shared(planner_data_)); + const auto planner_data = make_planner_data(start_pose, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = 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 156cf62f9ac4a..6ac7ed2f4e2f8 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 @@ -43,9 +43,10 @@ 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: @@ -60,11 +61,11 @@ class TestShiftPullOut : public ::testing::Test void TearDown() override { rclcpp::shutdown(); } - PlannerData make_planner_data( + std::shared_ptr 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_); + auto planner_data = std::make_shared(); + 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( @@ -76,7 +77,7 @@ class TestShiftPullOut : public ::testing::Test auto odometry = std::make_shared(); odometry->pose.pose = start_pose; odometry->header.frame_id = "map"; - planner_data.self_odometry = odometry; + planner_data->self_odometry = odometry; // Setup route const auto route = makeBehaviorRouteFromLaneId( @@ -85,7 +86,7 @@ class TestShiftPullOut : public ::testing::Test route_handler->setRoute(route); // Update planner data with the route handler - planner_data.route_handler = route_handler; + planner_data->route_handler = route_handler; return planner_data; } @@ -162,11 +163,9 @@ TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) 0.708314)); const auto planner_data = make_planner_data(start_pose, 4619, 4635); - shift_pull_out_->setPlannerData(std::make_shared(planner_data)); - // 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.";