feat: add RTC interface (tier4#765)

* feature(rtc_interface): add files

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

* feature(rtc_interface): implement functions

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

* feature(rtc_interface): reimprement functions to use CooperateCommands and write README.md

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

* feature(rtc_interface): fix README

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

* feature(rtc_interface): add getModuleType()

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

* feature(rtc_interface): fix definition of constructor

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

* feature(rtc_interface): fix time stamp

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

* feature(rtc_interface): fix README

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

* feature(rtc_interface): add isRegistered and clearCooperateStatus

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

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/rtc_interface/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtc_interface)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(rtc_interface SHARED
+  src/rtc_interface.cpp
+)
+
+# Test
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

planning/rtc_interface/README.md

@@ -0,0 +1,187 @@
+# RTC Interface
+
+## Purpose
+
+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");
+
+// 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 = ...
+
+  // Get time stamp
+  const rclcpp::Time stamp = ...
+
+  // Update status
+  rtc_interface.updateCooperateStatus(uuid, safe, distance, stamp);
+
+  if (rtc_interface.isActivated(uuid)) {
+    // Execute planning
+  } else {
+    // Stop planning
+  }
+  // Get time stamp
+  const rclcpp::Time stamp = ...
+
+  // Publish status topic
+  rtc_interface.publishCooperateStatus(stamp);
+}
+
+// Remove the status from array
+rtc_interface.removeCooperateStatus(uuid);
+```
+
+## Inputs / Outputs
+
+### RTCInterface (Constructor)
+
+```c++
+rtc_interface::RTCInterface(rclcpp::Node & node, const std::string & name);
+```
+
+#### 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`
+
+#### Output
+
+An instance of `RTCInterface`
+
+### publishCooperateStatus
+
+```c++
+rtc_interface::publishCooperateStatus(const rclcpp::Time & stamp)
+```
+
+#### Description
+
+Publish registered cooperate status.
+
+#### Input
+
+- `stamp` : Time stamp
+
+#### Output
+
+Nothing
+
+### updateCooperateStatus
+
+```c++
+rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp)
+```
+
+#### 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
+- `stamp` : Time stamp
+
+#### 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 expired module
+
+#### Output
+
+Nothing
+
+### clearCooperateStatus
+
+```c++
+rtc_interface::clearCooperateStatus()
+```
+
+#### Description
+
+Remove all cooperate statuses.
+
+#### Input
+
+Nothing
+
+#### 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`.
+
+### isRegistered
+
+```c++
+rtc_interface::isRegistered(const unique_identifier_msgs::msg::UUID & uuid)
+```
+
+#### Description
+
+Return `true` if `uuid` is registered.
+
+#### Input
+
+- `uuid` : UUID for checking module
+
+#### Output
+
+If `uuid` is registered, return `true`.
+If not, return `false`.
+
+## Assumptions / Known limits
+
+## Future extensions / Unimplemented parts

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

@@ -0,0 +1,71 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RTC_INTERFACE__RTC_INTERFACE_HPP_
+#define RTC_INTERFACE__RTC_INTERFACE_HPP_
+
+#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_commands.hpp"
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <string>
+#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::CooperateCommands;
+using unique_identifier_msgs::msg::UUID;
+
+class RTCInterface
+{
+public:
+  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;
+
+private:
+  void onCooperateCommandService(
+    const CooperateCommands::Request::SharedPtr request,
+    const CooperateCommands::Response::SharedPtr responses);
+  rclcpp::Logger getLogger() const;
+
+  rclcpp::Publisher<CooperateStatusArray>::SharedPtr pub_statuses_;
+  rclcpp::Service<CooperateCommands>::SharedPtr srv_commands_;
+
+  rclcpp::Logger logger_;
+  Module module_;
+  CooperateStatusArray registered_status_;
+};
+
+}  // namespace rtc_interface
+
+#endif  // RTC_INTERFACE__RTC_INTERFACE_HPP_

planning/rtc_interface/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rtc_interface</name>
+  <version>0.1.0</version>
+  <description>The rtc_interface package</description>
+
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>tier4_rtc_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>