Merge pull request #1047 from tier4/fix/set_activation

fix(behavior_velocity_planner_common): sync activation when safety status is set
tier4 · Nov 29, 2023 · 67d4c14 · 67d4c14
2 parents 192d72d + bea5454
commit 67d4c14
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 7 deletions.
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
@@ -53,8 +53,9 @@ class RTCInterface
     const rclcpp::Time & stamp);
   void removeCooperateStatus(const UUID & uuid);
   void clearCooperateStatus();
-  bool isActivated(const UUID & uuid);
-  bool isRegistered(const UUID & uuid);
+  bool isActivated(const UUID & uuid) const;
+  bool isRegistered(const UUID & uuid) const;
+  bool isRTCEnabled(const UUID & uuid) const;
   void lockCommandUpdate();
   void unlockCommandUpdate();
 
@@ -74,10 +75,9 @@ class RTCInterface
   rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_statuses_;
   rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
   rclcpp::Service<AutoMode>::SharedPtr srv_auto_mode_;
-
-  std::mutex mutex_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::Logger logger_;
+
   Module module_;
   CooperateStatusArray registered_status_;
   std::vector<CooperateCommand> stored_commands_;
@@ -87,6 +87,8 @@ class RTCInterface
   std::string cooperate_status_namespace_ = "/planning/cooperate_status";
   std::string cooperate_commands_namespace_ = "/planning/cooperate_commands";
   std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal";
+
+  mutable std::mutex mutex_;
 };
 
 }  // namespace rtc_interface

diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp
@@ -251,7 +251,7 @@ void RTCInterface::clearCooperateStatus()
   stored_commands_.clear();
 }
 
-bool RTCInterface::isActivated(const UUID & uuid)
+bool RTCInterface::isActivated(const UUID & uuid) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
   const auto itr = std::find_if(
@@ -271,7 +271,7 @@ bool RTCInterface::isActivated(const UUID & uuid)
   return false;
 }
 
-bool RTCInterface::isRegistered(const UUID & uuid)
+bool RTCInterface::isRegistered(const UUID & uuid) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
   const auto itr = std::find_if(
@@ -280,6 +280,22 @@ bool RTCInterface::isRegistered(const UUID & uuid)
   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;