fix(lane_change): set initail rtc state properly (autowarefoundation#…

…8902) set initail rtc state properly Signed-off-by: Go Sakayori <[email protected]>
tier4 · Sep 19, 2024 · 3c230eb · 3c230eb
1 parent f675b2d
commit 3c230eb
Showing 1 changed file with 10 additions and 5 deletions.
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
@@ -125,16 +125,21 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   } else {
     const auto path =
       assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition());
-    const auto force_activated = std::any_of(
+    const auto is_registered = std::any_of(
       rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
-      [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
-    if (!force_activated) {
+      [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)); });
+
+    if (!is_registered) {
       updateRTCStatus(
         path.start_distance_to_path_change, path.finish_distance_to_path_change, true,
-        State::RUNNING);
+        State::WAITING_FOR_EXECUTION);
     } else {
+      const auto force_activated = std::any_of(
+        rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
+        [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
+      const bool safe = force_activated ? false : true;
       updateRTCStatus(
-        path.start_distance_to_path_change, path.finish_distance_to_path_change, false,
+        path.start_distance_to_path_change, path.finish_distance_to_path_change, safe,
         State::RUNNING);
     }
   }