feat(behavior_path_planner): add new lateral acc condition

Signed-off-by: yutaka <[email protected]>
autowarefoundation · Apr 6, 2023 · bf43e91 · bf43e91
1 parent ca9cac3
commit bf43e91
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 4 deletions.
diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
@@ -8,8 +8,10 @@
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       lane_change_finish_judge_buffer: 2.0      # [m]
 
-      lane_changing_lateral_jerk: 0.5           # [m/s3]
-      lane_changing_lateral_acc: 0.5            # [m/s2]
+      lane_changing_lateral_jerk: 0.5              # [m/s3]
+      lane_changing_lateral_acc: 0.315             # [m/s2]
+      lane_changing_lateral_acc_at_low_velocity: 0.15 # [m/s2]
+      lateral_acc_switching_velocity: 4.0          #[m/s]
 
       minimum_lane_changing_velocity: 2.78        # [m/s]
       prediction_time_resolution: 0.5           # [s]

diff --git a/...r_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp b/...r_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp
@@ -28,7 +28,9 @@ struct LaneChangeParameters
   // trajectory generation
   double prepare_duration{2.0};
   double lane_changing_lateral_jerk{0.5};
-  double lane_changing_lateral_acc{0.5};
+  double lane_changing_lateral_acc{0.315};
+  double lane_changing_lateral_acc_at_low_velocity{0.15};
+  double lateral_acc_switching_velocity{0.4};
   double lane_change_finish_judge_buffer{3.0};
   double minimum_lane_changing_velocity{5.6};
   double prediction_time_resolution{0.5};

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -718,6 +718,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.prepare_duration = declare_parameter<double>(parameter("prepare_duration"));
   p.lane_changing_lateral_jerk = declare_parameter<double>(parameter("lane_changing_lateral_jerk"));
   p.lane_changing_lateral_acc = declare_parameter<double>(parameter("lane_changing_lateral_acc"));
+  p.lane_changing_lateral_acc_at_low_velocity =
+    declare_parameter<double>(parameter("lane_changing_lateral_acc_at_low_velocity"));
+  p.lateral_acc_switching_velocity =
+    declare_parameter<double>(parameter("lateral_acc_switching_velocity"));
   p.lane_change_finish_judge_buffer =
     declare_parameter<double>(parameter("lane_change_finish_judge_buffer"));
   p.minimum_lane_changing_velocity =

diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp
@@ -741,8 +741,11 @@ double calcLaneChangingDistance(
   const double lane_changing_speed, const double shift_length,
   const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param)
 {
+  const double lateral_acc = lane_changing_speed < lc_param.lateral_acc_switching_velocity
+                               ? lc_param.lane_changing_lateral_acc_at_low_velocity
+                               : lc_param.lane_changing_lateral_acc;
   const auto required_time = PathShifter::calcShiftTimeFromJerk(
-    shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc);
+    shift_length, lc_param.lane_changing_lateral_jerk, lateral_acc);
 
   const double & min_lane_change_length = com_param.minimum_lane_changing_length;
   const double lane_changing_distance =