fix(behavior_velocity_planner_common): sync activation when safety st…

…atus is set (autowarefoundation#5697) * fix(behavior_velocity_planner_common): sync activation when safety status is set Signed-off-by: Fumiya Watanabe <[email protected]> * Update planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp Co-authored-by: Kyoichi Sugahara <[email protected]> * Update planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp Co-authored-by: Tomoya Kimura <[email protected]> --------- Signed-off-by: Fumiya Watanabe <[email protected]> Co-authored-by: Kyoichi Sugahara <[email protected]> Co-authored-by: Tomoya Kimura <[email protected]>
YoshiRi · Nov 29, 2023 · bea5454 · bea5454
1 parent 203e97c
commit bea5454
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 1 deletion.
diff --git a/...locity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/...locity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp
@@ -101,6 +101,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_; }
@@ -112,6 +113,7 @@ class SceneModuleInterface
   const int64_t module_id_;
   bool activated_;
   bool safe_;
+  bool rtc_enabled_;
   double distance_;
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
@@ -120,8 +122,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()); }
 
   template <class T>
   size_t findEgoSegmentIndex(const std::vector<T> & points) const
@@ -357,6 +366,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
     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));
     }
   }
 

diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/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();
 

diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/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;