feature(rtc_interface): reimprement functions to use CooperateCommand…

…s and write README.md

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

planning/rtc_interface/README.md

@@ -2,12 +2,144 @@
 
 ## Purpose
 
-RTC Interface is an interface to receive execution command for planning from external of an autonomous driving system.
+RTC Interface is an interface to publish the decision status of behavior planning modules and receive execution command from external of an autonomous driving system.
 
 ## Inner-workings / Algorithms
 
+### Usage example
+
+```c++
+// Generate instance (in this example, "intersection" is selected)
+rtc_interface::RTCInterface rtc_interface(node, "intersection", tier4_rtc_msgs::msg::Module::INTERSECTION);
+
+// Generate UUID
+const unique_identifier_msgs::msg::UUID uuid = generateUUID(getModuleId());
+
+// Repeat while module is running
+while (...) {
+  // Get safety status of the module corresponding to the module id
+  const bool safe = ...
+
+  // Get distance to the object corresponding to the module id
+  const double distance = ...
+
+  // Update status
+  rtc_interface.updateCooperateStatus(uuid, safe, distance);
+
+  if (rtc_interface.isActivated()) {
+    // Execute planning
+  } else {
+    // Stop planning
+  }
+
+  // Publish status topic
+  rtc_interface.publishCooperateStatus();
+}
+
+// Remove the status from array
+rtc_interface.removeCooperateStatus(uuid);
+```
+
 ## Inputs / Outputs
 
+### RTCInterface (Constructor)
+
+```c++
+rtc_interface::RTCInterface(rclcpp::Node & node, const std::string & name, const tier4_rtc_msgs::msg::Module & module);
+```
+
+#### Description
+
+A constructor for `rtc_interface::RTCInterface`.
+
+#### Input
+
+- `node` : Node calling this interface
+- `name` : Name of cooperate status array topic and cooperate commands service
+  - Cooperate status array topic name : `~/{name}/cooperate_status`
+  - Cooperate commands service name : `~/{name}/cooperate_commands`
+- `module` : Module id defined in `tier4_rtc_msgs/Module`
+
+#### Output
+
+An instance of `RTCInterface`
+
+### publishCooperateStatus
+
+```c++
+rtc_interface::publishCooperateStatus()
+```
+
+#### Description
+
+Publish registered cooperate status.
+
+#### Input
+
+Nothing
+
+#### Output
+
+Nothing
+
+### updateCooperateStatus
+
+```c++
+rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double distance)
+```
+
+#### Description
+
+Update cooperate status corresponding to `uuid`.
+If cooperate status corresponding to `uuid` is not registered yet, add new cooperate status.
+
+#### Input
+
+- `uuid` : UUID for requesting module
+- `safe` : Safety status of requesting module
+- `distance` : Distance to the object from ego vehicle
+
+#### Output
+
+Nothing
+
+### removeCooperateStatus
+
+```c++
+rtc_interface::removeCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid)
+```
+
+#### Description
+
+Remove cooperate status corresponding to `uuid` from registered statuses.
+
+#### Input
+
+- `uuid` : UUID for unregistering module
+
+#### Output
+
+Nothing
+
+### isActivated
+
+```c++
+rtc_interface::isActivated(const unique_identifier_msgs::msg::UUID & uuid)
+```
+
+#### Description
+
+Return received command status corresponding to `uuid`.
+
+#### Input
+
+- `uuid` : UUID for checking module
+
+#### Output
+
+If received command is `ACTIVATED`, return `true`.
+If not, return `false`.
+
 ## Assumptions / Known limits
 
 ## Future extensions / Unimplemented parts

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -18,23 +18,26 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "tier4_rtc_msgs/msg/command.hpp"
+#include "tier4_rtc_msgs/msg/cooperate_command.hpp"
+#include "tier4_rtc_msgs/msg/cooperate_response.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status.hpp"
 #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
 #include "tier4_rtc_msgs/msg/module.hpp"
-#include "tier4_rtc_msgs/srv/cooperate_command.hpp"
+#include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <string>
-#include <unordered_map>
 #include <vector>
 
 namespace rtc_interface
 {
 using tier4_rtc_msgs::msg::Command;
+using tier4_rtc_msgs::msg::CooperateCommand;
+using tier4_rtc_msgs::msg::CooperateResponse;
 using tier4_rtc_msgs::msg::CooperateStatus;
 using tier4_rtc_msgs::msg::CooperateStatusArray;
 using tier4_rtc_msgs::msg::Module;
-using tier4_rtc_msgs::srv::CooperateCommand;
+using tier4_rtc_msgs::srv::CooperateCommands;
 using unique_identifier_msgs::msg::UUID;
 
 class RTCInterface
@@ -48,13 +51,15 @@ class RTCInterface
 
 private:
   void onCooperateCommandService(
-    const CooperateCommand::Request::SharedPtr request,
-    const CooperateCommand::Response::SharedPtr response);
+    const CooperateCommands::Request::SharedPtr request,
+    const CooperateCommands::Response::SharedPtr responses);
+  rclcpp::Logger getLogger() const;
 
