Merge pull request autowarefoundation#496 from tier4/hotfix/lc-dynami…

…c-lat-acc

feat(lane_change): add dynamic lateral acceleration for lane change

common/motion_utils/src/resample/resample.cpp

@@ -188,21 +188,21 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
     }
 
     const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i);
-    for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+    for (size_t j = 1; j < resampling_arclength.size(); ++j) {
       if (
-        resampling_arclength.at(i - 1) <= distance_to_resampling_point &&
-        distance_to_resampling_point < resampling_arclength.at(i)) {
+        resampling_arclength.at(j - 1) <= distance_to_resampling_point &&
+        distance_to_resampling_point < resampling_arclength.at(j)) {
         const double dist_to_prev_point =
-          std::fabs(distance_to_resampling_point - resampling_arclength.at(i - 1));
+          std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1));
         const double dist_to_following_point =
-          std::fabs(resampling_arclength.at(i) - distance_to_resampling_point);
+          std::fabs(resampling_arclength.at(j) - distance_to_resampling_point);
         if (dist_to_prev_point < motion_utils::overlap_threshold) {
-          resampling_arclength.at(i - 1) = distance_to_resampling_point;
+          resampling_arclength.at(j - 1) = distance_to_resampling_point;
         } else if (dist_to_following_point < motion_utils::overlap_threshold) {
-          resampling_arclength.at(i) = distance_to_resampling_point;
+          resampling_arclength.at(j) = distance_to_resampling_point;
         } else {
           resampling_arclength.insert(
-            resampling_arclength.begin() + i, distance_to_resampling_point);
+            resampling_arclength.begin() + j, distance_to_resampling_point);
         }
         break;
       }

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -8,13 +8,18 @@
       lane_change_finish_judge_buffer: 2.0      # [m]
 
       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]
