feat(rtc_interface): lock command update (#2073)

* feat(rtc_interface): lock command update

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

* feat(behavior_path_planner): add lock functions

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

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

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

@@ -67,6 +67,18 @@ class AvoidanceModule : public SceneModuleInterface
     return false;
   }
 
+  void lockRTCCommand() override
+  {
+    rtc_interface_left_.lockCommandUpdate();
+    rtc_interface_right_.lockCommandUpdate();
+  }
+
+  void unlockRTCCommand() override
+  {
+    rtc_interface_left_.unlockCommandUpdate();
+    rtc_interface_right_.unlockCommandUpdate();
+  }
+
 private:
   struct RegisteredShiftLine
   {

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

@@ -73,6 +73,18 @@ class LaneChangeModule : public SceneModuleInterface
     return false;
   }
 
+  void lockRTCCommand() override
+  {
+    rtc_interface_left_.lockCommandUpdate();
+    rtc_interface_right_.lockCommandUpdate();
+  }
+
+  void unlockRTCCommand() override
+  {
+    rtc_interface_left_.unlockCommandUpdate();
+    rtc_interface_right_.unlockCommandUpdate();
+  }
+
 private:
   std::shared_ptr<LaneChangeParameters> parameters_;
   LaneChangeStatus status_;

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

@@ -234,6 +234,22 @@ class SceneModuleInterface
   }
   bool isWaitingApproval() const { return is_waiting_approval_; }
 
+  virtual void lockRTCCommand()
+  {
+    if (!rtc_interface_ptr_) {
+      return;
+    }
+    rtc_interface_ptr_->lockCommandUpdate();
+  }
+
+  virtual void unlockRTCCommand()
+  {
+    if (!rtc_interface_ptr_) {
+      return;
+    }
+    rtc_interface_ptr_->unlockCommandUpdate();
+  }
+
 private:
   std::string name_;
   rclcpp::Logger logger_;

planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp

@@ -51,6 +51,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
 
   const bool is_waiting_approval = !scene_module_->isActivated();
   if (is_waiting_approval && !is_lane_following) {
+    scene_module_->lockRTCCommand();
     try {
       // NOTE: Since BehaviorTreeCpp has an issue to shadow the exception reason thrown
       // in the TreeNode, catch and display it here until the issue is fixed.
@@ -66,12 +67,14 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
         scene_module_->getLogger(), "behavior module has failed with exception: " << e.what());
       // std::exit(EXIT_FAILURE);  // TODO(Horibe) do appropriate handing
     }
+    scene_module_->unlockRTCCommand();
     return BT::NodeStatus::SUCCESS;
   }
 
   while (rclcpp::ok()) {
     // NOTE: Since BehaviorTreeCpp has an issue to shadow the exception reason thrown
     // in the TreeNode, catch and display it here until the issue is fixed.
+    scene_module_->lockRTCCommand();
     try {
       auto res = setOutput<BehaviorModuleOutput>("output", scene_module_->run());
       if (!res) {
@@ -97,6 +100,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
       break;
     }
 
+    scene_module_->unlockRTCCommand();
     setStatusRunningAndYield();
   }
 

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -55,14 +55,20 @@ class RTCInterface
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid);
   bool isRegistered(const UUID & uuid);
+  void lockCommandUpdate();
+  void unlockCommandUpdate();
 
 private:
   void onCooperateCommandService(
     const CooperateCommands::Request::SharedPtr request,
     const CooperateCommands::Response::SharedPtr responses);
   void onAutoModeService(
     const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response);
+  std::vector<CooperateResponse> validateCooperateCommands(
+    const std::vector<CooperateCommand> & commands);
+  void updateCooperateCommandStatus(const std::vector<CooperateCommand> & commands);
   rclcpp::Logger getLogger() const;
+  bool isLocked() const;
 
   rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_statuses_;
   rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
@@ -73,7 +79,9 @@ class RTCInterface
   rclcpp::Logger logger_;
   Module module_;
   CooperateStatusArray registered_status_;
+  std::vector<CooperateCommand> stored_commands_;
   bool is_auto_mode_;
+  bool is_locked_;
 
   std::string cooperate_status_namespace_ = "/planning/cooperate_status";
   std::string cooperate_commands_namespace_ = "/planning/cooperate_commands";

planning/rtc_interface/src/rtc_interface.cpp

@@ -69,7 +69,9 @@ 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 + "]")}, is_auto_mode_{false}
+: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
+  is_auto_mode_{false},
+  is_locked_{false}
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -105,28 +107,57 @@ void RTCInterface::onCooperateCommandService(
   const CooperateCommands::Response::SharedPtr responses)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  for (const auto & command : request->commands) {
+
+  responses->responses = validateCooperateCommands(request->commands);
+
+  if (isLocked()) {
+    stored_commands_ = request->commands;
+    return;
+  }
+
+  updateCooperateCommandStatus(request->commands);
+}
+
+std::vector<CooperateResponse> RTCInterface::validateCooperateCommands(
+  const std::vector<CooperateCommand> & commands)
+{
+  std::vector<CooperateResponse> responses;
+
+  for (const auto & command : commands) {
     CooperateResponse response;
     response.uuid = command.uuid;
     response.module = command.module;
 
     const auto itr = std::find_if(
       registered_status_.statuses.begin(), registered_status_.statuses.end(),
       [command](auto & s) { return s.uuid == command.uuid; });
-
-    // Update command if the command has been already received
     if (itr != registered_status_.statuses.end()) {
-      itr->command_status = command.command;
-      itr->auto_mode = false;
-      is_auto_mode_ = false;
       response.success = true;
     } else {
       RCLCPP_WARN_STREAM(
-        getLogger(), "[onCooperateCommandService] uuid : " << to_string(command.uuid)
+        getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid)
                                                            << " is not found." << std::endl);
       response.success = false;
     }
-    responses->responses.push_back(response);
+    responses.push_back(response);
+  }
+
+  return responses;
+}
+
+void RTCInterface::updateCooperateCommandStatus(const std::vector<CooperateCommand> & commands)
+{
+  for (const auto & command : commands) {
+    const auto itr = std::find_if(
+      registered_status_.statuses.begin(), registered_status_.statuses.end(),
+      [command](auto & s) { return s.uuid == command.uuid; });
+
+    // Update command if the command has been already received
+    if (itr != registered_status_.statuses.end()) {
+      itr->command_status = command.command;
+      itr->auto_mode = false;
+      is_auto_mode_ = false;
+    }
   }
 }
 
@@ -227,6 +258,16 @@ bool RTCInterface::isRegistered(const UUID & uuid)
   return itr != registered_status_.statuses.end();
 }
 
+void RTCInterface::lockCommandUpdate() { is_locked_ = true; }
+
+void RTCInterface::unlockCommandUpdate()
+{
+  is_locked_ = false;
+  updateCooperateCommandStatus(stored_commands_);
+}
+
 rclcpp::Logger RTCInterface::getLogger() const { return logger_; }
 
+bool RTCInterface::isLocked() const { return is_locked_; }
+
 }  // namespace rtc_interface