feat(rtc_interface, lane_change): check state transition for cooperat…

…e status (#8855) * update rtc state transition Signed-off-by: Go Sakayori <[email protected]> * remove transition from failuer and succeeded Signed-off-by: Go Sakayori <[email protected]> * fix Signed-off-by: Go Sakayori <[email protected]> * check initial state for cooperate status Signed-off-by: Go Sakayori <[email protected]> * change rtc cooperate status according to module status Signed-off-by: Go Sakayori <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]>
autowarefoundation · Sep 12, 2024 · 9cc5ef1 · 9cc5ef1
1 parent afcef77
commit 9cc5ef1
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 10 deletions.
diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp
@@ -258,20 +258,51 @@ void RTCInterface::updateCooperateStatus(
     status.module = module_;
     status.safe = safe;
     status.command_status.type = Command::DEACTIVATE;
-    status.state.type = state;
+    status.state.type = State::WAITING_FOR_EXECUTION;
     status.start_distance = start_distance;
     status.finish_distance = finish_distance;
     status.auto_mode = is_auto_mode_enabled_;
     registered_status_.statuses.push_back(status);
+
+    if (state != State::WAITING_FOR_EXECUTION)
+      RCLCPP_WARN_STREAM(
+        getLogger(), "[updateCooperateStatus]  Cannot register "
+                       << state_to_string(state) << " as initial state" << std::endl);
+
+    return;
+  }
+
+  auto update_status = [&](auto & status) {
+    status.stamp = stamp;
+    status.safe = safe;
+    status.state.type = state;
+    status.start_distance = start_distance;
+    status.finish_distance = finish_distance;
+  };
+
+  // If the registered status is found, update status by considering the state transition
+  if (
+    itr->state.type == State::WAITING_FOR_EXECUTION &&
+    (state == State::WAITING_FOR_EXECUTION || state == State::RUNNING || state == State::FAILED)) {
+    update_status(*itr);
+    return;
+  }
+
+  if (itr->state.type == State::RUNNING && state != State::WAITING_FOR_EXECUTION) {
+    update_status(*itr);
     return;
   }
 
-  // If the registered status is found, update status
-  itr->stamp = stamp;
-  itr->safe = safe;
-  itr->state.type = state;
-  itr->start_distance = start_distance;
-  itr->finish_distance = finish_distance;
+  if (itr->state.type == State::ABORTING && (state == State::ABORTING || state == State::FAILED)) {
+    update_status(*itr);
+    return;
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(), "[updateCooperateStatus] uuid : " << uuid_to_string(uuid)
+                                                   << " cannot transit from "
+                                                   << state_to_string(itr->state.type) << " to "
+                                                   << state_to_string(state) << std::endl);
 }
 
 void RTCInterface::removeCooperateStatus(const UUID & uuid)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
@@ -160,9 +160,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {
-    updateRTCStatus(
-      std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), false,
-      State::FAILED);
     path_candidate_ = std::make_shared<PathWithLaneId>();
     return out;
   }