Merge branch 'main' into fix/correct_accumulated_delay_times_

planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp

@@ -62,8 +62,6 @@ std::shared_ptr<BehaviorPathPlannerNode> generateNode()
                    behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml",
                    behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml",
                    behavior_path_planner_dir + "/config/scene_module_manager.param.yaml",
-                   ament_index_cpp::get_package_share_directory("behavior_path_planner") +
-                     "/config/lane_change/lane_change.param.yaml",
                    ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") +
                      "/config/avoidance.param.yaml"});
 

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

@@ -17,12 +17,55 @@
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
+#include <interpolation/linear_interpolation.hpp>
+
 #include <lanelet2_core/primitives/Lanelet.h>
 
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
 {
+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 LaneChangeCancelParameters
 {
@@ -33,6 +76,7 @@ struct LaneChangeCancelParameters
   double max_lateral_jerk{10.0};
   double overhang_tolerance{0.0};
 };
+
 struct LaneChangeParameters
 {
   // trajectory generation
@@ -41,6 +85,15 @@ struct LaneChangeParameters
   int longitudinal_acc_sampling_num{10};
   int lateral_acc_sampling_num{10};
 
+  // lane change parameters
+  double backward_length_buffer_for_end_of_lane;
+  double backward_length_buffer_for_blocking_object;
+  double lane_changing_lateral_jerk{0.5};
+  double minimum_lane_changing_velocity{5.6};
+  double lane_change_prepare_duration{4.0};
+  double lane_change_finish_judge_buffer{3.0};
+  LateralAccelerationMap lane_change_lat_acc_map;
+
   // parked vehicle
   double object_check_min_road_shoulder_width{0.5};
   double object_shiftable_ratio_threshold{0.6};

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

@@ -54,17 +54,21 @@ using tier4_autoware_utils::Polygon2d;
 double calcLaneChangeResampleInterval(
   const double lane_changing_length, const double lane_changing_velocity);
 
+double calcMinimumLaneChangeLength(
+  const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals,
+  const double length_to_intersection = 0.0);
+
 double calcMaximumLaneChangeLength(
-  const double current_velocity, const BehaviorPathPlannerParameters & common_param,
+  const double current_velocity, const LaneChangeParameters & lane_change_parameters,
   const std::vector<double> & shift_intervals, const double max_acc);
 
 double calcMinimumAcceleration(
   const double current_velocity, const double min_longitudinal_acc,
-  const BehaviorPathPlannerParameters & params);
+  const LaneChangeParameters & lane_change_parameters);
 
 double calcMaximumAcceleration(
   const double current_velocity, const double current_max_velocity,
-  const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params);
+  const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters);
 
 double calcLaneChangingAcceleration(
   const double initial_lane_changing_velocity, const double max_path_velocity,
@@ -130,7 +134,7 @@ double getLateralShift(const LaneChangePath & path);
 bool hasEnoughLengthToLaneChangeAfterAbort(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const Pose & curent_pose, const double abort_return_dist,
-  const BehaviorPathPlannerParameters & common_param, const Direction direction);
+  const LaneChangeParameters & lane_change_parameters, const Direction direction);
 
 lanelet::ConstLanelets getBackwardLanelets(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
@@ -150,7 +154,8 @@ std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
 
 std::vector<PoseWithVelocityStamped> convertToPredictedPath(
   const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose,
-  const BehaviorPathPlannerParameters & common_parameters, const double resolution);
+  const BehaviorPathPlannerParameters & common_parameters,
+  const LaneChangeParameters & lane_change_parameters, const double resolution);
 
 bool isParkedObject(
   const PathWithLaneId & path, const RouteHandler & route_handler,

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -238,39 +238,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   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");
-  p.backward_length_buffer_for_blocking_object =
-    declare_parameter<double>("lane_change.backward_length_buffer_for_blocking_object");
-  p.lane_changing_lateral_jerk =
-    declare_parameter<double>("lane_change.lane_changing_lateral_jerk");
-  p.lane_change_prepare_duration = declare_parameter<double>("lane_change.prepare_duration");
-  p.minimum_lane_changing_velocity =
-    declare_parameter<double>("lane_change.minimum_lane_changing_velocity");
-  p.minimum_lane_changing_velocity =
-    std::min(p.minimum_lane_changing_velocity, p.max_acc * p.lane_change_prepare_duration);
-  p.lane_change_finish_judge_buffer =
-    declare_parameter<double>("lane_change.lane_change_finish_judge_buffer");
-
-  // 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 =
@@ -297,10 +264,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
-  if (p.backward_length_buffer_for_end_of_lane < 1.0) {
-    RCLCPP_WARN_STREAM(
-      get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer.");
-  }
   return p;
 }
 

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -148,8 +148,7 @@ ModuleStatus LaneChangeInterface::updateState()
     return ModuleStatus::RUNNING;
   }
 
-  const auto & common_parameters = module_type_->getCommonParam();
-  const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane;
+  const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane;
   const auto status = module_type_->getLaneChangeStatus();
   if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) {
     log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change.");
@@ -432,8 +431,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
   const auto & common_parameter = module_type_->getCommonParam();
   const auto shift_intervals =
     route_handler->getLateralIntervalsToPreferredLane(current_lanes.back());
-  const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength(
-    common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane);
+  const double next_lane_change_buffer =
+    utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals);
   const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold;
   const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold;
   const double & base_to_front = common_parameter.base_link2front;

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -141,6 +141,49 @@ void LaneChangeModuleManager::init(rclcpp::Node * node)
   p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.lateral_distance_max_threshold"));
 
+  // lane change parameters
+  const auto max_acc = getOrDeclareParameter<double>(*node, "normal.max_acc");
+  p.backward_length_buffer_for_end_of_lane =
+    getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
+  p.backward_length_buffer_for_blocking_object =
+    getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
+  p.lane_changing_lateral_jerk =
+    getOrDeclareParameter<double>(*node, parameter("lane_changing_lateral_jerk"));
+  p.lane_change_prepare_duration =
+    getOrDeclareParameter<double>(*node, parameter("prepare_duration"));
+  p.minimum_lane_changing_velocity =
+    getOrDeclareParameter<double>(*node, parameter("minimum_lane_changing_velocity"));
+  p.minimum_lane_changing_velocity =
+    std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration);
+  p.lane_change_finish_judge_buffer =
+    getOrDeclareParameter<double>(*node, parameter("lane_change_finish_judge_buffer"));
+
+  if (p.backward_length_buffer_for_end_of_lane < 1.0) {
+    RCLCPP_WARN_STREAM(
+      node->get_logger().get_child(name()),
+      "Lane change buffer must be more than 1 meter. Modifying the buffer.");
+  }
+
+  // lateral acceleration map for lane change
+  const auto lateral_acc_velocity =
+    getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.velocity"));
+  const auto min_lateral_acc =
+    getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.min_values"));
+  const auto max_lateral_acc =
+    getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.max_values"));
+  if (
+    lateral_acc_velocity.size() != min_lateral_acc.size() ||
+    lateral_acc_velocity.size() != max_lateral_acc.size()) {
+    RCLCPP_ERROR(
+      node->get_logger().get_child(name()),
+      "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));
+  }
+
   // target object
   {
     const std::string ns = "lane_change.target_object.";