feat(lane_change): combine terminal lane change path at waiting appro…

…val (#6176)

* saved

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(lane_change): fix drivable area info

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(lane_change): update filter object

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* feat(lane_change): fix turn signal

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): fix typo

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): remove updateLaneChangeStatus()

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): remove redundant process

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): calculate distance to goal

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(lane_change): add empty guard

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase
 
   LaneChangePath getLaneChangePath() const override;
 
+  BehaviorModuleOutput getTerminalLaneChangePath() const override;
+
   BehaviorModuleOutput generateOutput() override;
 
   void extendOutputDrivableArea(BehaviorModuleOutput & output) const override;
@@ -141,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase
     const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
     const bool check_safety = true) const override;
 
+  std::optional<LaneChangePath> calcTerminalLaneChangePath(
+    const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes) const;
+
   TurnSignalInfo calcTurnSignalInfo() const override;
 
   bool isValidPath(const PathWithLaneId & path) const override;

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -88,6 +88,8 @@ class LaneChangeBase
 
   virtual LaneChangePath getLaneChangePath() const = 0;
 
+  virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0;
+
   virtual bool isEgoOnPreparePhase() const = 0;
 
   virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0;

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
   const PathWithLaneId & reference_path, const double shift_length);
 
+ShiftLine getLaneChangingShiftLine(
+  const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose,
+  const PathWithLaneId & reference_path, const double shift_length);
+
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double target_lane_length,

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -124,15 +124,12 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 {
   *prev_approved_path_ = getPreviousModuleOutput().path;
-  module_type_->insertStopPoint(
-    module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_);
 
   BehaviorModuleOutput out;
-  out.path = *prev_approved_path_;
-  out.reference_path = getPreviousModuleOutput().reference_path;
-  out.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
-  out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
-  out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
+  out = module_type_->getTerminalLaneChangePath();
+  module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path);
+  out.turn_signal_info =
+    getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info);
 
   const auto & lane_change_debug = module_type_->getDebugData();
   for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) {
@@ -438,16 +435,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
   const double buffer =
     next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front;
   const double path_length = motion_utils::calcArcLength(path.points);
-  const auto & front_point = path.points.front().point.pose.position;
   const size_t & current_nearest_seg_idx =
     motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
       path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
-  const double length_front_to_ego = motion_utils::calcSignedArcLength(
-    path.points, front_point, static_cast<size_t>(0), current_pose.position,
-    current_nearest_seg_idx);
+  const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes);
   const auto start_pose =
     motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0));
