feat(rtc_interface): apply multithread for rtc_interface (#1275)

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

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -59,7 +59,7 @@ class AvoidanceModule : public SceneModuleInterface
     rtc_interface_right_.publishCooperateStatus(clock_->now());
   }
 
-  bool isActivated() const override
+  bool isActivated() override
   {
     if (rtc_interface_left_.isRegistered(uuid_left_)) {
       return rtc_interface_left_.isActivated(uuid_left_);

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -98,7 +98,7 @@ class LaneChangeModule : public SceneModuleInterface
     rtc_interface_right_.publishCooperateStatus(clock_->now());
   }
 
-  bool isActivated() const override
+  bool isActivated() override
   {
     if (rtc_interface_left_.isRegistered(uuid_left_)) {
       return rtc_interface_left_.isActivated(uuid_left_);
@@ -124,6 +124,8 @@ class LaneChangeModule : public SceneModuleInterface
   UUID uuid_left_;
   UUID uuid_right_;
 
+  bool is_activated_ = false;
+
   void waitApprovalLeft(const double distance)
   {
     rtc_interface_left_.updateCooperateStatus(

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -203,7 +203,7 @@ class SceneModuleInterface
   /**
    * @brief Return true if the activation command is received
    */
-  virtual bool isActivated() const
+  virtual bool isActivated()
   {
     if (!rtc_interface_ptr_) {
       return true;
@@ -270,7 +270,7 @@ class SceneModuleInterface
 
   void clearWaitingApproval() { is_waiting_approval_ = false; }
 
-  UUID generateUUID()
+  static UUID generateUUID()
   {
     // Generate random number
     UUID uuid;

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -50,8 +50,8 @@ AvoidanceModule::AvoidanceModule(
   const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters)
 : SceneModuleInterface{name, node},
   parameters_{parameters},
-  rtc_interface_left_(node, "avoidance_left"),
-  rtc_interface_right_(node, "avoidance_right"),
+  rtc_interface_left_(&node, "avoidance_left"),
+  rtc_interface_right_(&node, "avoidance_right"),
   uuid_left_{generateUUID()},
   uuid_right_{generateUUID()}
 {

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -39,8 +39,8 @@ LaneChangeModule::LaneChangeModule(
   const std::string & name, rclcpp::Node & node, const LaneChangeParameters & parameters)
 : SceneModuleInterface{name, node},
   parameters_{parameters},
-  rtc_interface_left_(node, "lane_change_left"),
-  rtc_interface_right_(node, "lane_change_right"),
+  rtc_interface_left_(&node, "lane_change_left"),
+  rtc_interface_right_(&node, "lane_change_right"),
   uuid_left_{generateUUID()},
   uuid_right_{generateUUID()}
 {
@@ -50,6 +50,7 @@ BehaviorModuleOutput LaneChangeModule::run()
 {
   RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan().");
   current_state_ = BT::NodeStatus::RUNNING;
+  is_activated_ = isActivated();
   const auto output = plan();
   const auto turn_signal_info = output.turn_signal_info;
   if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
@@ -475,7 +476,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const
     return false;
   }
 
-  if (!isActivated()) {
+  if (!is_activated_) {
     return false;
   }
 

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -40,7 +40,7 @@ PullOutModule::PullOutModule(
   const std::string & name, rclcpp::Node & node, const PullOutParameters & parameters)
 : SceneModuleInterface{name, node}, parameters_{parameters}
 {
-  rtc_interface_ptr_ = std::make_shared<RTCInterface>(node, "pull_out");
+  rtc_interface_ptr_ = std::make_shared<RTCInterface>(&node, "pull_out");
 }
 
 BehaviorModuleOutput PullOutModule::run()

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -39,7 +39,7 @@ PullOverModule::PullOverModule(
   const std::string & name, rclcpp::Node & node, const PullOverParameters & parameters)
 : SceneModuleInterface{name, node}, parameters_{parameters}
 {
-  rtc_interface_ptr_ = std::make_shared<RTCInterface>(node, "pull_over");
+  rtc_interface_ptr_ = std::make_shared<RTCInterface>(&node, "pull_over");
 }
 
 BehaviorModuleOutput PullOverModule::run()

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -269,7 +269,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
 {
 public:
   SceneModuleManagerInterfaceWithRTC(rclcpp::Node & node, const char * module_name)
-  : SceneModuleManagerInterface(node, module_name), rtc_interface_(node, module_name)
+  : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name)
   {
   }
 

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -26,6 +26,7 @@
 #include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -43,14 +44,14 @@ using unique_identifier_msgs::msg::UUID;
 class RTCInterface
 {
 public:
-  RTCInterface(rclcpp::Node & node, const std::string & name);
+  RTCInterface(rclcpp::Node * node, const std::string & name);
   void publishCooperateStatus(const rclcpp::Time & stamp);
   void updateCooperateStatus(
     const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp);
   void removeCooperateStatus(const UUID & uuid);
   void clearCooperateStatus();
-  bool isActivated(const UUID & uuid) const;
-  bool isRegistered(const UUID & uuid) const;
+  bool isActivated(const UUID & uuid);
+  bool isRegistered(const UUID & uuid);
 
 private:
   void onCooperateCommandService(
@@ -61,6 +62,8 @@ class RTCInterface
   rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_statuses_;
   rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
 
+  std::mutex mutex_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::Logger logger_;
   Module module_;
   CooperateStatusArray registered_status_;

planning/rtc_interface/src/rtc_interface.cpp

@@ -68,26 +68,30 @@ Module getModuleType(const std::string & module_name)
 
 namespace rtc_interface
 {
-RTCInterface::RTCInterface(rclcpp::Node & node, const std::string & name)
-: logger_{node.get_logger().get_child("RTCInterface[" + name + "]")}
+RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name)
+: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
 
   // Publisher
-  pub_statuses_ = node.create_publisher<CooperateStatusArray>("~/" + name + "/cooperate_status", 1);
+  pub_statuses_ =
+    node->create_publisher<CooperateStatusArray>("~/" + name + "/cooperate_status", 1);
 
   // Service
-  srv_commands_ = node.create_service<CooperateCommands>(
+  callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  srv_commands_ = node->create_service<CooperateCommands>(
     "~/" + name + "/cooperate_commands",
-    std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2));
+    std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2),
+    rmw_qos_profile_services_default, callback_group_);
 
   // Module
   module_ = getModuleType(name);
 }
 
 void RTCInterface::publishCooperateStatus(const rclcpp::Time & stamp)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   registered_status_.stamp = stamp;
   pub_statuses_->publish(registered_status_);
 }
@@ -96,6 +100,7 @@ void RTCInterface::onCooperateCommandService(
   const CooperateCommands::Request::SharedPtr request,
   const CooperateCommands::Response::SharedPtr responses)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   for (const auto & command : request->commands) {
     CooperateResponse response;
     response.uuid = command.uuid;
@@ -122,6 +127,7 @@ void RTCInterface::onCooperateCommandService(
 void RTCInterface::updateCooperateStatus(
   const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   // Find registered status which has same uuid
   auto itr = std::find_if(
     registered_status_.statuses.begin(), registered_status_.statuses.end(),
@@ -148,6 +154,7 @@ void RTCInterface::updateCooperateStatus(
 
 void RTCInterface::removeCooperateStatus(const UUID & uuid)
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   // Find registered status which has same uuid and erase it
   const auto itr = std::find_if(
     registered_status_.statuses.begin(), registered_status_.statuses.end(),
@@ -163,10 +170,15 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid)
     "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl);
 }
 
-void RTCInterface::clearCooperateStatus() { registered_status_.statuses.clear(); }
+void RTCInterface::clearCooperateStatus()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  registered_status_.statuses.clear();
+}
 
-bool RTCInterface::isActivated(const UUID & uuid) const
+bool RTCInterface::isActivated(const UUID & uuid)
 {
+  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; });
@@ -180,8 +192,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const
   return false;
 }
 
-bool RTCInterface::isRegistered(const UUID & uuid) const
+bool RTCInterface::isRegistered(const UUID & uuid)
 {
+  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; });