Refactoring and renaming

Remove some of the unused variables

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -23,4 +23,4 @@
     expected_front_deceleration: -1.0
     expected_rear_deceleration: -1.0
     rear_vehicle_reaction_time: 1.0
-    safety_time_margin_for_control: 2.0
+    rear_vehicle_safety_time_margin: 2.0

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

@@ -1,26 +1,15 @@
 /**:
   ros__parameters:
     lane_change:
-      min_stop_distance: 5.0
-      stop_time: 2.0
-      hysteresis_buffer_distance: 2.0
       lane_change_prepare_duration: 4.0
       lane_changing_duration: 8.0
       lane_change_finish_judge_buffer: 3.0
       minimum_lane_change_velocity: 5.6
-      prediction_duration: 8.0
       prediction_time_resolution: 0.5
-      static_obstacle_velocity_thresh: 1.5
       maximum_deceleration: 1.0
       lane_change_sampling_num: 10
       enable_abort_lane_change: false
       enable_collision_check_at_prepare_phase: false
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
-      enable_blocked_by_obstacle: false
       lane_change_search_distance: 30.0
-      safety_time_margin_for_control: 2.0
-      rear_vehicle_reaction_time: 1.0
-      lateral_distance_threshold: 5.0
-      expected_front_deceleration: -1.0
-      expected_rear_deceleration: -1.0

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -60,7 +60,7 @@ struct BehaviorPathPlannerParameters
   double expected_front_deceleration;
   double expected_rear_deceleration;
   double rear_vehicle_reaction_time;
-  double safety_time_margin_for_control;
+  double rear_vehicle_safety_time_margin;
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_

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

@@ -40,16 +40,12 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
 struct LaneChangeParameters
 {
-  double min_stop_distance;
-  double stop_time;
-  double hysteresis_buffer_distance;
   double lane_change_prepare_duration;
   double lane_changing_duration;
+  double minimum_lane_change_prepare_distance;
   double lane_change_finish_judge_buffer;
   double minimum_lane_change_velocity;
-  double prediction_duration;
   double prediction_time_resolution;
-  double static_obstacle_velocity_thresh;
   double maximum_deceleration;
   int lane_change_sampling_num;
   double abort_lane_change_velocity_thresh;
@@ -59,7 +55,6 @@ struct LaneChangeParameters
   bool enable_collision_check_at_prepare_phase;
   bool use_predicted_path_outside_lanelet;
   bool use_all_predicted_path;
-  bool enable_blocked_by_obstacle;
   double lane_change_search_distance;
 };
 

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

@@ -45,6 +45,12 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
+double getExpectedVelocityWhenDecelerate(
+  const double & current_velocity, const double & expected_acceleration,
+  const double & lane_change_prepare_duration);
+double getDistanceWhenDecelerate(
+  const double & velocity, const double & expected_acceleration, const double & duration,
+  const double & minimum_distance);
 std::vector<LaneChangePath> getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -340,17 +340,13 @@ bool isObjectFront(const Pose & projected_ego_pose);
 
 double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel);
 
-double stoppingDistance(
-  const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
-  const double & rear_vehicle_reaction_time);
-
 double frontVehicleStopDistance(
   const double & front_vehicle_velocity, const double & front_vehicle_accel,
   const double & distance_to_collision);
 
 double rearVehicleStopDistance(
   const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
-  const double & rear_vehicle_reaction_time, const double & safety_time_margin_for_control);
+  const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin);
 
 bool isLongitudinalDistanceEnough(
   const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -206,7 +206,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0);
   p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0);
   p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 1.0);
-  p.safety_time_margin_for_control = declare_parameter("safety_time_margin_for_control", 2.0);
+  p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0);
   return p;
 }
 
@@ -311,16 +311,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   };
 
   LaneChangeParameters p{};
-  p.min_stop_distance = dp("min_stop_distance", 5.0);
-  p.stop_time = dp("stop_time", 2.0);
-  p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0);
   p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0);
   p.lane_changing_duration = dp("lane_changing_duration", 4.0);
+  p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0);
   p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0);
   p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3);
-  p.prediction_duration = dp("prediction_duration", 8.0);
   p.prediction_time_resolution = dp("prediction_time_resolution", 0.5);
-  p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1);
   p.maximum_deceleration = dp("maximum_deceleration", 1.0);
   p.lane_change_sampling_num = dp("lane_change_sampling_num", 10);
   p.enable_abort_lane_change = dp("enable_abort_lane_change", true);
@@ -331,7 +327,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.abort_lane_change_angle_thresh =
     dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0));
   p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3);
-  p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false);
   p.lane_change_search_distance = dp("lane_change_search_distance", 30.0);
 
   // validation of parameters

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

@@ -93,9 +93,8 @@ bool LaneChangeModule::isExecutionRequested() const
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
   // Find lane change path
-  bool found_valid_path, found_safe_path;
   LaneChangePath selected_path;
-  std::tie(found_valid_path, found_safe_path) =
+  const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
 
   return found_valid_path;
@@ -111,10 +110,8 @@ bool LaneChangeModule::isExecutionReady() const
   const auto current_lanes = getCurrentLanes();
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
-  // Find lane change path
-  bool found_valid_path, found_safe_path;
   LaneChangePath selected_path;
-  std::tie(found_valid_path, found_safe_path) =
+  const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
 
   return found_safe_path;
@@ -182,12 +179,8 @@ CandidateOutput LaneChangeModule::planCandidate() const
   const auto current_lanes = getCurrentLanes();
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
-  // Find lane change path
-  bool found_valid_path = false;
-  bool found_safe_path = false;
-
   LaneChangePath selected_path;
-  std::tie(found_valid_path, found_safe_path) =
+  [[maybe_unused]] const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
   selected_path.path.header = planner_data_->route_handler->getRouteHeader();
 
@@ -234,9 +227,8 @@ void LaneChangeModule::updateLaneChangeStatus()
   status_.lane_change_lanes = lane_change_lanes;
 
   // Find lane change path
-  bool found_valid_path, found_safe_path;
   LaneChangePath selected_path;
-  std::tie(found_valid_path, found_safe_path) =
+  const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
 
   // Update status

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

@@ -66,7 +66,19 @@ bool isPathInLanelets(
   }
   return true;
 }
+double getExpectedVelocityWhenDecelerate(
+  const double & velocity, const double & expected_acceleration, const double & duration)
+{
+  return velocity + expected_acceleration * duration;
+}
 
+double getDistanceWhenDecelerate(
+  const double & velocity, const double & expected_acceleration, const double & duration,
+  const double & minimum_distance)
+{
+  const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2);
+  return std::max(distance, minimum_distance);
+}
 std::vector<LaneChangePath> getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,
@@ -79,17 +91,19 @@ std::vector<LaneChangePath> getLaneChangePaths(
   }
 
   // rename parameter
-  const double backward_path_length = common_parameter.backward_path_length;
-  const double forward_path_length = common_parameter.forward_path_length;
-  const double lane_change_prepare_duration = parameter.lane_change_prepare_duration;
-  const double lane_changing_duration = parameter.lane_changing_duration;
-  const double minimum_lane_change_length = common_parameter.minimum_lane_change_length;
-  const double minimum_lane_change_velocity = parameter.minimum_lane_change_velocity;
-  const double maximum_deceleration = parameter.maximum_deceleration;
-  const int lane_change_sampling_num = parameter.lane_change_sampling_num;
+  const auto & backward_path_length = common_parameter.backward_path_length;
+  const auto & forward_path_length = common_parameter.forward_path_length;
+  const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration;
+  const auto & lane_changing_duration = parameter.lane_changing_duration;
+  const auto & minimum_lane_change_prepare_distance =
+    parameter.minimum_lane_change_prepare_distance;
+  const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length;
+  const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity;
+  const auto & maximum_deceleration = parameter.maximum_deceleration;
+  const auto & lane_change_sampling_num = parameter.lane_change_sampling_num;
 
   // get velocity
-  const double v0 = util::l2Norm(twist.linear);
+  const double current_velocity = util::l2Norm(twist.linear);
 
   constexpr double buffer = 1.0;  // buffer for min_lane_change_length
   const double acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num;
@@ -99,45 +113,47 @@ std::vector<LaneChangePath> getLaneChangePaths(
 
   for (double acceleration = 0.0; acceleration >= -maximum_deceleration;
        acceleration -= acceleration_resolution) {
-    PathWithLaneId reference_path{};
-    const double v1 = v0 + acceleration * lane_change_prepare_duration;
-    const double v2 = v1 + acceleration * lane_changing_duration;
+    const double lane_change_prepare_velocity = getExpectedVelocityWhenDecelerate(
+      current_velocity, acceleration, lane_change_prepare_duration);
+    const double lane_changing_velocity = getExpectedVelocityWhenDecelerate(
+      lane_change_prepare_velocity, acceleration, lane_changing_duration);
 
     // skip if velocity becomes less than zero before finishing lane change
-    if (v2 < 0.0) {
+    if (lane_changing_velocity < 0.0) {
       continue;
     }
 
     // get path on original lanes
-    const double straight_distance = v0 * lane_change_prepare_duration +
-                                     0.5 * acceleration * std::pow(lane_change_prepare_duration, 2);
-    double lane_change_distance =
-      v1 * lane_changing_duration + 0.5 * acceleration * std::pow(lane_changing_duration, 2);
-    lane_change_distance = std::max(lane_change_distance, minimum_lane_change_length);
-
-    if (straight_distance < target_distance) {
+    const double prepare_distance = getDistanceWhenDecelerate(
+      current_velocity, acceleration, lane_change_prepare_duration,
+      minimum_lane_change_prepare_distance);
+    const double lane_changing_distance = getDistanceWhenDecelerate(
+      lane_change_prepare_velocity, acceleration, lane_changing_duration,
+      minimum_lane_change_length + buffer);
+
+    if (prepare_distance < target_distance) {
       continue;
     }
 
     PathWithLaneId reference_path1;
     {
       const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose);
       const double s_start = arc_position.length - backward_path_length;
-      const double s_end = arc_position.length + straight_distance;
+      const double s_end = arc_position.length + prepare_distance;
       reference_path1 = route_handler.getCenterLinePath(original_lanelets, s_start, s_end);
     }
 
     reference_path1.points.back().point.longitudinal_velocity_mps = std::min(
       reference_path1.points.back().point.longitudinal_velocity_mps,
       static_cast<float>(
-        std::max(straight_distance / lane_change_prepare_duration, minimum_lane_change_velocity)));
+        std::max(prepare_distance / lane_change_prepare_duration, minimum_lane_change_velocity)));
 
     PathWithLaneId reference_path2{};
     {
       const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets);
       const auto arc_position = lanelet::utils::getArcCoordinates(target_lanelets, pose);
       const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back()));
-      double s_start = arc_position.length + straight_distance + lane_change_distance;
+      double s_start = arc_position.length + prepare_distance + lane_changing_distance;
       s_start = std::min(s_start, lane_length - num * minimum_lane_change_length);
       double s_end = s_start + forward_path_length;
       s_end = std::min(s_end, lane_length - num * (minimum_lane_change_length + buffer));
@@ -149,7 +165,7 @@ std::vector<LaneChangePath> getLaneChangePaths(
       point.point.longitudinal_velocity_mps = std::min(
         point.point.longitudinal_velocity_mps,
         static_cast<float>(
-          std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity)));
+          std::max(lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity)));
     }
 
     if (reference_path1.points.empty() || reference_path2.points.empty()) {
@@ -161,16 +177,16 @@ std::vector<LaneChangePath> getLaneChangePaths(
 
     LaneChangePath candidate_path;
     candidate_path.acceleration = acceleration;
-    candidate_path.preparation_length = straight_distance;
-    candidate_path.lane_change_length = lane_change_distance;
+    candidate_path.preparation_length = prepare_distance;
+    candidate_path.lane_change_length = lane_changing_distance;
 
     PathWithLaneId target_lane_reference_path;
     {
       const lanelet::ArcCoordinates lane_change_start_arc_position =
         lanelet::utils::getArcCoordinates(
           target_lanelets, reference_path1.points.back().point.pose);
       double s_start = lane_change_start_arc_position.length;
-      double s_end = s_start + straight_distance + lane_change_distance + forward_path_length;
+      double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length;
       target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end);
     }
 
@@ -220,8 +236,8 @@ std::vector<LaneChangePath> getLaneChangePaths(
         }
         point.point.longitudinal_velocity_mps = std::min(
           point.point.longitudinal_velocity_mps,
-          static_cast<float>(
-            std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity)));
+          static_cast<float>(std::max(
+            lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity)));
         const auto nearest_idx =
           motion_utils::findNearestIndex(reference_path2.points, point.point.pose);
         point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids;

planning/behavior_path_planner/src/utilities.cpp

@@ -2144,13 +2144,6 @@ double stoppingDistance(const double & vehicle_velocity, const double & vehicle_
   return -std::pow(vehicle_velocity, 2) / (2.0 * deceleration);
 }
 
-double stoppingDistance(
-  const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
-  const double & rear_vehicle_reaction_time)
-{
-  return rear_vehicle_velocity * rear_vehicle_reaction_time +
-         stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel);
-}
 double frontVehicleStopDistance(
   const double & front_vehicle_velocity, const double & front_vehicle_accel,
   const double & distance_to_collision)
@@ -2160,10 +2153,11 @@ double frontVehicleStopDistance(
 
 double rearVehicleStopDistance(
   const double & rear_vehicle_velocity, const double & rear_vehicle_accel,
-  const double & rear_vehicle_reaction_time, const double & safety_time_margin_for_control)
+  const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin)
 {
-  return stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel, rear_vehicle_reaction_time) +
-         rear_vehicle_velocity * safety_time_margin_for_control;
+  return rear_vehicle_velocity * rear_vehicle_reaction_time +
+         stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel) +
+         rear_vehicle_velocity * rear_vehicle_safety_time_margin;
 }
 
 bool isLongitudinalDistanceEnough(
@@ -2202,7 +2196,7 @@ bool hasEnoughDistance(
 
   const auto rear_vehicle_stop_threshold = rearVehicleStopDistance(
     util::l2Norm(rear_vehicle_velocity), rear_vehicle_accel, param.rear_vehicle_reaction_time,
-    param.safety_time_margin_for_control);
+    param.rear_vehicle_safety_time_margin);
 
   return isLongitudinalDistanceEnough(rear_vehicle_stop_threshold, front_vehicle_stop_threshold);
 }