-  if (path_length - length_front_to_ego < buffer && start_pose) {
+  if (dist_to_terminal - base_to_front < buffer && start_pose) {
     // modify turn signal
     current_turn_signal_info.desired_start_point = *start_pose;
     current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end;

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -140,6 +140,46 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const
   return status_.lane_change_path;
 }
 
+BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
+{
+  BehaviorModuleOutput output;
+
+  const auto current_lanes = getCurrentLanes();
+  if (current_lanes.empty()) {
+    RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong.");
+    output.path = prev_module_path_;
+    output.reference_path = prev_module_reference_path_;
+    output.drivable_area_info = prev_drivable_area_info_;
+    output.turn_signal_info = prev_turn_signal_info_;
+    return output;
+  }
+
+  const auto terminal_path =
+    calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_));
+  if (!terminal_path) {
+    RCLCPP_WARN(logger_, "Terminal path not found!!!");
+    output.path = prev_module_path_;
+    output.reference_path = prev_module_reference_path_;
+    output.drivable_area_info = prev_drivable_area_info_;
+    output.turn_signal_info = prev_turn_signal_info_;
+    return output;
+  }
+
+  output.path = terminal_path->path;
+  output.reference_path = prev_module_reference_path_;
+  output.turn_signal_info = updateOutputTurnSignal();
+
+  extendOutputDrivableArea(output);
+
+  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
+  output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+    output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
+    planner_data_->parameters.ego_nearest_dist_threshold,
+    planner_data_->parameters.ego_nearest_yaw_threshold);
+
+  return output;
+}
+
 BehaviorModuleOutput NormalLaneChange::generateOutput()
 {
   BehaviorModuleOutput output;
@@ -835,6 +875,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const auto current_pose = getEgoPose();
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
+  const auto shift_intervals =
+    route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
+  const double minimum_lane_change_length =
+    utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
 
   // Guard
   if (objects.objects.empty()) {
@@ -904,6 +948,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
       return std::abs(lateral) > (common_parameters.vehicle_width / 2);
     };
 
+    // ignore object that are ahead of terminal lane change start
+    {
+      double distance_to_terminal_from_object = std::numeric_limits<double>::max();
+      for (const auto & polygon_p : obj_polygon_outer) {
+        const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
+        Pose polygon_pose;
+        polygon_pose.position = obj_p;
+        polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
+        const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes);
+        if (dist < distance_to_terminal_from_object) {
+          distance_to_terminal_from_object = dist;
+        }
+      }
+      if (minimum_lane_change_length > distance_to_terminal_from_object) {
+        continue;
+      }
+    }
+
     // ignore static object that are behind the ego vehicle
     if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) {
       continue;
@@ -1413,6 +1475,151 @@ bool NormalLaneChange::getLaneChangePaths(
   return false;
 }
 
+std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
+{
+  if (current_lanes.empty() || target_lanes.empty()) {
+    RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
+    return {};
+  }
+  const auto & route_handler = *getRouteHandler();
+  const auto & common_parameters = planner_data_->parameters;
+
+  const auto forward_path_length = common_parameters.forward_path_length;
+  const auto minimum_lane_changing_velocity =
+    lane_change_parameters_->minimum_lane_changing_velocity;
+
+  const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
+
+  const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_,
+    route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()));
+  const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_,
+    route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
+
+  const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
+
+  const auto sorted_lane_ids =
+    utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes);
+
+  const auto target_neighbor_preferred_lane_poly_2d =
+    utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
+  if (target_neighbor_preferred_lane_poly_2d.empty()) {
+    RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
+    return {};
+  }
+
+  // lane changing start getEgoPose() is at the end of prepare segment
+  const auto current_lane_terminal_point =
+    lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back());
+
+  double distance_to_terminal_from_goal = 0;
+  if (is_goal_in_route) {
+    distance_to_terminal_from_goal =
+      utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes);
+  }
+
+  const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose(
+    prev_module_path_.points, current_lane_terminal_point,
+    -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal));
+
+  if (!lane_changing_start_pose) {
+    RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!");
+    return {};
+  }
+
+  const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet(
+    current_lanes, target_lanes.front(), lane_changing_start_pose.value());
+
+  // Check if the lane changing start point is not on the lanes next to target lanes,
+  if (target_length_from_lane_change_start_pose > 0.0) {
+    RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!");
+    return {};
+  }
+
+  const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(
+    target_lanes, lane_changing_start_pose.value());
+
+  const auto [min_lateral_acc, max_lateral_acc] =
+    lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity);
+
+  const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
+    shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc);
+
+  const auto target_segment = getTargetSegment(
+    target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer,
+    minimum_lane_changing_velocity, next_lane_change_buffer);
+
+  if (target_segment.points.empty()) {
+    RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong...");
+    return {};
+  }
+
+  const lanelet::BasicPoint2d lc_start_point(
+    lane_changing_start_pose->position.x, lane_changing_start_pose->position.y);
+
+  const auto target_lane_polygon =
+    lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits<double>::max());
+  const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon();
+
+  const auto is_valid_start_point =
+    boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
+    boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
+
+  LaneChangeInfo lane_change_info;
+  lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0};
+  lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time};
+  lane_change_info.velocity =
+    LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity};
+  lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer};
+  lane_change_info.current_lanes = current_lanes;
+  lane_change_info.target_lanes = target_lanes;
+  lane_change_info.lane_changing_start = lane_changing_start_pose.value();
+  lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
+  lane_change_info.lateral_acceleration = max_lateral_acc;
+  lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity;
+
+  if (!is_valid_start_point) {
+    RCLCPP_WARN(
+      logger_,
+      "Reject: lane changing points are not inside of the target preferred lanes or its "
+      "neighbors");
+    return {};
+  }
+
+  const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(
+    lane_change_buffer, minimum_lane_changing_velocity);
+  const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane(
+    route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length,
+    lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route,
+    next_lane_change_buffer);
+
+  if (target_lane_reference_path.points.empty()) {
+    RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!");
+    return {};
+  }
+
+  lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
+    lane_changing_start_pose.value(), target_segment.points.front().point.pose,
+    target_lane_reference_path, shift_length);
+
+  auto reference_segment = prev_module_path_;
+  const double length_to_lane_changing_start = motion_utils::calcSignedArcLength(
+    reference_segment.points, reference_segment.points.front().point.pose.position,
+    lane_changing_start_pose->position);
+  utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0);
+  // remove terminal points because utils::clipPathLength() calculates extra long path
+  reference_segment.points.pop_back();
+  reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity;
+
+  const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath(
+    lane_change_info, reference_segment, target_segment, target_lane_reference_path,
+    sorted_lane_ids);
+
+  return terminal_lane_change_path;
+}
+
 PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
 {
   const auto & path = status_.lane_change_path;
@@ -1465,6 +1672,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const
 
   TurnSignalInfo turn_signal_info{};
 
+  if (path.path.points.empty()) {
+    return turn_signal_info;
+  }
+
   // desired start pose = prepare start pose
   turn_signal_info.desired_start_point = std::invoke([&]() {
     const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time;

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -412,17 +412,17 @@ PathWithLaneId getReferencePathFromTargetLane(
     return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer);
   });
 
-  if (s_end - s_start < lane_changing_length) {
-    return PathWithLaneId();
-  }
-
   RCLCPP_DEBUG(
     rclcpp::get_logger("behavior_path_planner")
       .get_child("lane_change")
       .get_child("util")
       .get_child("getReferencePathFromTargetLane"),
     "start: %f, end: %f", s_start, s_end);
 
+  if (s_end - s_start < lane_changing_length) {
+    return PathWithLaneId();
+  }
+
   const auto lane_changing_reference_path =
     route_handler.getCenterLinePath(target_lanes, s_start, s_end);
 
@@ -437,6 +437,14 @@ ShiftLine getLaneChangingShiftLine(
   const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose;
   const Pose & lane_changing_end_pose = target_segment.points.front().point.pose;
 
+  return getLaneChangingShiftLine(
+    lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length);
+}
+
+ShiftLine getLaneChangingShiftLine(
+  const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose,
+  const PathWithLaneId & reference_path, const double shift_length)
+{
   ShiftLine shift_line;
   shift_line.end_shift_length = shift_length;
   shift_line.start = lane_changing_start_pose;