From 3fd346ea525d63b96d6647824f54c39b63083e2d Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 3 May 2023 01:57:38 +0900 Subject: [PATCH] refactor(behavior_path_planner): refactoring goal_planner and pull_out params Signed-off-by: kosuke55 --- .../goal_planner/goal_planner.param.yaml | 182 +++++++------ ...havior_path_planner_goal_planner_design.md | 16 +- .../goal_planner/goal_planner_module.hpp | 1 - .../scene_module/pull_out/pull_out_module.hpp | 1 - .../geometric_parallel_parking.hpp | 40 ++- .../goal_planner/geometric_pull_over.hpp | 1 - .../goal_planner/goal_planner_parameters.hpp | 60 ++--- .../utils/pull_out/geometric_pull_out.hpp | 4 +- .../utils/pull_out/pull_out_parameters.hpp | 9 +- .../src/behavior_path_planner_node.cpp | 245 +++++++++++------- .../goal_planner/goal_planner_module.cpp | 29 +-- .../scene_module/pull_out/pull_out_module.cpp | 20 +- .../geometric_parallel_parking.cpp | 45 ++-- .../goal_planner/freespace_pull_over.cpp | 9 +- .../goal_planner/geometric_pull_over.cpp | 12 +- .../utils/goal_planner/shift_pull_over.cpp | 8 +- .../src/utils/pull_out/geometric_pull_out.cpp | 15 +- 17 files changed, 361 insertions(+), 336 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 5a840098e5956..3c5846edf3ee9 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -1,96 +1,108 @@ /**: ros__parameters: goal_planner: + # general params minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. - pull_over_velocity: 3.0 - pull_over_minimum_velocity: 1.38 - margin_from_boundary: 0.5 - decide_path_distance: 10.0 - maximum_deceleration: 1.0 - maximum_jerk: 1.0 + # goal search - search_priority: "efficient_path" # "efficient_path" or "close_goal" - parking_policy: "left_side" # "left_side" or "right_side" - forward_goal_search_length: 20.0 - backward_goal_search_length: 20.0 - goal_search_interval: 2.0 - longitudinal_margin: 3.0 - max_lateral_offset: 0.5 - lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + goal_search: + search_priority: "efficient_path" # "efficient_path" or "close_goal" + parking_policy: "left_side" # "left_side" or "right_side" + forward_goal_search_length: 20.0 + backward_goal_search_length: 20.0 + goal_search_interval: 2.0 + longitudinal_margin: 3.0 + max_lateral_offset: 0.5 + lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.0 + margin_from_boundary: 0.5 + # occupancy grid map - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false - occupancy_grid_collision_check_margin: 0.0 - theta_size: 360 - obstacle_threshold: 60 + occupancy_grid: + use_occupancy_grid: true + use_occupancy_grid_for_longitudinal_margin: false + occupancy_grid_collision_check_margin: 0.0 + theta_size: 360 + obstacle_threshold: 60 + # object recognition - use_object_recognition: true - object_recognition_collision_check_margin: 1.0 - # shift path - enable_shift_parking: true - pull_over_sampling_num: 4 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - after_pull_over_straight_distance: 1.0 - # freespace parking - enable_freespace_parking: true - freespace_parking: - planning_algorithm: "astar" # options: astar, rrtstar - velocity: 1.0 - vehicle_shape_margin: 1.0 - time_limit: 3000.0 - minimum_turning_radius: 5.0 - maximum_turning_radius: 5.0 - turning_radius_size: 1 - # search configs - theta_size: 144 - angle_goal_range: 6.0 - curve_weight: 1.2 - reverse_weight: 1.0 - lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 - # costmap configs - obstacle_threshold: 30 - # -- A* search Configurations -- - astar: - only_behind_solutions: false - use_back: false - distance_heuristic_weight: 1.0 - # -- RRT* search Configurations -- - rrtstar: - enable_update: true - use_informed_sampling: true - max_planning_time: 150.0 - neighbor_radius: 8.0 - margin: 1.0 - # parallel parking path - enable_arc_forward_parking: true - enable_arc_backward_parking: true - after_forward_parking_straight_distance: 2.0 - after_backward_parking_straight_distance: 2.0 - forward_parking_velocity: 1.38 - backward_parking_velocity: -1.38 - forward_parking_lane_departure_margin: 0.0 - backward_parking_lane_departure_margin: 0.0 - arc_path_interval: 1.0 - pull_over_max_steer_angle: 0.35 # 20deg - # hazard on when parked - hazard_on_threshold_distance: 1.0 - hazard_on_threshold_velocity: 0.5 - # check safety with dynamic objects. Not used now. - pull_over_duration: 2.0 - pull_over_prepare_duration: 4.0 - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - prediction_time_resolution: 0.5 - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false + object_recognition: + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 + + # pull over + pull_over: + pull_over_velocity: 3.0 + pull_over_minimum_velocity: 1.38 + decide_path_distance: 10.0 + maximum_deceleration: 1.0 + maximum_jerk: 1.0 + + # shift parking + shift_parking: + enable_shift_parking: true + shift_sampling_num: 4 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + after_shift_straight_distance: 1.0 + + # parallel parking path + parallel_parking: + path_interval: 1.0 + max_steer_angle: 0.35 # 20deg + forward: + enable_arc_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.35 # 20deg + backward: + enable_arc_backward_parking: true + 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.35 # 20deg + + # freespace parking + freespace_parking: + enable_freespace_parking: true + freespace_parking_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 + # debug - print_debug_info: false + debug: + print_debug_info: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 06f81758f163f..d927546089159 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -153,14 +153,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val #### Parameters for shift parking -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | -| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | -| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | -| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| shift_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_shift_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | ### **geometric parallel parking** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 500e37b293a97..c458fd16a7073 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -165,7 +165,6 @@ class GoalPlannerModule : public SceneModuleInterface bool incrementPathIndex(); PathWithLaneId getCurrentPath() const; Pose calcRefinedGoal(const Pose & goal_pose) const; - ParallelParkingParameters getGeometricGoalPlannerParameters() const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart(const Pose & search_start_pose, PathWithLaneId & path) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 1546015d5479c..63720996782f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -117,7 +117,6 @@ class PullOutModule : public SceneModuleInterface std::shared_ptr getCurrentPlanner() const; PathWithLaneId getFullPath() const; - ParallelParkingParameters getGeometricPullOutParameters() const; std::vector searchPullOutStartPoses(); std::shared_ptr lane_departure_checker_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index b5cc03897e2fd..18bf7ad54e70c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -49,17 +49,27 @@ struct ParallelParkingParameters { double th_arrived_distance; double th_stopped_velocity; + double maximum_deceleration; + + // forward parking double after_forward_parking_straight_distance; - double after_backward_parking_straight_distance; double forward_parking_velocity; + double forward_parking_lane_departure_margin; + double forward_parking_path_interval; + double forward_parking_max_steer_angle; + + // backward parking + double after_backward_parking_straight_distance; double backward_parking_velocity; - double departing_velocity; double backward_parking_lane_departure_margin; - double forward_parking_lane_departure_margin; - double departing_lane_departure_margin; - double arc_path_interval; - double maximum_deceleration; - double max_steer_angle; + double backward_parking_path_interval; + double backward_parking_max_steer_angle; + + // pull_out + double pull_out_velocity; + double pull_out_lane_departure_margin; + double pull_out_path_interval; + double pull_out_max_steer_angle; }; class GeometricParallelParking @@ -72,9 +82,14 @@ class GeometricParallelParking bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes); - void setData( - const std::shared_ptr & planner_data, - const ParallelParkingParameters & parameters); + void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } + void setPlannerData(const std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + void setTurningRadius( + const BehaviorPathPlannerParameters & common_params, const double max_steer_angle); + void incrementPathIndex(); std::vector getArcPaths() const { return arc_paths_; } @@ -103,10 +118,11 @@ class GeometricParallelParking std::vector planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_r, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - bool is_forward, const double end_pose_offset, const double lane_departure_margin); + bool is_forward, const double end_pose_offset, const double lane_departure_margin, + const double arc_path_interval); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, - const bool is_left_turn, const bool is_forward); + const double arc_path_interval, const bool is_left_turn, const bool is_forward); PathPointWithLaneId generateArcPathPoint( const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp index 6177a6197a142..aee78930d2e1f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp @@ -34,7 +34,6 @@ class GeometricPullOver : public PullOverPlannerBase public: GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr occupancy_grid_map, const bool is_forward); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index ff091a88ed44a..6da7c131a281f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -16,6 +16,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" + #include #include #include @@ -37,14 +39,12 @@ enum class ParkingPolicy { struct GoalPlannerParameters { + // general params double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; - double margin_from_boundary; - double decide_path_distance; - double maximum_deceleration; - double maximum_jerk; + // goal search std::string search_priority; // "efficient_path" or "close_goal" ParkingPolicy parking_policy; // "left_side" or "right_side" @@ -55,60 +55,50 @@ struct GoalPlannerParameters double max_lateral_offset; double lateral_offset_interval; double ignore_distance_from_lane_start; + double margin_from_boundary; + // occupancy grid map bool use_occupancy_grid; bool use_occupancy_grid_for_longitudinal_margin; double occupancy_grid_collision_check_margin; int theta_size; int obstacle_threshold; + // object recognition bool use_object_recognition; double object_recognition_collision_check_margin; + + // pull over general params + double pull_over_velocity; + double pull_over_minimum_velocity; + double decide_path_distance; + double maximum_deceleration; + double maximum_jerk; + // shift path bool enable_shift_parking; - int pull_over_sampling_num; + int shift_sampling_num; double maximum_lateral_jerk; double minimum_lateral_jerk; double deceleration_interval; - double pull_over_velocity; - double pull_over_minimum_velocity; - double after_pull_over_straight_distance; + double after_shift_straight_distance; + // parallel parking bool enable_arc_forward_parking; bool enable_arc_backward_parking; - double after_forward_parking_straight_distance; - double after_backward_parking_straight_distance; - double forward_parking_velocity; - double backward_parking_velocity; - double forward_parking_lane_departure_margin; - double backward_parking_lane_departure_margin; - double arc_path_interval; - double pull_over_max_steer_angle; + ParallelParkingParameters parallel_parking_parameters; + // freespace parking bool enable_freespace_parking; - // hazard - double hazard_on_threshold_distance; - double hazard_on_threshold_velocity; - // check safety with dynamic objects. Not used now. - double pull_over_duration; - double pull_over_prepare_duration; - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; - double prediction_time_resolution; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - // debug - bool print_debug_info; - - // freespace pull over - std::string algorithm; + std::string freespace_parking_algorithm; double freespace_parking_velocity; double vehicle_shape_margin; - PlannerCommonParam common_parameters; + PlannerCommonParam freespace_parking_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // debug + bool print_debug_info; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp index 788b37448c4a8..0ab6bd465fb26 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp @@ -26,9 +26,7 @@ namespace behavior_path_planner class GeometricPullOut : public PullOutPlannerBase { public: - explicit GeometricPullOut( - rclcpp::Node & node, const PullOutParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters); + explicit GeometricPullOut(rclcpp::Node & node, const PullOutParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; boost::optional plan(Pose start_pose, Pose goal_pose) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp index ce4629bf9da19..0fc7d367718ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp @@ -16,6 +16,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ +#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" + #include #include @@ -39,14 +41,11 @@ struct PullOutParameters // geometric pull out bool enable_geometric_pull_out; bool divide_pull_out_path; - double geometric_pull_out_velocity; - double arc_path_interval; - double lane_departure_margin; - double backward_velocity; - double pull_out_max_steer_angle; + ParallelParkingParameters parallel_parking_parameters; // search start pose backward std::string search_priority; // "efficient_path" or "short_back_distance" bool enable_back; + double backward_velocity; double max_back_distance; double backward_search_resolution; double backward_path_update_duration; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 1e4f336383ac8..e58c71f00fc62 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -761,17 +761,18 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() { GoalPlannerParameters p; + // general params { std::string ns = "goal_planner."; p.minimum_request_length = declare_parameter(ns + "minimum_request_length"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); - p.margin_from_boundary = declare_parameter(ns + "margin_from_boundary"); - p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); - p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); - p.maximum_jerk = declare_parameter(ns + "maximum_jerk"); - // goal search + } + + // goal search + { + std::string ns = "goal_planner.goal_search."; p.search_priority = declare_parameter(ns + "search_priority"); p.forward_goal_search_length = declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = declare_parameter(ns + "backward_goal_search_length"); @@ -781,7 +782,24 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() p.lateral_offset_interval = declare_parameter(ns + "lateral_offset_interval"); p.ignore_distance_from_lane_start = declare_parameter(ns + "ignore_distance_from_lane_start"); - // occupancy grid map + p.margin_from_boundary = declare_parameter(ns + "margin_from_boundary"); + + const std::string parking_policy_name = declare_parameter(ns + "parking_policy"); + if (parking_policy_name == "left_side") { + p.parking_policy = ParkingPolicy::LEFT_SIDE; + } else if (parking_policy_name == "right_side") { + p.parking_policy = ParkingPolicy::RIGHT_SIDE; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); + exit(EXIT_FAILURE); + } + } + + // occupancy grid map + { + std::string ns = "goal_planner.occupancy_grid."; p.use_occupancy_grid = declare_parameter(ns + "use_occupancy_grid"); p.use_occupancy_grid_for_longitudinal_margin = declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); @@ -789,116 +807,118 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = declare_parameter(ns + "theta_size"); p.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); - // object recognition + } + + // object recognition + { + std::string ns = "goal_planner.object_recognition."; p.use_object_recognition = declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = declare_parameter(ns + "object_recognition_collision_check_margin"); - // shift path + } + + // pull over general params + { + std::string ns = "goal_planner.pull_over."; + p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); + p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); + p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); + p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); + p.maximum_jerk = declare_parameter(ns + "maximum_jerk"); + } + + // shift parking + { + std::string ns = "goal_planner.pull_over.shift_parking."; p.enable_shift_parking = declare_parameter(ns + "enable_shift_parking"); - p.pull_over_sampling_num = declare_parameter(ns + "pull_over_sampling_num"); + p.shift_sampling_num = declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = declare_parameter(ns + "maximum_lateral_jerk"); p.minimum_lateral_jerk = declare_parameter(ns + "minimum_lateral_jerk"); p.deceleration_interval = declare_parameter(ns + "deceleration_interval"); - p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); - p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); - p.after_pull_over_straight_distance = - declare_parameter(ns + "after_pull_over_straight_distance"); - // parallel parking + p.after_shift_straight_distance = + declare_parameter(ns + "after_shift_straight_distance"); + } + + // forward parallel parking forward + { + std::string ns = "goal_planner.pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = declare_parameter(ns + "enable_arc_forward_parking"); - p.enable_arc_backward_parking = declare_parameter(ns + "enable_arc_backward_parking"); - p.after_forward_parking_straight_distance = + p.parallel_parking_parameters.after_forward_parking_straight_distance = declare_parameter(ns + "after_forward_parking_straight_distance"); - p.after_backward_parking_straight_distance = - declare_parameter(ns + "after_backward_parking_straight_distance"); - p.forward_parking_velocity = declare_parameter(ns + "forward_parking_velocity"); - p.backward_parking_velocity = declare_parameter(ns + "backward_parking_velocity"); - p.forward_parking_lane_departure_margin = + p.parallel_parking_parameters.forward_parking_velocity = + declare_parameter(ns + "forward_parking_velocity"); + p.parallel_parking_parameters.forward_parking_lane_departure_margin = declare_parameter(ns + "forward_parking_lane_departure_margin"); - p.backward_parking_lane_departure_margin = - declare_parameter(ns + "backward_parking_lane_departure_margin"); - p.arc_path_interval = declare_parameter(ns + "arc_path_interval"); - p.pull_over_max_steer_angle = - declare_parameter(ns + "pull_over_max_steer_angle"); // 20deg - // freespace parking - p.enable_freespace_parking = declare_parameter(ns + "enable_freespace_parking"); - // hazard - p.hazard_on_threshold_distance = declare_parameter(ns + "hazard_on_threshold_distance"); - p.hazard_on_threshold_velocity = declare_parameter(ns + "hazard_on_threshold_velocity"); - // safety with dynamic objects. Not used now. - p.pull_over_duration = declare_parameter(ns + "pull_over_duration"); - p.pull_over_prepare_duration = declare_parameter(ns + "pull_over_prepare_duration"); - p.min_stop_distance = declare_parameter(ns + "min_stop_distance"); - p.stop_time = declare_parameter(ns + "stop_time"); - p.hysteresis_buffer_distance = declare_parameter(ns + "hysteresis_buffer_distance"); - p.prediction_time_resolution = declare_parameter(ns + "prediction_time_resolution"); - p.enable_collision_check_at_prepare_phase = - declare_parameter(ns + "enable_collision_check_at_prepare_phase"); - p.use_predicted_path_outside_lanelet = - declare_parameter(ns + "use_predicted_path_outside_lanelet"); - p.use_all_predicted_path = declare_parameter(ns + "use_all_predicted_path"); - // debug - p.print_debug_info = declare_parameter(ns + "print_debug_info"); - - // validation of parameters - if (p.pull_over_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - get_logger(), "pull_over_sampling_num must be positive integer. Given parameter: " - << p.pull_over_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } + p.parallel_parking_parameters.forward_parking_path_interval = + declare_parameter(ns + "forward_parking_path_interval"); + p.parallel_parking_parameters.forward_parking_max_steer_angle = + declare_parameter(ns + "forward_parking_max_steer_angle"); // 20deg + } - const std::string parking_policy_name = declare_parameter(ns + "parking_policy"); - if (parking_policy_name == "left_side") { - p.parking_policy = ParkingPolicy::LEFT_SIDE; - } else if (parking_policy_name == "right_side") { - p.parking_policy = ParkingPolicy::RIGHT_SIDE; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); - exit(EXIT_FAILURE); - } + // forward parallel parking backward + { + std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + p.enable_arc_backward_parking = declare_parameter(ns + "enable_arc_backward_parking"); + p.parallel_parking_parameters.after_backward_parking_straight_distance = + declare_parameter(ns + "after_backward_parking_straight_distance"); + p.parallel_parking_parameters.backward_parking_velocity = + declare_parameter(ns + "backward_parking_velocity"); + p.parallel_parking_parameters.backward_parking_lane_departure_margin = + declare_parameter(ns + "backward_parking_lane_departure_margin"); + p.parallel_parking_parameters.backward_parking_path_interval = + declare_parameter(ns + "backward_parking_path_interval"); + p.parallel_parking_parameters.backward_parking_max_steer_angle = + declare_parameter(ns + "backward_parking_max_steer_angle"); // 20deg } + // freespace parking general params { - std::string ns = "goal_planner.freespace_parking."; - // search configs - p.algorithm = declare_parameter(ns + "planning_algorithm"); + std::string ns = "goal_planner.pull_over.freespace_parking."; + p.enable_freespace_parking = declare_parameter(ns + "enable_freespace_parking"); + p.freespace_parking_algorithm = + declare_parameter(ns + "freespace_parking_algorithm"); p.freespace_parking_velocity = declare_parameter(ns + "velocity"); p.vehicle_shape_margin = declare_parameter(ns + "vehicle_shape_margin"); - p.common_parameters.time_limit = declare_parameter(ns + "time_limit"); - p.common_parameters.minimum_turning_radius = + p.freespace_parking_common_parameters.time_limit = declare_parameter(ns + "time_limit"); + p.freespace_parking_common_parameters.minimum_turning_radius = declare_parameter(ns + "minimum_turning_radius"); - p.common_parameters.maximum_turning_radius = + p.freespace_parking_common_parameters.maximum_turning_radius = declare_parameter(ns + "maximum_turning_radius"); - p.common_parameters.turning_radius_size = declare_parameter(ns + "turning_radius_size"); - p.common_parameters.maximum_turning_radius = std::max( - p.common_parameters.maximum_turning_radius, p.common_parameters.minimum_turning_radius); - p.common_parameters.turning_radius_size = std::max(p.common_parameters.turning_radius_size, 1); - - p.common_parameters.theta_size = declare_parameter(ns + "theta_size"); - p.common_parameters.angle_goal_range = declare_parameter(ns + "angle_goal_range"); - - p.common_parameters.curve_weight = declare_parameter(ns + "curve_weight"); - p.common_parameters.reverse_weight = declare_parameter(ns + "reverse_weight"); - p.common_parameters.lateral_goal_range = declare_parameter(ns + "lateral_goal_range"); - p.common_parameters.longitudinal_goal_range = + p.freespace_parking_common_parameters.turning_radius_size = + declare_parameter(ns + "turning_radius_size"); + p.freespace_parking_common_parameters.maximum_turning_radius = std::max( + p.freespace_parking_common_parameters.maximum_turning_radius, + p.freespace_parking_common_parameters.minimum_turning_radius); + p.freespace_parking_common_parameters.turning_radius_size = + std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); + } + + // freespace parking search config + { + std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + p.freespace_parking_common_parameters.theta_size = declare_parameter(ns + "theta_size"); + p.freespace_parking_common_parameters.angle_goal_range = + declare_parameter(ns + "angle_goal_range"); + p.freespace_parking_common_parameters.curve_weight = + declare_parameter(ns + "curve_weight"); + p.freespace_parking_common_parameters.reverse_weight = + declare_parameter(ns + "reverse_weight"); + p.freespace_parking_common_parameters.lateral_goal_range = + declare_parameter(ns + "lateral_goal_range"); + p.freespace_parking_common_parameters.longitudinal_goal_range = declare_parameter(ns + "longitudinal_goal_range"); + } - // costmap configs - p.common_parameters.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); + // freespace parking costmap configs + { + std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + p.freespace_parking_common_parameters.obstacle_threshold = + declare_parameter(ns + "obstacle_threshold"); } + // freespace parking astar { - std::string ns = "goal_planner.freespace_parking.astar."; + std::string ns = "goal_planner.pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = declare_parameter(ns + "use_back"); @@ -906,8 +926,9 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() declare_parameter(ns + "distance_heuristic_weight"); } + // freespace parking rrtstar { - std::string ns = "goal_planner.freespace_parking.rrtstar."; + std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = declare_parameter(ns + "use_informed_sampling"); @@ -916,6 +937,28 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() p.rrt_star_parameters.margin = declare_parameter(ns + "margin"); } + // debug + { + std::string ns = "goal_planner.debug."; + p.print_debug_info = declare_parameter(ns + "print_debug_info"); + } + + // validation of parameters + if (p.shift_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + get_logger(), "shift_sampling_num must be positive integer. Given parameter: " + << p.shift_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + if (p.maximum_deceleration < 0.0) { + RCLCPP_FATAL_STREAM( + get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + return p; } @@ -943,15 +986,19 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() // geometric pull out p.enable_geometric_pull_out = declare_parameter(ns + "enable_geometric_pull_out"); p.divide_pull_out_path = declare_parameter(ns + "divide_pull_out_path"); - p.geometric_pull_out_velocity = declare_parameter(ns + "geometric_pull_out_velocity"); - p.arc_path_interval = declare_parameter(ns + "arc_path_interval"); - p.lane_departure_margin = declare_parameter(ns + "lane_departure_margin"); - p.backward_velocity = declare_parameter(ns + "backward_velocity"); - p.pull_out_max_steer_angle = declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.pull_out_velocity = + declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_path_interval = + declare_parameter(ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + declare_parameter(ns + "lane_departure_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg // search start pose backward p.search_priority = declare_parameter( ns + "search_priority"); // "efficient_path" or "short_back_distance" p.enable_back = declare_parameter(ns + "enable_back"); + p.backward_velocity = declare_parameter(ns + "backward_velocity"); p.max_back_distance = declare_parameter(ns + "max_back_distance"); p.backward_search_resolution = declare_parameter(ns + "backward_search_resolution"); p.backward_path_update_duration = declare_parameter(ns + "backward_path_update_duration"); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 5db1381d50905..df2bef8dafb4c 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -85,14 +85,12 @@ GoalPlannerModule::GoalPlannerModule( if (parameters_->enable_arc_forward_parking) { constexpr bool is_forward = true; pull_over_planners_.push_back(std::make_shared( - node, *parameters, getGeometricGoalPlannerParameters(), lane_departure_checker, - occupancy_grid_map_, is_forward)); + node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); } if (parameters_->enable_arc_backward_parking) { constexpr bool is_forward = false; pull_over_planners_.push_back(std::make_shared( - node, *parameters, getGeometricGoalPlannerParameters(), lane_departure_checker, - occupancy_grid_map_, is_forward)); + node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); } } if (pull_over_planners_.empty()) { @@ -250,28 +248,6 @@ BehaviorModuleOutput GoalPlannerModule::run() return plan(); } -ParallelParkingParameters GoalPlannerModule::getGeometricGoalPlannerParameters() const -{ - ParallelParkingParameters params{}; - - params.th_arrived_distance = parameters_->th_arrived_distance; - params.th_stopped_velocity = parameters_->th_stopped_velocity; - params.after_forward_parking_straight_distance = - parameters_->after_forward_parking_straight_distance; - params.after_backward_parking_straight_distance = - parameters_->after_backward_parking_straight_distance; - params.forward_parking_velocity = parameters_->forward_parking_velocity; - params.backward_parking_velocity = parameters_->backward_parking_velocity; - params.forward_parking_lane_departure_margin = parameters_->forward_parking_lane_departure_margin; - params.backward_parking_lane_departure_margin = - parameters_->backward_parking_lane_departure_margin; - params.arc_path_interval = parameters_->arc_path_interval; - params.maximum_deceleration = parameters_->maximum_deceleration; - params.max_steer_angle = parameters_->pull_over_max_steer_angle; - - return params; -} - void GoalPlannerModule::processOnEntry() { const auto & route_handler = planner_data_->route_handler; @@ -300,7 +276,6 @@ void GoalPlannerModule::processOnEntry() // initialize when receiving new route if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status - parallel_parking_parameters_ = getGeometricGoalPlannerParameters(); resetStatus(); // calculate goal candidates diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index d9bb7ac7c6286..1ace845a18c5d 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -51,8 +51,7 @@ PullOutModule::PullOutModule( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - pull_out_planners_.push_back( - std::make_shared(node, *parameters, getGeometricPullOutParameters())); + pull_out_planners_.push_back(std::make_shared(node, *parameters)); } if (pull_out_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -76,8 +75,7 @@ PullOutModule::PullOutModule( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - pull_out_planners_.push_back( - std::make_shared(node, *parameters, getGeometricPullOutParameters())); + pull_out_planners_.push_back(std::make_shared(node, *parameters)); } if (pull_out_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -376,20 +374,6 @@ void PullOutModule::resetStatus() status_ = initial_status; } -ParallelParkingParameters PullOutModule::getGeometricPullOutParameters() const -{ - ParallelParkingParameters params{}; - - params.th_arrived_distance = parameters_->th_arrived_distance; - params.th_stopped_velocity = parameters_->th_stopped_velocity; - params.arc_path_interval = parameters_->arc_path_interval; - params.departing_velocity = parameters_->geometric_pull_out_velocity; - params.departing_lane_departure_margin = parameters_->lane_departure_margin; - params.max_steer_angle = parameters_->pull_out_max_steer_angle; - - return params; -} - void PullOutModule::incrementPathIndex() { status_.current_path_idx = diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 12dd17722257a..03e79ef5e57ff 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -123,9 +123,11 @@ std::vector GeometricParallelParking::generatePullOverPaths( const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin : parameters_.backward_parking_lane_departure_margin; + const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval + : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - lane_departure_margin); + lane_departure_margin, arc_path_interval); if (arc_paths.empty()) { return std::vector{}; } @@ -182,7 +184,7 @@ bool GeometricParallelParking::planPullOver( constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; - for (double steer = parameters_.max_steer_angle; steer > min_steer_rad; + for (double steer = parameters_.forward_parking_max_steer_angle; steer > min_steer_rad; steer -= steer_interval) { const double R_E_r = common_params.wheel_base / std::tan(steer); const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward); @@ -228,14 +230,14 @@ bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes) { - constexpr bool is_forward = false; // parking backward means departing forward + constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose constexpr double max_offset = 10.0; constexpr double offset_interval = 1.0; for (double end_pose_offset = 0; end_pose_offset < max_offset; end_pose_offset += offset_interval) { - // departing end pose which is the second arc path end + // pull_out end pose which is the second arc path end const auto end_pose = calcStartPose(start_pose, end_pose_offset, R_E_min_, is_forward); if (!end_pose) { continue; @@ -244,7 +246,7 @@ bool GeometricParallelParking::planPullOut( // plan reverse path of parking. end_pose <-> start_pose auto arc_paths = planOneTrial( *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, start_pose_offset, - parameters_.departing_lane_departure_margin); + parameters_.pull_out_lane_departure_margin, parameters_.pull_out_path_interval); if (arc_paths.empty()) { // not found path continue; @@ -265,7 +267,7 @@ bool GeometricParallelParking::planPullOut( } } - // get road center line path from departing end to goal, and combine after the second arc path + // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; const double road_lanes_length = std::accumulate( @@ -287,9 +289,9 @@ bool GeometricParallelParking::planPullOut( continue; } - // set departing velocity to arc paths and 0 velocity to end point + // set pull_out velocity to arc paths and 0 velocity to end point constexpr bool set_stop_end = false; - setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end); + setVelocityToArcPaths(arc_paths, parameters_.pull_out_velocity, set_stop_end); arc_paths.back().points.front().point.longitudinal_velocity_mps = 0.0; // combine the road center line path with the second arc path @@ -358,7 +360,8 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start std::vector GeometricParallelParking::planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_r, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double lane_departure_margin) + const bool is_forward, const double end_pose_offset, const double lane_departure_margin, + const double arc_path_interval) { clearPaths(); @@ -426,12 +429,14 @@ std::vector GeometricParallelParking::planOneTrial( (2 * R_E_l * (R_E_l + R_E_r))); theta_l = is_forward ? theta_l : -theta_l; - PathWithLaneId path_turn_left = - generateArcPath(Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), is_forward, is_forward); + PathWithLaneId path_turn_left = generateArcPath( + Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, + is_forward); path_turn_left.header = planner_data_->route_handler->getRouteHeader(); PathWithLaneId path_turn_right = generateArcPath( - Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); + Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, + is_forward); path_turn_right.header = planner_data_->route_handler->getRouteHeader(); auto setLaneIds = [lanes](PathPointWithLaneId & p) { @@ -473,11 +478,12 @@ std::vector GeometricParallelParking::planOneTrial( PathWithLaneId GeometricParallelParking::generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, + const double arc_path_interval, const bool is_left_turn, // is_left_turn means clockwise around center. const bool is_forward) { PathWithLaneId path; - const double yaw_interval = parameters_.arc_path_interval / radius; + const double yaw_interval = arc_path_interval / radius; double yaw = start_yaw; if (is_left_turn) { if (end_yaw < start_yaw) end_yaw += M_PI_2; @@ -530,18 +536,13 @@ PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( return p; } -void GeometricParallelParking::setData( - const std::shared_ptr & planner_data, - const ParallelParkingParameters & parameters) +void GeometricParallelParking::setTurningRadius( + const BehaviorPathPlannerParameters & common_params, const double max_steer_angle) { - planner_data_ = planner_data; - parameters_ = parameters; - - auto common_params = planner_data_->parameters; - - R_E_min_ = common_params.wheel_base / std::tan(parameters_.max_steer_angle); + R_E_min_ = common_params.wheel_base / std::tan(max_steer_angle); R_Bl_min_ = std::hypot( R_E_min_ + common_params.wheel_tread / 2 + common_params.left_over_hang, common_params.wheel_base + common_params.front_overhang); } + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 6b94fdcf0ea22..a3e710b853503 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -29,14 +29,15 @@ FreespacePullOver::FreespacePullOver( { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); - if (parameters.algorithm == "astar") { + if (parameters.freespace_parking_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( - parameters.common_parameters, vehicle_shape, parameters.astar_parameters); - } else if (parameters.algorithm == "rrtstar") { + parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_parking_algorithm == "rrtstar") { use_back_ = true; // no option for disabling back in rrtstar planner_ = std::make_unique( - parameters.common_parameters, vehicle_shape, parameters.rrt_star_parameters); + parameters.freespace_parking_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 2943a33607bba..ed38d09f879ba 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -27,16 +27,16 @@ namespace behavior_path_planner { GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr occupancy_grid_map, const bool is_forward) : PullOverPlannerBase{node, parameters}, - parallel_parking_parameters_{parallel_parking_parameters}, + parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{lane_departure_checker}, occupancy_grid_map_{occupancy_grid_map}, is_forward_{is_forward} { + planner_.setParameters(parallel_parking_parameters_); } boost::optional GeometricPullOver::plan(const Pose & goal_pose) @@ -53,8 +53,12 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) auto lanes = road_lanes; lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - // todo: set param only once - planner_.setData(planner_data_, parallel_parking_parameters_); + const auto & p = parallel_parking_parameters_; + const double max_steer_angle = + is_forward_ ? p.forward_parking_max_steer_angle : p.backward_parking_max_steer_angle; + planner_.setTurningRadius(planner_data_->parameters, max_steer_angle); + planner_.setPlannerData(planner_data_); + const bool found_valid_path = planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); if (!found_valid_path) { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 562dd93aa2306..073e97e01912e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -40,8 +40,8 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto & route_handler = planner_data_->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; const double max_jerk = parameters_.maximum_lateral_jerk; - const int pull_over_sampling_num = parameters_.pull_over_sampling_num; - const double jerk_resolution = std::abs(max_jerk - min_jerk) / pull_over_sampling_num; + const int shift_sampling_num = parameters_.shift_sampling_num; + const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; // get road and shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_); @@ -99,10 +99,10 @@ boost::optional ShiftPullOver::generatePullOverPath( const Pose & goal_pose, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; - const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; + const double after_shift_straight_distance = parameters_.after_shift_straight_distance; const Pose shift_end_pose = - tier4_autoware_utils::calcOffsetPose(goal_pose, -after_pull_over_straight_distance, 0, 0); + tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); // generate road lane reference path to shift end const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( diff --git a/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp index 4dbe7bc2d890f..21111f5b06ee6 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp @@ -28,11 +28,11 @@ namespace behavior_path_planner using pull_out_utils::combineReferencePath; using pull_out_utils::getPullOutLanes; -GeometricPullOut::GeometricPullOut( - rclcpp::Node & node, const PullOutParameters & parameters, - const ParallelParkingParameters & parallel_parking_parameters) -: PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parallel_parking_parameters} +GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const PullOutParameters & parameters) +: PullOutPlannerBase{node, parameters}, + parallel_parking_parameters_{parameters.parallel_parking_parameters} { + planner_.setParameters(parallel_parking_parameters_); } boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) @@ -45,8 +45,9 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p auto lanes = road_lanes; lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - // todo: set params only once? - planner_.setData(planner_data_, parallel_parking_parameters_); + planner_.setTurningRadius( + 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, shoulder_lanes); if (!found_valid_path) { @@ -68,7 +69,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p auto partial_paths = planner_.getPaths(); // remove stop velocity of first arc path partial_paths.front().points.back().point.longitudinal_velocity_mps = - parameters_.geometric_pull_out_velocity; + parallel_parking_parameters_.pull_out_velocity; const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); }