From a3dfbda8fc1c91db5acadf6076817fe04beb1023 Mon Sep 17 00:00:00 2001 From: Takumi Ito Date: Thu, 30 Jan 2025 21:06:55 -0500 Subject: [PATCH] append enable_clothoid_* param instead of *_use_clothoid Signed-off-by: Takumi Ito --- .../config/goal_planner.param.yaml | 6 +++--- .../goal_planner_parameters.hpp | 4 ++++ .../pull_over_planner/geometric_pull_over.hpp | 3 ++- .../src/goal_planner_module.cpp | 10 ++++++++-- .../src/manager.cpp | 20 +++++++++---------- .../pull_over_planner/geometric_pull_over.cpp | 5 +++-- .../geometric_parallel_parking.hpp | 9 +++------ .../geometric_parallel_parking.cpp | 20 +++++++++---------- .../config/start_planner.param.yaml | 2 +- .../data_structs.hpp | 1 + .../geometric_pull_out.hpp | 3 ++- .../src/data_structs.cpp | 2 ++ .../src/geometric_pull_out.cpp | 7 ++++--- .../src/manager.cpp | 4 +--- .../src/start_planner_module.cpp | 5 ++++- .../test/test_geometric_pull_out.cpp | 2 +- 16 files changed, 58 insertions(+), 45 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 74af4f11dfa44..2d3c9053df44c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -59,7 +59,7 @@ maximum_deceleration: 1.0 maximum_jerk: 1.0 path_priority: "efficient_path" # "efficient_path" or "close_goal" - efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) + efficient_path_order: ["SHIFT", "ARC_FORWARD", "CLOTHOID_FORWARD", "ARC_BACKWARD", "CLOTHOID_BACKWARD"] # only lane based pull over(exclude freespace parking) lane_departure_check_expansion_margin: 0.2 # shift parking @@ -77,22 +77,22 @@ max_steer_angle: 0.4 #22.9deg forward: enable_arc_forward_parking: true + enable_clothoid_forward_parking: true after_forward_parking_straight_distance: 2.0 forward_parking_velocity: 1.38 forward_parking_lane_departure_margin: 0.0 forward_parking_path_interval: 1.0 forward_parking_max_steer_angle: 0.4 # 22.9deg forward_parking_steer_rate_lim: 0.35 - forward_parking_use_clothoid: true backward: enable_arc_backward_parking: true + enable_clothoid_backward_parking: false # Not supported yet after_backward_parking_straight_distance: 2.0 backward_parking_velocity: -1.38 backward_parking_lane_departure_margin: 0.0 backward_parking_path_interval: 1.0 backward_parking_max_steer_angle: 0.4 # 22.9deg backward_parking_steer_rate_lim: 0.35 - backward_parking_use_clothoid: false # Not supported yet # freespace parking freespace_parking: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 814cffd388703..f8cc606092f2d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -111,6 +111,10 @@ struct GoalPlannerParameters bool enable_arc_backward_parking{false}; ParallelParkingParameters parallel_parking_parameters; + // clothoid parking + bool enable_clothoid_forward_parking{false}; + bool enable_clothoid_backward_parking{false}; + // freespace parking bool enable_freespace_parking{false}; std::string freespace_parking_algorithm; 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 e12a759eb2137..363eb82c205b4 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,7 +32,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward); + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward, const bool use_clothoid); PullOverPlannerType getPlannerType() const override { @@ -61,6 +61,7 @@ class GeometricPullOver : public PullOverPlannerBase const ParallelParkingParameters parallel_parking_parameters_; const LaneDepartureChecker lane_departure_checker_; const bool is_forward_; + const bool use_clothoid_; const bool left_side_parking_; GeometricParallelParking planner_; 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 f4d960a868cb2..c9b9a671b0048 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 @@ -275,10 +275,16 @@ LaneParkingPlanner::LaneParkingPlanner( 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, /*is_forward*/ true)); + std::make_shared(node, parameters, /*is_forward*/ true, /*use_clothoid*/ false)); + } else if (planner_type == "CLOTHOID_FORWARD" && parameters.enable_clothoid_forward_parking) { + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ true, /*use_clothoid*/ true)); } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { pull_over_planners_.push_back( - std::make_shared(node, parameters, /*is_forward*/ false)); + std::make_shared(node, parameters, /*is_forward*/ false, /*use_clothoid*/ false)); + } else if (planner_type == "CLOTHOID_BACKWARD" && parameters.enable_clothoid_backward_parking) { + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ false, /*use_clothoid*/ true)); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 922a31201e1d9..2390c76611eff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -174,7 +174,10 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( // forward parallel parking forward { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; - p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); + p.enable_arc_forward_parking = + node->declare_parameter(ns + "enable_arc_forward_parking"); + p.enable_clothoid_forward_parking = + node->declare_parameter(ns + "enable_clothoid_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); p.parallel_parking_parameters.forward_parking_velocity = @@ -187,8 +190,6 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( node->declare_parameter(ns + "forward_parking_max_steer_angle"); // 20deg p.parallel_parking_parameters.forward_parking_steer_rate_lim = node->declare_parameter(ns + "forward_parking_steer_rate_lim"); - p.parallel_parking_parameters.forward_parking_use_clothoid = - node->declare_parameter(ns + "forward_parking_use_clothoid"); } // forward parallel parking backward @@ -196,6 +197,8 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); + p.enable_clothoid_backward_parking = + node->declare_parameter(ns + "enable_clothoid_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = node->declare_parameter(ns + "after_backward_parking_straight_distance"); p.parallel_parking_parameters.backward_parking_velocity = @@ -208,8 +211,6 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( node->declare_parameter(ns + "backward_parking_max_steer_angle"); // 20deg p.parallel_parking_parameters.backward_parking_steer_rate_lim = node->declare_parameter(ns + "backward_parking_steer_rate_lim"); - p.parallel_parking_parameters.backward_parking_use_clothoid = - node->declare_parameter(ns + "backward_parking_use_clothoid"); } // freespace parking general params @@ -571,6 +572,7 @@ void GoalPlannerModuleManager::updateModuleParams( { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; updateParam(parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); + updateParam(parameters, ns + "enable_clothoid_forward_parking", p->enable_clothoid_forward_parking); updateParam( parameters, ns + "after_forward_parking_straight_distance", p->parallel_parking_parameters.after_forward_parking_straight_distance); @@ -589,9 +591,6 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "forward_parking_steer_rate_lim", p->parallel_parking_parameters.forward_parking_steer_rate_lim); - updateParam( - parameters, ns + "forward_parking_use_clothoid", - p->parallel_parking_parameters.forward_parking_use_clothoid); } // forward parallel parking backward @@ -599,6 +598,8 @@ void GoalPlannerModuleManager::updateModuleParams( const std::string ns = base_ns + "pull_over.parallel_parking.backward."; updateParam( parameters, ns + "enable_arc_backward_parking", p->enable_arc_backward_parking); + updateParam( + parameters, ns + "enable_clothoid_backward_parking", p->enable_clothoid_backward_parking); updateParam( parameters, ns + "after_backward_parking_straight_distance", p->parallel_parking_parameters.after_backward_parking_straight_distance); @@ -617,9 +618,6 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "backward_parking_steer_rate_lim", p->parallel_parking_parameters.backward_parking_steer_rate_lim); - updateParam( - parameters, ns + "backward_parking_use_clothoid", - p->parallel_parking_parameters.backward_parking_use_clothoid); } // freespace parking general params 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 b7b4ad83de362..ce6a53e3660da 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,7 +26,7 @@ namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward, const bool use_clothoid) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{[&]() { @@ -36,6 +36,7 @@ GeometricPullOver::GeometricPullOver( return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; }()}, is_forward_{is_forward}, + use_clothoid_{use_clothoid}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { planner_.setParameters(parallel_parking_parameters_); @@ -67,7 +68,7 @@ std::optional GeometricPullOver::plan( planner_.setPlannerData(planner_data); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_, use_clothoid_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index f85d4348797a5..3ee2e47d9835a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -50,7 +50,6 @@ struct ParallelParkingParameters double forward_parking_path_interval{0.0}; double forward_parking_max_steer_angle{0.0}; double forward_parking_steer_rate_lim{0.0}; - bool forward_parking_use_clothoid{false}; // backward parking double after_backward_parking_straight_distance{0.0}; @@ -59,7 +58,6 @@ struct ParallelParkingParameters double backward_parking_path_interval{0.0}; double backward_parking_max_steer_angle{0.0}; double backward_parking_steer_rate_lim{0.0}; - bool backward_parking_use_clothoid{false}; // pull_out double pull_out_velocity{0.0}; @@ -67,7 +65,6 @@ struct ParallelParkingParameters double pull_out_arc_path_interval{0.0}; double pull_out_max_steer_angle{0.0}; double pull_out_steer_rate_lim{0.0}; - bool pull_out_use_clothoid{false}; }; class GeometricParallelParking @@ -77,10 +74,10 @@ class GeometricParallelParking bool planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, - const bool left_side_parking); + const bool left_side_parking, const bool use_clothoid); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, const bool use_clothoid, const std::shared_ptr autoware_lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } @@ -149,7 +146,7 @@ class GeometricParallelParking std::vector generatePullOverPaths( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, - const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const bool is_forward, const bool left_side_parking, const bool use_clothoid, const double end_pose_offset, const double velocity); PathWithLaneId generateStraightPath( const Pose & start_pose, const lanelet::ConstLanelets & road_lanes, const bool set_stop_end); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index f7556f43171d4..735a6bda129d0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -110,7 +110,7 @@ void GeometricParallelParking::setVelocityToArcPaths( std::vector GeometricParallelParking::generatePullOverPaths( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, - const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const bool is_forward, const bool left_side_parking, const bool use_clothoid, const double end_pose_offset, const double velocity) { const double lane_departure_margin = is_forward @@ -119,7 +119,7 @@ std::vector GeometricParallelParking::generatePullOverPaths( const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval : parameters_.backward_parking_path_interval; std::vector arc_paths; - if (is_forward && parameters_.forward_parking_use_clothoid) { // clothoid parking only supports + if (is_forward && use_clothoid) { // clothoid parking only supports // forward for now const double L_min = is_forward @@ -148,7 +148,7 @@ std::vector GeometricParallelParking::generatePullOverPaths( setVelocityToArcPaths(arc_paths, velocity, set_stop_end); // straight path from current to parking start - const bool set_stop_straight_end = !(is_forward && parameters_.forward_parking_use_clothoid); + const bool set_stop_straight_end = !(is_forward && use_clothoid); const auto straight_path = generateStraightPath(start_pose, road_lanes, set_stop_straight_end); // check the continuity of straight path and arc path @@ -163,7 +163,7 @@ std::vector GeometricParallelParking::generatePullOverPaths( // combine straight_path -> arc_path*2 std::vector paths; - if (is_forward && parameters_.forward_parking_use_clothoid) { + if (is_forward && use_clothoid) { paths.push_back(straight_path); for (const auto & path : arc_paths) { for (const auto & path_point : path.points) { @@ -190,7 +190,7 @@ void GeometricParallelParking::clearPaths() bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, - const bool left_side_parking) + const bool left_side_parking, const bool use_clothoid) { const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance @@ -219,7 +219,7 @@ bool GeometricParallelParking::planPullOver( } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, + *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, use_clothoid, end_pose_offset, parameters_.forward_parking_velocity); if (!paths.empty()) { paths_ = paths; @@ -242,7 +242,7 @@ bool GeometricParallelParking::planPullOver( const auto paths = generatePullOverPaths( *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, - left_side_parking, end_pose_offset, parameters_.backward_parking_velocity); + left_side_parking, use_clothoid, end_pose_offset, parameters_.backward_parking_velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -255,7 +255,7 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, const bool use_clothoid, const std::shared_ptr lane_departure_checker) { @@ -275,7 +275,7 @@ bool GeometricParallelParking::planPullOut( // plan reverse path of parking. end_pose <-> start_pose std::vector arc_paths; - if (parameters_.pull_out_use_clothoid) { + if (use_clothoid) { const double L_min = std::abs( parameters_.pull_out_velocity * (parameters_.pull_out_max_steer_angle / parameters_.pull_out_steer_rate_lim)); @@ -982,7 +982,7 @@ std::vector GeometricParallelParking::generateClothoidalSequence std::reverse(p.lane_ids.begin(), p.lane_ids.end()); } - if (theta != 0) { + if (abs(theta) > 1e-6) { // generate arc path which connect two clothoid paths const auto end_of_prev_path = clothoid_path_first.points.back().point.pose; const auto start_of_next_path = clothoid_path_second.points.front().point.pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index a803bc7359ed4..1b6792a180855 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -36,6 +36,7 @@ end_pose_curvature_threshold: 0.1 # geometric pull out enable_geometric_pull_out: true + enable_clothoid_pull_out: true geometric_collision_check_distance_from_end: 0.0 arc_path_interval: 1.0 divide_pull_out_path: true @@ -45,7 +46,6 @@ backward_velocity: -1.0 pull_out_max_steer_angle: 0.26 # 15deg pull_out_steer_rate_lim: 0.35 - pull_out_use_clothoid: true # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 4a2ca54e7d38c..e0a31703e5eba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -132,6 +132,7 @@ struct StartPlannerParameters double maximum_longitudinal_deviation{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + bool enable_clothoid_pull_out{false}; double geometric_collision_check_distance_from_end{0.0}; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; 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 5d36c996cca3a..b52ecccbeddf3 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 @@ -32,7 +32,7 @@ class GeometricPullOut : public PullOutPlannerBase { public: explicit GeometricPullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, + rclcpp::Node & node, const StartPlannerParameters & parameters, const bool use_clothoid, std::shared_ptr time_keeper = std::make_shared()); @@ -45,6 +45,7 @@ class GeometricPullOut : public PullOutPlannerBase GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; std::shared_ptr lane_departure_checker_; + const bool use_clothoid_; friend class TestGeometricPullOut; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp index f613b9345ce42..9936aacd7331e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -72,6 +72,8 @@ StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) // geometric pull out p.enable_geometric_pull_out = getOrDeclareParameter(node, ns + "enable_geometric_pull_out"); + p.enable_clothoid_pull_out = + getOrDeclareParameter(node, ns + "enable_clothoid_pull_out"); p.geometric_collision_check_distance_from_end = getOrDeclareParameter(node, ns + "geometric_collision_check_distance_from_end"); p.divide_pull_out_path = getOrDeclareParameter(node, ns + "divide_pull_out_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 ed09e0c0a33d6..c111cc61ad8a2 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 @@ -35,10 +35,11 @@ namespace autoware::behavior_path_planner using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, + rclcpp::Node & node, const StartPlannerParameters & parameters, const bool use_clothoid, std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, - parallel_parking_parameters_{parameters.parallel_parking_parameters} + parallel_parking_parameters_{parameters.parallel_parking_parameters}, + use_clothoid_{use_clothoid} { auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; lane_departure_checker_params.footprint_extra_margin = @@ -70,7 +71,7 @@ std::optional GeometricPullOut::plan( 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_); + start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, use_clothoid_, lane_departure_checker_); if (!found_valid_path) { planner_debug_data.conditions_evaluation.emplace_back("no path found"); return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 7fb115b3fb43e..498da60572c54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -113,6 +113,7 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + updateParam(parameters, ns + "enable_clothoid_pull_out", p->enable_clothoid_pull_out); updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); updateParam( parameters, ns + "arc_path_interval", @@ -129,9 +130,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "pull_out_steer_rate_lim", p->parallel_parking_parameters.pull_out_steer_rate_lim); - updateParam( - parameters, ns + "pull_out_use_clothoid", - p->parallel_parking_parameters.pull_out_use_clothoid); updateParam(parameters, ns + "enable_back", p->enable_back); updateParam(parameters, ns + "backward_velocity", p->backward_velocity); updateParam( 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 37bf91d6d0a1b..d05aa92f1ceba 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 @@ -74,7 +74,10 @@ StartPlannerModule::StartPlannerModule( 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, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, /*use_clothoid*/ false, time_keeper_)); + } + if (parameters_->enable_clothoid_pull_out) { + start_planners_.push_back(std::make_shared(node, *parameters, /*use_clothoid*/ true, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled 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 4548c058b871d..4af240f0f8a40 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 @@ -70,7 +70,7 @@ class TestGeometricPullOut : public ::testing::Test { auto parameters = StartPlannerParameters::init(*node_); - geometric_pull_out_ = std::make_shared(*node_, parameters); + geometric_pull_out_ = std::make_shared(*node_, parameters, false); } };