Add params for parking velocity

autowarefoundation · May 10, 2022 · dea3fb6 · dea3fb6
1 parent 514594b
commit dea3fb6
Show file tree

Hide file tree

Showing 7 changed files with 25 additions and 5 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
@@ -29,6 +29,9 @@
       # parallel parking path
       after_forward_parking_straight_distance: 0.5
       after_backward_parking_straight_distance: 0.5
+      forward_parking_velocity: 1.0
+      backward_parking_velocity: -0.5
+      arc_path_interval: 1.0
       # hazard. Not used now.
       double hazard_on_threshold_dis: 1.0
       double hazard_on_threshold_vel: 0.5

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
@@ -29,6 +29,9 @@
       # parallel parking path
       after_forward_parking_straight_distance: 0.5
       after_backward_parking_straight_distance: 0.5
+      forward_parking_velocity: 1.0
+      backward_parking_velocity: -0.5
+      arc_path_interval: 1.0
       # hazard. Not used now.
       double hazard_on_threshold_dis: 1.0
       double hazard_on_threshold_vel: 0.5

diff --git a/...anner/include/behavior_path_planner/parallel_parking_planner/parallel_parking_planner.hpp b/...anner/include/behavior_path_planner/parallel_parking_planner/parallel_parking_planner.hpp
@@ -52,6 +52,9 @@ struct ParallelParkingParameters
   double after_forward_parking_straight_distance;
   double after_backward_parking_straight_distance;
   double decide_path_distance;
+  double forward_parking_velocity;
+  double backward_parking_velocity;
+  double arc_path_interval;
 };
 
 class ParallelParkingPlanner

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
@@ -76,6 +76,9 @@ struct PullOverParameters
   // parallel parking
   double after_forward_parking_straight_distance;
   double after_backward_parking_straight_distance;
+  double forward_parking_velocity;
+  double backward_parking_velocity;
+  double arc_path_interval;
   // hazard. Not used now.
   double hazard_on_threshold_dis;
   double hazard_on_threshold_vel;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -402,6 +402,9 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam()
   // parallel parking
   p.after_forward_parking_straight_distance = dp("after_forward_parking_straight_distance", 0.5);
   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.arc_path_interval = dp("arc_path_interval", 1.0);
   // hazard
   p.hazard_on_threshold_dis = dp("hazard_on_threshold_dis", 1.0);
   p.hazard_on_threshold_vel = dp("hazard_on_threshold_vel", 0.5);

diff --git a/planning/behavior_path_planner/src/parallel_parking_planner/parallel_parking_planner.cpp b/planning/behavior_path_planner/src/parallel_parking_planner/parallel_parking_planner.cpp
@@ -282,9 +282,9 @@ PathWithLaneId ParallelParkingPlanner::generateArcPath(
 {
   PathWithLaneId path;
 
-  const float velocity = is_forward ? 1.0 : -0.5;
-  const float interval = 1.0;
-  const float yaw_interval = interval / radius;
+  const float velocity =
+    is_forward ? parameters_.forward_parking_velocity : parameters_.backward_parking_velocity;
+  const float yaw_interval = parameters_.arc_path_interval / radius;
   float yaw = start_yaw;
   if (is_left_turn) {
     if (end_yaw < start_yaw) end_yaw += M_PI_2;

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
@@ -101,9 +101,14 @@ void PullOverModule::onEntry()
   // Initialize sratus
   parallel_parking_planner_.clear();
   parallel_parking_prameters_ = ParallelParkingParameters{
-    parameters_.th_arrived_distance_m, parameters_.th_stopped_velocity_mps,
+    parameters_.th_arrived_distance_m,
+    parameters_.th_stopped_velocity_mps,
     parameters_.after_forward_parking_straight_distance,
-    parameters_.after_backward_parking_straight_distance, parameters_.decide_path_distance};
+    parameters_.after_backward_parking_straight_distance,
+    parameters_.decide_path_distance,
+    parameters_.forward_parking_velocity,
+    parameters_.backward_parking_velocity,
+    parameters_.arc_path_interval};
   status_.has_decided_path = false;
   status_.path_type = PathType::NONE;
   status_.is_safe = false;