-  rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_status_;
-  rclcpp::Service<CooperateCommand>::SharedPtr srv_command_;
+  rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_statuses_;
+  rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
 
   mutable rclcpp::Clock clock_;
+  rclcpp::Logger logger_;
   Module module_;
   CooperateStatusArray registered_status_;
 };

planning/rtc_interface/src/rtc_interface.cpp

@@ -14,47 +14,68 @@
 
 #include "rtc_interface/rtc_interface.hpp"
 
+namespace
+{
+std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
+{
+  std::stringstream ss;
+  for (auto i = 0; i < 16; ++i) {
+    ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i];
+  }
+  return ss.str();
+}
+}  // namespace
+
 namespace rtc_interface
 {
 RTCInterface::RTCInterface(rclcpp::Node & node, const std::string & name, const Module & module)
-: clock_{*node.get_clock()}, module_(module)
+: clock_{*node.get_clock()},
+  logger_{node.get_logger().get_child("RTCInterface[" + name + "]")},
+  module_{module}
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
 
   // Publisher
-  pub_status_ = node.create_publisher<CooperateStatusArray>(name + "/status", 1);
+  pub_statuses_ = node.create_publisher<CooperateStatusArray>("~/" + name + "/cooperate_status", 1);
 
   // Service
-  srv_command_ = node.create_service<CooperateCommand>(
-    name + "/command", std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2));
+  srv_commands_ = node.create_service<CooperateCommands>(
+    "~/" + name + "/cooperate_commands",
+    std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2));
 }
 
 void RTCInterface::publishCooperateStatus()
 {
   registered_status_.stamp = clock_.now();
-  pub_status_->publish(registered_status_);
+  pub_statuses_->publish(registered_status_);
 }
 
 void RTCInterface::onCooperateCommandService(
-  const CooperateCommand::Request::SharedPtr request,
-  const CooperateCommand::Response::SharedPtr response)
+  const CooperateCommands::Request::SharedPtr request,
+  const CooperateCommands::Response::SharedPtr responses)
 {
-  const auto itr = std::find_if(
-    registered_status_.statuses.begin(), registered_status_.statuses.end(),
-    [request](auto & s) { return s.uuid == request->uuid; });
-
-  // Update command if the command has been already received
-  if (itr != registered_status_.statuses.end()) {
-    itr->command_status = request->command;
-    response->success = true;
-    return;
+  for (const auto & command : request->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;
+      response.success = true;
+    } else {
+      RCLCPP_WARN_STREAM(
+        getLogger(), "[onCooperateCommandService] uuid : " << to_string(command.uuid)
+                                                           << " is not found." << std::endl);
+      response.success = false;
+    }
+    responses->responses.push_back(response);
   }
-
-  // Status with same uuid is not registered
-  response->success = false;
-
-  return;
 }
 
 void RTCInterface::updateCooperateStatus(const UUID & uuid, const bool safe, const double distance)
@@ -64,7 +85,7 @@ void RTCInterface::updateCooperateStatus(const UUID & uuid, const bool safe, con
     registered_status_.statuses.begin(), registered_status_.statuses.end(),
     [uuid](auto & s) { return s.uuid == uuid; });
 
-  // If there is no registered status, add it.
+  // If there is no registered status, add it
   if (itr == registered_status_.statuses.end()) {
     CooperateStatus status;
     status.stamp = clock_.now();
@@ -95,7 +116,9 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid)
     return;
   }
 
-  std::cout << "Something wrong" << std::endl;
+  RCLCPP_WARN_STREAM(
+    getLogger(),
+    "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl);
 }
 
 bool RTCInterface::isActivated(const UUID & uuid) const
@@ -108,8 +131,11 @@ bool RTCInterface::isActivated(const UUID & uuid) const
     return itr->command_status.type == Command::ACTIVATE;
   }
 
-  std::cout << "Something wrong" << std::endl;
+  RCLCPP_WARN_STREAM(
+    getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl);
   return false;
 }
 
+rclcpp::Logger RTCInterface::getLogger() const { return logger_; }
+
 }  // namespace rtc_interface