Merge pull request #1141 from tier4/feat/wait_approval_at_abort

feat(lane_change): terminal lane change and abort behavior

Author: shmpwk

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -53,9 +53,11 @@ class NormalLaneChange : public LaneChangeBase
 
   LaneChangePath getLaneChangePath() const override;
 
+  BehaviorModuleOutput getTerminalLaneChangePath() const override;
+
   BehaviorModuleOutput generateOutput() override;
 
-  void extendOutputDrivableArea(BehaviorModuleOutput & output) override;
+  void extendOutputDrivableArea(BehaviorModuleOutput & output) const override;
 
   void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;
 
@@ -65,7 +67,7 @@ class NormalLaneChange : public LaneChangeBase
 
   void resetParameters() override;
 
-  TurnSignalInfo updateOutputTurnSignal() override;
+  TurnSignalInfo updateOutputTurnSignal() const override;
 
   bool calcAbortPath() override;
 
@@ -89,7 +91,7 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbortState() const override;
 
-  bool isLaneChangeRequired() const override;
+  bool isLaneChangeRequired() override;
 
   bool isStoppedAtRedTrafficLight() const override;
 
@@ -141,7 +143,11 @@ class NormalLaneChange : public LaneChangeBase
     const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
     const bool check_safety = true) const override;
 
-  TurnSignalInfo calcTurnSignalInfo() 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

@@ -66,28 +66,30 @@ class LaneChangeBase
 
   virtual BehaviorModuleOutput generateOutput() = 0;
 
-  virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0;
+  virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0;
 
   virtual PathWithLaneId getReferencePath() const = 0;
 
   virtual std::optional<PathWithLaneId> extendPath() = 0;
 
   virtual void resetParameters() = 0;
 
-  virtual TurnSignalInfo updateOutputTurnSignal() = 0;
+  virtual TurnSignalInfo updateOutputTurnSignal() const = 0;
 
   virtual bool hasFinishedLaneChange() const = 0;
 
   virtual bool hasFinishedAbort() const = 0;
 
-  virtual bool isLaneChangeRequired() const = 0;
+  virtual bool isLaneChangeRequired() = 0;
 
   virtual bool isAbortState() const = 0;
 
   virtual bool isAbleToReturnCurrentLane() const = 0;
 
   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;
@@ -225,7 +227,7 @@ class LaneChangeBase
     LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
     const bool is_stuck, const bool check_safety) const = 0;
 
-  virtual TurnSignalInfo calcTurnSignalInfo() = 0;
+  virtual TurnSignalInfo calcTurnSignalInfo() const = 0;
 
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 

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

@@ -45,7 +45,7 @@ struct LaneChangeStatus
   std::vector<lanelet::Id> lane_follow_lane_ids{};
   std::vector<lanelet::Id> lane_change_lane_ids{};
   bool is_safe{false};
-  bool is_valid_path{true};
+  bool is_valid_path{false};
   double start_distance{0.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

@@ -50,9 +50,6 @@ LaneChangeInterface::LaneChangeInterface(
 void LaneChangeInterface::processOnEntry()
 {
   waitApproval();
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  module_type_->updateLaneChangeStatus();
 }
 
 void LaneChangeInterface::processOnExit()
@@ -73,13 +70,21 @@ bool LaneChangeInterface::isExecutionRequested() const
 
 bool LaneChangeInterface::isExecutionReady() const
 {
-  return module_type_->isSafe();
+  return module_type_->isSafe() && !module_type_->isAbortState();
 }
 
 void LaneChangeInterface::updateData()
 {
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
+  module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
+  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
+
+  if (isWaitingApproval()) {
+    module_type_->updateLaneChangeStatus();
+  }
+  setObjectDebugVisualization();
+
   module_type_->updateSpecialData();
   module_type_->resetStopPose();
 }
@@ -98,8 +103,6 @@ BehaviorModuleOutput LaneChangeInterface::plan()
     return {};
   }
 
-  module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
-  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   auto output = module_type_->generateOutput();
   path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
   *prev_approved_path_ = getPreviousModuleOutput().path;
@@ -112,38 +115,36 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   }
 
   updateSteeringFactorPtr(output);
-  clearWaitingApproval();
+  if (module_type_->isAbortState()) {
+    waitApproval();
+    removeRTCStatus();
+    const auto candidate = planCandidate();
+    path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
+    updateRTCStatus(
+      candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change);
+  } else {
+    clearWaitingApproval();
+  }
 
   return output;
 }
 
 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;
-
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  module_type_->updateLaneChangeStatus();
-  setObjectDebugVisualization();
+  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);
 
   for (const auto & [uuid, data] : module_type_->getDebugData()) {
     const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
 
-  // change turn signal when the vehicle reaches at the end of the path for waiting lane change
-  out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
-
   path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
-
   stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {
@@ -211,6 +212,7 @@ bool LaneChangeInterface::canTransitSuccessState()
   }
 
   if (module_type_->hasFinishedLaneChange()) {
+    module_type_->resetParameters();
     log_debug_throttled("Lane change process has completed.");
     return true;
   }
@@ -468,16 +470,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;