Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(behavior_path_planner): refactoring goal_planner and pull_out params #3610

Merged
merged 1 commit into from
May 4, 2023
Merged

refactor(behavior_path_planner): refactoring goal_planner and pull_out params #3610

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
182 changes: 97 additions & 85 deletions planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
16 changes: 8 additions & 8 deletions planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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**

Expand Down
1 change: 0 additions & 1 deletion ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 0 additions & 1 deletion ...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,6 @@ class PullOutModule : public SceneModuleInterface

std::shared_ptr<PullOutPlannerBase> getCurrentPlanner() const;
PathWithLaneId getFullPath() const;
ParallelParkingParameters getGeometricPullOutParameters() const;
std::vector<Pose> searchPullOutStartPoses();

std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
Expand Down
40 changes: 28 additions & 12 deletions ...ude/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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<const PlannerData> & planner_data,
const ParallelParkingParameters & parameters);
void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
{
planner_data_ = planner_data;
}
void setTurningRadius(
const BehaviorPathPlannerParameters & common_params, const double max_steer_angle);

void incrementPathIndex();

std::vector<PathWithLaneId> getArcPaths() const { return arc_paths_; }
Expand Down Expand Up @@ -103,10 +118,11 @@ class GeometricParallelParking
std::vector<PathWithLaneId> 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);
Expand Down
1 change: 0 additions & 1 deletion ...ior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const bool is_forward);
Expand Down
Loading