fix(behavior_velocity_planner_common): sync activation when safety status is set (#5697)

* fix(behavior_velocity_planner_common): sync activation when safety status is set

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

* Update planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

* Update planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>

Author: 3 people

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -87,6 +87,7 @@ class SceneModuleInterface
   boost::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
 
   void setActivation(const bool activated) { activated_ = activated; }
+  void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; }
   bool isActivated() const { return activated_; }
   bool isSafe() const { return safe_; }
   double getDistance() const { return distance_; }
@@ -98,6 +99,7 @@ class SceneModuleInterface
   const int64_t module_id_;
   bool activated_;
   bool safe_;
+  bool rtc_enabled_;
   double distance_;
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
@@ -106,8 +108,15 @@ class SceneModuleInterface
   boost::optional<int> first_stop_path_point_index_;
   VelocityFactorInterface velocity_factor_;
 
-  void setSafe(const bool safe) { safe_ = safe; }
+  void setSafe(const bool safe)
+  {
+    safe_ = safe;
+    if (!rtc_enabled_) {
+      syncActivation();
+    }
+  }
   void setDistance(const double distance) { distance_ = distance; }
+  void syncActivation() { setActivation(isSafe()); }
 
   size_t findEgoSegmentIndex(
     const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points) const;

planning/behavior_velocity_planner_common/src/scene_module_interface.cpp

@@ -218,6 +218,7 @@ void SceneModuleManagerInterfaceWithRTC::setActivation()
   for (const auto & scene_module : scene_modules_) {
     const UUID uuid = getUUID(scene_module->getModuleId());
     scene_module->setActivation(rtc_interface_.isActivated(uuid));
+    scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid));
   }
 }
 

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -55,6 +55,7 @@ class RTCInterface
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid) const;
   bool isRegistered(const UUID & uuid) const;
+  bool isRTCEnabled(const UUID & uuid) const;
   void lockCommandUpdate();
   void unlockCommandUpdate();
 

planning/rtc_interface/src/rtc_interface.cpp

@@ -280,6 +280,22 @@ bool RTCInterface::isRegistered(const UUID & uuid) const
   return itr != registered_status_.statuses.end();
 }
 
+bool RTCInterface::isRTCEnabled(const UUID & uuid) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  const auto itr = std::find_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [uuid](auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    return !itr->auto_mode;
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl);
+  return is_auto_mode_init_;
+}
+
 void RTCInterface::lockCommandUpdate()
 {
   is_locked_ = true;