From 206452f3568d945701b7fed30391e4efc5373126 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 8 Aug 2022 15:00:23 +0900 Subject: [PATCH] update params Signed-off-by: kosuke55 --- .../behavior_path_planner/pull_over/pull_over.param.yaml | 5 ++++- .../config/pull_over/pull_over.param.yaml | 7 +++++-- .../scene_module/pull_over/pull_over_module.hpp | 2 ++ .../scene_module/utils/geometric_parallel_parking.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 2 ++ .../src/scene_module/pull_over/pull_over_module.cpp | 3 +++ .../src/scene_module/utils/geometric_parallel_parking.cpp | 6 ++++-- 7 files changed, 23 insertions(+), 6 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 049ad2785f3a6..7d2927fcc6397 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -37,8 +37,11 @@ after_backward_parking_straight_distance: 2.0 forward_parking_velocity: 0.3 backward_parking_velocity: -0.3 + forward_parking_lane_departure_margin: 0.2 + backward_parking_lane_departure_margin: 0.2 arc_path_interval: 1.0 - # hazard_on when parked + max_steer_rad: 0.35 # 20deg + # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 # check safety with dynamic objects. Not used now. diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 1575fcb7a383a..7d2927fcc6397 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -21,7 +21,7 @@ goal_search_interval: 1.0 goal_to_obj_margin: 2.0 # occupancy grid map - collision_check_margin: 0.0 + collision_check_margin: 0.5 theta_size: 360 obstacle_threshold: 60 # shift path @@ -37,8 +37,11 @@ after_backward_parking_straight_distance: 2.0 forward_parking_velocity: 0.3 backward_parking_velocity: -0.3 + forward_parking_lane_departure_margin: 0.2 + backward_parking_lane_departure_margin: 0.2 arc_path_interval: 1.0 - # hazard. Not used now. + max_steer_rad: 0.35 # 20deg + # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 # check safety with dynamic objects. Not used now. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 00030910cfbb9..c5c5442ed3777 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -86,6 +86,8 @@ struct PullOverParameters 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 max_steer_rad; // hazard. Not used now. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index 803977a9d89d7..1dda5981fa67a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -55,7 +55,9 @@ struct ParallelParkingParameters double forward_parking_velocity; double backward_parking_velocity; double departing_velocity; - double lane_departure_margin; + double backward_parking_lane_departure_margin; + double forward_parking_lane_departure_margin; + double departing_lane_departure_margin; double arc_path_interval; double max_deceleration; double max_steer_rad; 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 0fcee2ed649f4..db382fa9852a1 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -401,6 +401,8 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.after_backward_parking_straight_distance = dp("after_backward_parking_straight_distance", 0.5); p.forward_parking_velocity = dp("forward_parking_velocity", 1.0); p.backward_parking_velocity = dp("backward_parking_velocity", -0.5); + p.forward_parking_lane_departure_margin = dp("forward_parking_lane_departure_margin", 0.2); + p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.2); p.arc_path_interval = dp("arc_path_interval", 1.0); // hazard p.hazard_on_threshold_dis = dp("hazard_on_threshold_dis", 1.0); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index b14d1f26b8034..d7d9a28a39d0c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -104,6 +104,9 @@ ParallelParkingParameters PullOverModule::getGeometricPullOutParameters() const 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.max_deceleration = parameters_.max_deceleration; params.max_steer_rad = parameters_.max_steer_rad; diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 1d3fe7066836c..374ed8555a006 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -146,7 +146,9 @@ std::vector GeometricParallelParking::generateParkingPaths( if (!isEnoughDistanceToStart(start_pose)) { return std::vector{}; } - const double lane_departure_margin = 0.0; // todo make it param + const double lane_departure_margin = is_forward + ? parameters_.forward_parking_lane_departure_margin + : parameters_.backward_parking_lane_departure_margin; auto arc_paths = planOneTrial( start_pose, goal_pose, R_E_r, lanes, is_forward, end_pose_offset, lane_departure_margin); if (arc_paths.empty()) { @@ -263,7 +265,7 @@ bool GeometricParallelParking::planDeparting( // plan reverse path of parking. end_pose <-> start_pose auto arc_paths = planOneTrial( *end_pose, start_pose, R_E_min_, lanes, is_forward, start_pose_offset, - parameters_.lane_departure_margin); + parameters_.departing_lane_departure_margin); if (arc_paths.empty()) { // not found path continue;