refactor(behavior_path_planeer): use common.params for lane change (a…

…utowarefoundation#3520) * refactor(behavior_path_planeer): use common.params for lane change Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> --------- Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>
Mingyu1991 · Jun 26, 2023 · b10b341 · b10b341
1 parent b4c83df
commit b10b341
Show file tree

Hide file tree

Showing 9 changed files with 36 additions and 36 deletions.
diff --git a/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -35,6 +35,9 @@ def launch_setup(context, *args, **kwargs):
     with open(vehicle_param_path, "r") as f:
         vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    # common parameter
+    with open(LaunchConfiguration("common_param_path").perform(context), "r") as f:
+        common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     # nearest search parameter
     with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f:
         nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"]
@@ -82,6 +85,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"),
         ],
         parameters=[
+            common_param,
             nearest_search_param,
             side_shift_param,
             avoidance_param,
@@ -113,8 +117,6 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # smoother param
-    with open(LaunchConfiguration("common_param_path").perform(context), "r") as f:
-        common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(
         LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r"
     ) as f:

diff --git a/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -14,6 +14,7 @@
     <remap from="~/output/turn_indicators_cmd" to="/planning/turn_signal_cmd"/>
     <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_signal_cmd"/>
     <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
+    <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/common/common.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml"/>

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
@@ -15,7 +15,6 @@
 
       minimum_lane_changing_velocity: 2.78        # [m/s]
       prediction_time_resolution: 0.5           # [s]
-      maximum_deceleration: 1.0                 # [m/s2]
       lane_change_sampling_num: 3
 
       # target object

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md
@@ -407,20 +407,19 @@ The last behavior will also occur if the ego vehicle has departed from the curre
 
 The following parameters are configurable in `lane_change.param.yaml`.
 
-| Name                                     | Unit    | Type   | Description                                                                             | Default value |
-| :--------------------------------------- | ------- | ------ | --------------------------------------------------------------------------------------- | ------------- |
-| `lane_change_prepare_duration`           | [m]     | double | The preparation time for the ego vehicle to be ready to perform lane change.            | 4.0           |
-| `lane_changing_safety_check_duration`    | [m]     | double | The total time that is taken to complete the lane-changing task.                        | 8.0           |
-| `minimum_lane_change_prepare_distance`   | [m]     | double | Minimum prepare distance for lane change                                                | 2.0           |
-| `minimum_lane_change_length`             | [m]     | double | The minimum distance needed for changing lanes.                                         | 16.5          |
-| `backward_length_buffer_for_end_of_lane` | [m]     | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change   | 2.0           |
-| `lane_change_finish_judge_buffer`        | [m]     | double | The additional buffer used to confirm lane change process completion                    | 3.0           |
-| `lane_changing_lateral_jerk`             | [m/s3]  | double | Lateral jerk value for lane change path generation                                      | 0.5           |
-| `lane_changing_lateral_acc`              | [m/s2]  | double | Lateral acceleration value for lane change path generation                              | 0.5           |
-| `minimum_lane_change_velocity`           | [m/s]   | double | Minimum speed during lane changing process.                                             | 2.78          |
-| `prediction_time_resolution`             | [s]     | double | Time resolution for object's path interpolation and collision check.                    | 0.5           |
-| `maximum_deceleration`                   | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change.                           | 1.0           |
-| `lane_change_sampling_num`               | [-]     | int    | Number of possible lane-changing trajectories that are being influenced by deceleration | 10            |
+| Name                                     | Unit   | Type   | Description                                                                             | Default value |
+| :--------------------------------------- | ------ | ------ | --------------------------------------------------------------------------------------- | ------------- |
+| `lane_change_prepare_duration`           | [m]    | double | The preparation time for the ego vehicle to be ready to perform lane change.            | 4.0           |
+| `lane_changing_safety_check_duration`    | [m]    | double | The total time that is taken to complete the lane-changing task.                        | 8.0           |
+| `minimum_lane_change_prepare_distance`   | [m]    | double | Minimum prepare distance for lane change                                                | 2.0           |
+| `minimum_lane_change_length`             | [m]    | double | The minimum distance needed for changing lanes.                                         | 16.5          |
+| `backward_length_buffer_for_end_of_lane` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change   | 2.0           |
+| `lane_change_finish_judge_buffer`        | [m]    | double | The additional buffer used to confirm lane change process completion                    | 3.0           |
+| `lane_changing_lateral_jerk`             | [m/s3] | double | Lateral jerk value for lane change path generation                                      | 0.5           |
+| `lane_changing_lateral_acc`              | [m/s2] | double | Lateral acceleration value for lane change path generation                              | 0.5           |
+| `minimum_lane_change_velocity`           | [m/s]  | double | Minimum speed during lane changing process.                                             | 2.78          |
+| `prediction_time_resolution`             | [s]    | double | Time resolution for object's path interpolation and collision check.                    | 0.5           |
+| `lane_change_sampling_num`               | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by deceleration | 10            |
 
 ### Collision checks during lane change
 

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
@@ -49,6 +49,10 @@ struct BehaviorPathPlannerParameters
   double backward_length_buffer_for_end_of_pull_over;
   double backward_length_buffer_for_end_of_pull_out;
 
+  // common parameters
+  double min_acc;
+  double max_acc;
+
   // lane change parameters
   double lane_changing_lateral_jerk{0.5};
   double lane_changing_lateral_acc{0.315};

diff --git a/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
@@ -29,7 +29,6 @@ struct LaneChangeParameters
   double prepare_duration{2.0};
   double lane_change_finish_judge_buffer{3.0};
   double prediction_time_resolution{0.5};
-  double maximum_deceleration{1.0};
   int lane_change_sampling_num{10};
 
   // collision check

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -377,6 +377,10 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.backward_path_length = declare_parameter<double>("backward_path_length") + backward_offset;
   p.forward_path_length = declare_parameter<double>("forward_path_length");
 
+  // acceleration parameters
+  p.min_acc = declare_parameter<double>("normal.min_acc");
+  p.max_acc = declare_parameter<double>("normal.max_acc");
+
   // lane change parameters
   p.backward_length_buffer_for_end_of_lane =
     declare_parameter<double>("lane_change.backward_length_buffer_for_end_of_lane");
@@ -652,7 +656,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.lane_change_finish_judge_buffer =
     declare_parameter<double>(parameter("lane_change_finish_judge_buffer"));
   p.prediction_time_resolution = declare_parameter<double>(parameter("prediction_time_resolution"));
-  p.maximum_deceleration = declare_parameter<double>(parameter("maximum_deceleration"));
   p.lane_change_sampling_num = declare_parameter<int>(parameter("lane_change_sampling_num"));
 
   // collision check
@@ -695,13 +698,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
                       << "Terminating the program...");
     exit(EXIT_FAILURE);
   }