-      lane_change_sampling_num: 3
+      longitudinal_acceleration_sampling_num: 3
+      lateral_acceleration_sampling_num: 3
+
+      # lateral acceleration map
+      lateral_acceleration:
+        velocity: [0.0, 4.0, 10.0]
+        min_values: [0.15, 0.15, 0.15]
+        max_values: [0.5, 0.5, 0.5]
 
       # target object
       target_object:

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -417,7 +417,6 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `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_changing_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            |

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -15,9 +15,11 @@
 #ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
 #define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
 
+#include <interpolation/linear_interpolation.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <string>
+#include <utility>
 #include <vector>
 
 struct ModuleConfigParameters
@@ -29,6 +31,47 @@ struct ModuleConfigParameters
   uint8_t max_module_size{0};
 };
 
+struct LateralAccelerationMap
+{
+  std::vector<double> base_vel;
+  std::vector<double> base_min_acc;
+  std::vector<double> base_max_acc;
+
+  void add(const double velocity, const double min_acc, const double max_acc)
+  {
+    if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) {
+      return;
+    }
+
+    size_t idx = 0;
+    for (size_t i = 0; i < base_vel.size(); ++i) {
+      if (velocity < base_vel.at(i)) {
+        break;
+      }
+      idx = i + 1;
+    }
+
+    base_vel.insert(base_vel.begin() + idx, velocity);
+    base_min_acc.insert(base_min_acc.begin() + idx, min_acc);
+    base_max_acc.insert(base_max_acc.begin() + idx, max_acc);
+  }
+
+  std::pair<double, double> find(const double velocity) const
+  {
+    if (!base_vel.empty() && velocity < base_vel.front()) {
+      return std::make_pair(base_min_acc.front(), base_max_acc.front());
+    }
+    if (!base_vel.empty() && velocity > base_vel.back()) {
+      return std::make_pair(base_min_acc.back(), base_max_acc.back());
+    }
+
+    const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity);
+    const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity);
+
+    return std::make_pair(min_acc, max_acc);
+  }
+};
+
 struct BehaviorPathPlannerParameters
 {
   bool verbose;
@@ -56,12 +99,11 @@ struct BehaviorPathPlannerParameters
 
   // lane change parameters
   double lane_changing_lateral_jerk{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 minimum_lane_changing_velocity{5.6};
   double lane_change_prepare_duration{4.0};
   double minimum_prepare_length;
+  LateralAccelerationMap lane_change_lat_acc_map;
 
   double minimum_pull_over_length;
   double minimum_pull_out_length;

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -21,6 +21,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -31,7 +32,8 @@ struct LaneChangeParameters
   double backward_lane_length{200.0};
   double lane_change_finish_judge_buffer{3.0};
   double prediction_time_resolution{0.5};
-  int lane_change_sampling_num{10};
+  int longitudinal_acc_sampling_num{10};
+  int lateral_acc_sampling_num{10};
 
   // collision check
   bool enable_prepare_segment_collision_check{true};

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -68,15 +68,16 @@ bool isPathInLanelets(
   const lanelet::ConstLanelets & target_lanelets);
 
 double calcLaneChangingLength(
-  const double lane_changing_velocity, const double shift_length,
-  const BehaviorPathPlannerParameters & common_parameter);
+  const double lane_changing_velocity, const double shift_length, const double lateral_acc,
+  const double lateral_jerk);
 
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const std::vector<std::vector<int64_t>> & sorted_lane_ids, const double acceleration,
-  const LaneChangePhaseInfo lane_change_length, const LaneChangePhaseInfo lane_change_velocity,
+  const std::vector<std::vector<int64_t>> & sorted_lane_ids, const double longitudinal_acceleration,
+  const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length,
+  const LaneChangePhaseInfo lane_change_velocity,
   const BehaviorPathPlannerParameters & common_parameter,
   const LaneChangeParameters & lane_change_param);
 
@@ -110,16 +111,6 @@ PathWithLaneId getReferencePathFromTargetLane(
   const double resample_interval, const bool is_goal_in_route,
   const double next_lane_change_buffer);
 
-PathWithLaneId getPrepareSegment(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
-  const double arc_length_from_current, const double backward_path_length,
-  const double prepare_length, const double prepare_velocity);
-
-PathWithLaneId getPrepareSegment(
-  const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets,
-  const Pose & current_pose, const double backward_path_length, const double prepare_length,
-  const double prepare_velocity);
-
 PathWithLaneId getTargetSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
   const double forward_path_length, const Pose & lane_changing_start_pose,

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -376,10 +376,6 @@ boost::optional<std::pair<Pose, Polygon2d>> getEgoExpectedPoseAndConvertToPolygo
 
 bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
 
-double calcLaneChangingTime(
-  const double lane_changing_velocity, const double shift_length,
-  const BehaviorPathPlannerParameters & common_parameter);
-
 double calcMinimumLaneChangeLength(
   const BehaviorPathPlannerParameters & common_param, const std::vector<double> & shift_intervals,
   const double length_to_intersection = 0.0);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -397,9 +397,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
     declare_parameter<double>("lane_change.backward_length_buffer_for_end_of_lane");
   p.lane_changing_lateral_jerk =
     declare_parameter<double>("lane_change.lane_changing_lateral_jerk");
-  p.lane_changing_lateral_acc = declare_parameter<double>("lane_change.lane_changing_lateral_acc");
-  p.lane_changing_lateral_acc_at_low_velocity =
-    declare_parameter<double>("lane_change.lane_changing_lateral_acc_at_low_velocity");
   p.lateral_acc_switching_velocity =
     declare_parameter<double>("lane_change.lateral_acc_switching_velocity");
   p.lane_change_prepare_duration = declare_parameter<double>("lane_change.prepare_duration");
@@ -410,6 +407,24 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.minimum_prepare_length =
     0.5 * p.max_acc * p.lane_change_prepare_duration * p.lane_change_prepare_duration;
 
+  // lateral acceleration map for lane change
+  const auto lateral_acc_velocity =
+    declare_parameter<std::vector<double>>("lane_change.lateral_acceleration.velocity");
+  const auto min_lateral_acc =
+    declare_parameter<std::vector<double>>("lane_change.lateral_acceleration.min_values");
+  const auto max_lateral_acc =
+    declare_parameter<std::vector<double>>("lane_change.lateral_acceleration.max_values");
+  if (
+    lateral_acc_velocity.size() != min_lateral_acc.size() ||
+    lateral_acc_velocity.size() != max_lateral_acc.size()) {
+    RCLCPP_ERROR(get_logger(), "Lane change lateral acceleration map has invalid size.");
+    exit(EXIT_FAILURE);
+  }
+  for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) {
+    p.lane_change_lat_acc_map.add(
+      lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i));
+  }
+
   p.backward_length_buffer_for_end_of_pull_over =
     declare_parameter<double>("backward_length_buffer_for_end_of_pull_over");
   p.backward_length_buffer_for_end_of_pull_out =
@@ -712,7 +727,10 @@ 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.lane_change_sampling_num = declare_parameter<int>(parameter("lane_change_sampling_num"));
+  p.longitudinal_acc_sampling_num =
+    declare_parameter<int>(parameter("longitudinal_acceleration_sampling_num"));
+  p.lateral_acc_sampling_num =
+    declare_parameter<int>(parameter("lateral_acceleration_sampling_num"));
 
   // collision check
   p.enable_prepare_segment_collision_check =
@@ -748,11 +766,13 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.publish_debug_marker = declare_parameter<bool>(parameter("publish_debug_marker"));
 
   // validation of parameters
-  if (p.lane_change_sampling_num < 1) {
+  if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(
-      get_logger(), "lane_change_sampling_num must be positive integer. Given parameter: "
-                      << p.lane_change_sampling_num << std::endl
-                      << "Terminating the program...");
+      get_logger(),
+      "lane_change_sampling_num must be positive integer. Given longitudinal parameter: "
+        << p.longitudinal_acc_sampling_num
+        << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl
+        << "Terminating the program...");
     exit(EXIT_FAILURE);
   }