fix(lane_change): cap ego's predicted path velocity (RT1-8505)

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -116,10 +116,9 @@ CandidateOutput assignToCandidate(
 std::optional<lanelet::ConstLanelet> get_lane_change_target_lane(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes);
 
-std::vector<PoseWithVelocityStamped> convertToPredictedPath(
-  const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose,
-  const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters, const double resolution);
+std::vector<PoseWithVelocityStamped> convert_to_predicted_path(
+  const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
+  const double lane_changing_acceleration);
 
 bool isParkedObject(
   const PathWithLaneId & path, const RouteHandler & route_handler,

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1812,8 +1812,6 @@ bool NormalLaneChange::has_collision_with_decel_patterns(
     return false;
   }
 
-  const auto current_pose = common_data_ptr_->get_ego_pose();
-  const auto current_twist = common_data_ptr_->get_ego_twist();
   const auto bpp_param = *common_data_ptr_->bpp_param_ptr;
   const auto global_min_acc = bpp_param.min_acc;
   const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing;
@@ -1830,17 +1828,11 @@ bool NormalLaneChange::has_collision_with_decel_patterns(
     acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(),
     [&](double n) { return lane_changing_acc + n * acc_resolution; });
 
-  const auto time_resolution =
-    lane_change_parameters_->safety.collision_check.prediction_time_resolution;
-
   const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity;
   const auto all_collided = std::all_of(
     acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) {
-      const auto ego_predicted_path = utils::lane_change::convertToPredictedPath(
-        lane_change_path, current_twist, current_pose, acceleration, bpp_param,
-        *lane_change_parameters_, time_resolution);
-      const auto debug_predicted_path =
-        utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
+      const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path(
+        common_data_ptr_, lane_change_path, acceleration);
 
       return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) {
         const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th)

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -599,52 +599,59 @@ std::optional<lanelet::ConstLanelet> get_lane_change_target_lane(
   return route_handler_ptr->getLaneChangeTargetExceptPreferredLane(current_lanes, direction);
 }
 
-std::vector<PoseWithVelocityStamped> convertToPredictedPath(
-  const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose,
-  const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters, const double resolution)
+std::vector<PoseWithVelocityStamped> convert_to_predicted_path(
+  const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
+  const double lane_changing_acceleration)
 {
   if (lane_change_path.path.points.empty()) {
     return {};
   }
 
   const auto & path = lane_change_path.path;
-  const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare;
-  const auto duration = lane_change_path.info.duration.sum();
-  const auto prepare_time = lane_change_path.info.duration.prepare;
-  const auto & minimum_lane_changing_velocity =
-    lane_change_parameters.trajectory.min_lane_changing_velocity;
-
+  const auto & vehicle_pose = common_data_ptr->get_ego_pose();
+  const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr;
   const auto nearest_seg_idx =
     autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold,
-      common_parameters.ego_nearest_yaw_threshold);
+      path.points, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold,
+      bpp_param_ptr->ego_nearest_yaw_threshold);
 
-  std::vector<PoseWithVelocityStamped> predicted_path;
   const auto vehicle_pose_frenet =
     convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx);
-  const double initial_velocity = std::abs(vehicle_twist.linear.x);
+
+  const auto lc_params = *common_data_ptr->lc_param_ptr;
+  const auto initial_velocity = common_data_ptr->get_ego_speed();
+  const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare;
+  const auto duration = lane_change_path.info.duration.sum();
+  const auto prepare_time = lane_change_path.info.duration.prepare;
+  const auto minimum_lane_changing_velocity = lc_params.trajectory.min_lane_changing_velocity;
+  const auto resolution = lc_params.safety.collision_check.prediction_time_resolution;
+
+  std::vector<PoseWithVelocityStamped> predicted_path;
 
   // prepare segment
   for (double t = 0.0; t < prepare_time; t += resolution) {
-    const double velocity =
-      std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity);
-    const double length = initial_velocity * t + 0.5 * prepare_acc * t * t;
+    const auto velocity = std::min(
+      std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity),
+      lane_change_path.info.velocity.prepare);
+    const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t;
     const auto pose = autoware::motion_utils::calcInterpolatedPose(
       path.points, vehicle_pose_frenet.length + length);
     predicted_path.emplace_back(t, pose, velocity);
   }
 
   // lane changing segment
-  const double lane_changing_velocity =
-    std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity);
-  const double offset =
+  const auto lane_changing_velocity = std::min(
+    std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity),
+    lane_change_path.info.velocity.prepare);
+  const auto offset =
     initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time;
   for (double t = prepare_time; t < duration; t += resolution) {
-    const double delta_t = t - prepare_time;
-    const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t;
-    const double length = lane_changing_velocity * delta_t +
-                          0.5 * lane_changing_acceleration * delta_t * delta_t + offset;
+    const auto delta_t = t - prepare_time;
+    const auto velocity = std::min(
+      lane_changing_velocity + lane_changing_acceleration * delta_t,
+      lane_change_path.info.velocity.lane_changing);
+    const auto length = lane_changing_velocity * delta_t +
+                        0.5 * lane_changing_acceleration * delta_t * delta_t + offset;
     const auto pose = autoware::motion_utils::calcInterpolatedPose(
       path.points, vehicle_pose_frenet.length + length);
     predicted_path.emplace_back(t, pose, velocity);