feat(rtc_reaplyer): add rtc_replayer

planning/rtc_replayer/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtc_replayer)
+
+### 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(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "rtc_replayer::RTCReplayerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

planning/rtc_replayer/README.md

@@ -0,0 +1,31 @@
+# rtc_replayer
+
+## Purpose
+
+This package is to handler rtc commands.
+
+## Inputs / Outputs
+
+### Input
+
+tbd.
+
+### Output
+
+tbd.
+
+## Parameters
+
+tbd.
+
+## Inner-workings / Algorithms
+
+```plantuml
+
+tbd
+
+```
+
+## Assumptions / Known limits
+
+## Future extensions / Unimplemented parts

planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp

@@ -0,0 +1,65 @@
+// 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_REPLAYER__RTC_REPLAYER_NODE_HPP_
+#define RTC_REPLAYER__RTC_REPLAYER_NODE_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/auto_mode.hpp"
+#include "tier4_rtc_msgs/srv/cooperate_commands.hpp"
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <map>
+
+namespace rtc_replayer
+{
+using std::placeholders::_1;
+using std::placeholders::_2;
+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::AutoMode;
+using tier4_rtc_msgs::srv::CooperateCommands;
+using unique_identifier_msgs::msg::UUID;
+class RTCReplayerNode : public rclcpp::Node
+{
+public:
+  explicit RTCReplayerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void onCoorperateStatus(const CooperateStatusArray::ConstSharedPtr msg);
+
+  std::vector<CooperateCommand> cooperate_commands_;
+  rclcpp::Subscription<CooperateStatusArray>::SharedPtr sub_statuses_;
+  rclcpp::Client<CooperateCommands>::SharedPtr client_rtc_commands_;
+  std::string module_name_;
+  std::map<std::string, uint8_t> prev_cmd_status_;
+};
+
+}  // namespace rtc_replayer
+
+#endif  // RTC_REPLAYER__RTC_REPLAYER_NODE_HPP_

planning/rtc_replayer/launch/rtc_replayer.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="param_path" default="$(find-pkg-share rtc_auto_mode_manager)/config/rtc_auto_mode_manager.param.yaml"/>
+
+  <node pkg="rtc_replayer" exec="rtc_replayer_node" name="rtc_replayer" output="screen">
+    <param from="$(var param_path)"/>
+  </node>
+</launch>

planning/rtc_replayer/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtc_replayer</name>
+  <version>0.1.0</version>
+  <description>The rtc_replayer package</description>
+
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="taiki.tanaka@tier4.jp">Fumiya Watanabe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_rtc_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>

planning/rtc_replayer/src/rtc_replayer_node.cpp

@@ -0,0 +1,68 @@
+// 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.
+
+#include "rtc_replayer/rtc_replayer_node.hpp"
+
+#include <algorithm>
+
+namespace rtc_replayer
+{
+
+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();
+}
+
+RTCReplayerNode::RTCReplayerNode(const rclcpp::NodeOptions & node_options)
+: Node("rtc_replayer_node", node_options)
+{
+  sub_statuses_ = create_subscription<CooperateStatusArray>(
+    "/debug/rtc_status", 1, std::bind(&RTCReplayerNode::onCoorperateStatus, this, _1));
+  client_rtc_commands_ = create_client<CooperateCommands>(
+    "/api/external/set/rtc_commands", rmw_qos_profile_services_default);
+}
+
+void RTCReplayerNode::onCoorperateStatus(const CooperateStatusArray::ConstSharedPtr msg)
+{
+  if (msg->statuses.empty()) return;
+  CooperateCommands::Request::SharedPtr request = std::make_shared<CooperateCommands::Request>();
+  for (auto status : msg->statuses) {
+    const auto cmd_status = status.command_status.type;
+    const auto uuid_string = to_string(status.uuid);
+    // add command which has change from previous status and command is already registered
+    if (prev_cmd_status_.find(uuid_string) != prev_cmd_status_.end() && cmd_status != prev_cmd_status_[uuid_string]) {
+      CooperateCommand cc;
+      // send previous command status
+      cc.command.type = cmd_status;
+      cc.uuid = status.uuid;
+      cc.module = status.module;
+      request->stamp = status.stamp;
+      request->commands.emplace_back(cc);
+    }
+    // post process
+    prev_cmd_status_[uuid_string] = cmd_status;
+  }
+  if (!request->commands.empty()) {
+    client_rtc_commands_->async_send_request(request);
+  }
+}
+
+}  // namespace rtc_replayer
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(rtc_replayer::RTCReplayerNode)