-  if (p.maximum_deceleration < 0.0) {
-    RCLCPP_FATAL_STREAM(
-      get_logger(), "maximum_deceleration cannot be negative value. Given parameter: "
-                      << p.maximum_deceleration << std::endl
-                      << "Terminating the program...");
-    exit(EXIT_FAILURE);
-  }
 
   if (p.abort_delta_time < 1.0) {
     RCLCPP_FATAL_STREAM(

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -310,13 +310,13 @@ bool NormalLaneChange::getLaneChangePaths(
   const auto current_velocity = getEgoTwist().linear.x;
 
   // compute maximum_deceleration
-  const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, &current_velocity,
-                                                 this]() {
-    const double min_a =
-      (minimum_lane_changing_velocity - current_velocity) / parameters_->prepare_duration;
-    return std::clamp(
-      min_a, -std::abs(parameters_->maximum_deceleration), -std::numeric_limits<double>::epsilon());
-  });
+  const auto maximum_deceleration =
+    std::invoke([&minimum_lane_changing_velocity, &current_velocity, &common_parameter, this]() {
+      const double min_a =
+        (minimum_lane_changing_velocity - current_velocity) / parameters_->prepare_duration;
+      return std::clamp(
+        min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits<double>::epsilon());
+    });
 
   const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num;
 

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -230,12 +230,12 @@ bool getLaneChangePaths(
   const auto current_velocity = twist.linear.x;
 
   // compute maximum_deceleration
-  const auto maximum_deceleration =
-    std::invoke([&minimum_lane_changing_velocity, &current_velocity, &parameter]() {
+  const auto maximum_deceleration = std::invoke(
+    [&minimum_lane_changing_velocity, &current_velocity, &common_parameter, &parameter]() {
       const double min_a =
         (minimum_lane_changing_velocity - current_velocity) / parameter.prepare_duration;
       return std::clamp(
-        min_a, -std::abs(parameter.maximum_deceleration), -std::numeric_limits<double>::epsilon());
+        min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits<double>::epsilon());
     });
 
   const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num;