fix(lane_change): use current lane for num to preferred lane input (a…

…utowarefoundation#2615)

* fix(lane_change): use current lane for num to preferred lane input

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* fix lane change distance from deadend

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Make separate function to compute resampling interval

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Change default config

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Make phase info data structure

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Added error for finish judge buffer

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* Fix rebase

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* ci(pre-commit): autofix

* warn user of the modified values

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

@@ -6,8 +6,8 @@
 
       minimum_lane_change_prepare_distance: 2.0 # [m]
       minimum_lane_change_length: 16.5          # [m]
-      backward_length_buffer_for_end_of_lane: 2.0 # [m]
-      lane_change_finish_judge_buffer: 3.0      # [m]
+      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]

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

@@ -77,6 +77,13 @@ enum class LaneChangeStates {
   Stop,
 };
 
+struct LaneChangePhaseInfo
+{
+  double prepare{0.0};
+  double lane_changing{0.0};
+
+  [[nodiscard]] double sum() const { return prepare + lane_changing; }
+};
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -113,23 +113,21 @@ ShiftLine getLaneChangeShiftLine(
 
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
-  const Pose & lane_changing_start_pose, const double prepare_distance,
-  const double lane_changing_distance, const double forward_path_length,
-  const double lane_changing_speed);
-
-PathWithLaneId getReferencePathFromTargetLane(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
-  const Pose & in_target_front_pose, const Pose & in_target_end_pose);
+  const Pose & lane_changing_start_pose, const double target_lane_length,
+  const LaneChangePhaseInfo dist_prepare_to_lc_end, const double min_total_lane_changing_distance,
+  const double forward_path_length, const double resample_interval, const bool is_goal_in_route);
 
 PathWithLaneId getLaneChangePathPrepareSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
-  const Pose & current_pose, const double & backward_path_length, const double & prepare_distance,
-  const double & prepare_duration, const double & minimum_lane_change_velocity);
+  const double arc_length_from_current, const double backward_path_length,
+  const double prepare_distance, const double prepare_speed);
 
 PathWithLaneId getLaneChangePathLaneChangingSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
-  const Pose & current_pose, const double prepare_distance, const double lane_change_distance,
-  const double lane_changing_speed, const BehaviorPathPlannerParameters & common_param);
+  const double forward_path_length, const double arc_length_from_target,
+  const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end,
+  const double lane_changing_speed, const double total_required_min_dist);
+
 bool isEgoWithinOriginalLane(
   const lanelet::ConstLanelets & current_lanes, const Pose & current_pose,
   const BehaviorPathPlannerParameters & common_param);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -250,6 +250,11 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 
   p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0);
   p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0);
+
+  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;
 }
 
@@ -266,8 +271,8 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam()
   p.shifting_lateral_jerk = dp("shifting_lateral_jerk", 0.2);
   p.min_shifting_distance = dp("min_shifting_distance", 5.0);
   p.min_shifting_speed = dp("min_shifting_speed", 5.56);
-  p.shift_request_time_limit = dp("shift_request_time_limit", 1.0);
   p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0);
+  p.shift_request_time_limit = dp("shift_request_time_limit", 1.0);
   p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0);
 
   return p;
@@ -422,6 +427,15 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
     exit(EXIT_FAILURE);
   }
 
+  const auto lc_buffer =
+    this->get_parameter("lane_change.backward_length_buffer_for_end_of_lane").get_value<double>();
+  if (lc_buffer < p.lane_change_finish_judge_buffer + 1.0) {
+    p.lane_change_finish_judge_buffer = lc_buffer - 1;
+    RCLCPP_WARN_STREAM(
+      get_logger(), "lane change buffer is less than finish buffer. Modifying the value to "
+                      << p.lane_change_finish_judge_buffer << "....");
+  }
+
   return p;
 }