feat(lane_change): apply new RTC state (#7152)

* feat(lane_change): support for new RTC state transition

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

* fix: distance at aborting

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

* fix(lane_change): empty check

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

* fix(rtc_interface):update activation depends on RTC state

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

---------

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

Author: rej55

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp

@@ -125,6 +125,18 @@ class LaneChangeInterface : public SceneModuleInterface
 
   ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; };
 
+  void updateRTCStatus(
+    const double start_distance, const double finish_distance, const bool safe,
+    const uint8_t & state)
+  {
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      if (ptr) {
+        ptr->updateCooperateStatus(
+          uuid_map_.at(module_name), safe, state, start_distance, finish_distance, clock_->now());
+      }
+    }
+  }
+
   void updateDebugMarker() const;
 
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -117,13 +117,18 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   updateSteeringFactorPtr(output);
   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);
+      std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+      State::ABORTING);
   } else {
     clearWaitingApproval();
+    const auto path =
+      assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition());
+    updateRTCStatus(
+      path.start_distance_to_path_change, path.finish_distance_to_path_change, true,
+      State::RUNNING);
   }
 
   return output;
@@ -147,7 +152,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {
-    removeRTCStatus();
+    updateRTCStatus(
+      std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+      State::FAILED);
     path_candidate_ = std::make_shared<PathWithLaneId>();
     return out;
   }
@@ -156,7 +163,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
 
   updateRTCStatus(
-    candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change);
+    candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
+    isExecutionReady(), State::WAITING_FOR_EXECUTION);
   is_abort_path_approved_ = false;
 
   return out;
@@ -239,6 +247,9 @@ bool LaneChangeInterface::canTransitFailureState()
     if (module_type_->isStoppedAtRedTrafficLight()) {
       log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change");
       module_type_->toCancelState();
+      updateRTCStatus(
+        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+        State::FAILED);
       return true;
     }
 
@@ -249,6 +260,9 @@ bool LaneChangeInterface::canTransitFailureState()
 
     if (module_type_->isAbleToReturnCurrentLane()) {
       log_debug_throttled("It's possible to return to current lane. Cancel lane change.");
+      updateRTCStatus(
+        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+        State::FAILED);
       return true;
     }
   }

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -646,8 +646,14 @@ std::vector<DrivableLanes> generateDrivableLanes(
 
 double getLateralShift(const LaneChangePath & path)
 {
-  const auto start_idx = path.info.shift_line.start_idx;
-  const auto end_idx = path.info.shift_line.end_idx;
+  if (path.shifted_path.shift_length.empty()) {
+    return 0.0;
+  }
+
+  const auto start_idx =
+    std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1);
+  const auto end_idx =
+    std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1);
 
   return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx);
 }

planning/rtc_interface/src/rtc_interface.cpp

@@ -153,7 +153,16 @@ std::vector<CooperateResponse> RTCInterface::validateCooperateCommands(
       registered_status_.statuses.begin(), registered_status_.statuses.end(),
       [command](auto & s) { return s.uuid == command.uuid; });
     if (itr != registered_status_.statuses.end()) {
-      response.success = true;
+      if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) {
+        response.success = true;
+      } else {
+        RCLCPP_WARN_STREAM(
+          getLogger(), "[validateCooperateCommands] uuid : "
+                         << to_string(command.uuid)
+                         << " state is not WAITING_FOR_EXECUTION or RUNNING. state : "
+                         << itr->state.type << std::endl);
+        response.success = false;
+      }
     } else {
       RCLCPP_WARN_STREAM(
         getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid)
@@ -175,8 +184,10 @@ void RTCInterface::updateCooperateCommandStatus(const std::vector<CooperateComma
 
     // Update command if the command has been already received
     if (itr != registered_status_.statuses.end()) {
-      itr->command_status = command.command;
-      itr->auto_mode = false;
+      if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) {
+        itr->command_status = command.command;
+        itr->auto_mode = false;
+      }
     }
   }
 }
@@ -219,7 +230,7 @@ void RTCInterface::updateCooperateStatus(
     status.module = module_;
     status.safe = safe;
     status.command_status.type = Command::DEACTIVATE;
-    status.state.type = State::WAITING_FOR_EXECUTION;
+    status.state.type = state;
     status.start_distance = start_distance;
     status.finish_distance = finish_distance;
     status.auto_mode = is_auto_mode_enabled_;
@@ -291,6 +302,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const
     [uuid](auto & s) { return s.uuid == uuid; });
 
   if (itr != registered_status_.statuses.end()) {
+    if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) {
+      return false;
+    }
     if (itr->auto_mode) {
       return itr->safe;
     } else {