update params

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Aug 8, 2022 · 206452f · 206452f
1 parent 079ed1e
commit 206452f
Show file tree

Hide file tree

Showing 7 changed files with 23 additions and 6 deletions.
diff --git a/...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/...nning/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
@@ -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/...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/...or_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/...h_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/...h_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
@@ -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
@@ -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
@@ -146,7 +146,9 @@ std::vector<PathWithLaneId> GeometricParallelParking::generateParkingPaths(
   if (!isEnoughDistanceToStart(start_pose)) {
     return std::vector<PathWithLaneId>{};
   }
-  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;