feat(rtc_reaplyer): add rtc_replayer (autowarefoundation#1993) (autow…

…arefoundation#178)

* feat(rtc_replayer): add rtc_reaplayer

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore: small fix

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc: update

Signed-off-by: taikitanaka3 <taiki.tanaka@tier4.jp>

* chore: add debug print

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: fix

* fix: build fail

* style: spell check

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
Signed-off-by: taikitanaka3 <taiki.tanaka@tier4.jp>

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
Signed-off-by: taikitanaka3 <taiki.tanaka@tier4.jp>

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,49 @@
+# rtc_replayer
+
+## Purpose
+
+The current issue for RTC commands is that service is not recorded to rosbag, so it's very hard to analyze what was happened exactly.
+So this package makes it possible to replay rtc commands service from rosbag rtc status topic to resolve that issue.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                | Type                                      | Description                                     |
+| ------------------- | ----------------------------------------- | ----------------------------------------------- |
+| `/debug/rtc_status` | tier4_rtc_msgs::msg::CooperateStatusArray | CooperateStatusArray that is recorded in rosbag |
+
+### Output
+
+| Name                             | Type                                   | Description                                        |
+| -------------------------------- | -------------------------------------- | -------------------------------------------------- |
+| `/api/external/set/rtc_commands` | tier4_rtc_msgs::msg::CooperateCommands | CooperateCommands that is replayed by this package |
+
+## Inner-workings / Algorithms
+
+```plantuml
+
+@startuml
+title rtc replayer
+start
+
+:rosbag;
+
+:rtc_replayer;
+
+:rtc_interface;
+
+end
+
+@enduml
+
+```
+
+## Assumptions / Known limits
+
+This package can't replay CooperateCommands correctly if CooperateStatusArray is not stable.
+And this replay is always later one step than actual however it will not affect much for behavior.
+
+## Future extensions / Unimplemented parts
+
+tbd.

planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp

@@ -0,0 +1,59 @@
+// 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_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 <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+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::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 RTCReplayerNode : public rclcpp::Node
+{
+public:
+  explicit RTCReplayerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void onCooperateStatus(const CooperateStatusArray::ConstSharedPtr msg);
+
+  rclcpp::Subscription<CooperateStatusArray>::SharedPtr sub_statuses_;
+  rclcpp::Client<CooperateCommands>::SharedPtr client_rtc_commands_;
+  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,131 @@
+// 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 getModuleStatus(const uint8_t module_status)
+{
+  switch (module_status) {
+    case Command::ACTIVATE: {
+      return "execute";
+    }
+    case Command::DEACTIVATE: {
+      return "wait";
+    }
+  }
+  return "none";
+}
+
+std::string getModuleName(const uint8_t module_type)
+{
+  switch (module_type) {
+    case Module::LANE_CHANGE_LEFT: {
+      return "lane_change_left";
+    }
+    case Module::LANE_CHANGE_RIGHT: {
+      return "lane_change_right";
+    }
+    case Module::AVOIDANCE_LEFT: {
+      return "avoidance_left";
+    }
+    case Module::AVOIDANCE_RIGHT: {
+      return "avoidance_right";
+    }
+    case Module::PULL_OVER: {
+      return "pull_over";
+    }
+    case Module::PULL_OUT: {
+      return "pull_out";
+    }
+    case Module::TRAFFIC_LIGHT: {
+      return "traffic_light";
+    }
+    case Module::INTERSECTION: {
+      return "intersection";
+    }
+    case Module::CROSSWALK: {
+      return "crosswalk";
+    }
+    case Module::BLIND_SPOT: {
+      return "blind_spot";
+    }
+    case Module::DETECTION_AREA: {
+      return "detection_area";
+    }
+    case Module::NO_STOPPING_AREA: {
+      return "no_stopping_area";
+    }
+    case Module::OCCLUSION_SPOT: {
+      return "occlusion_spot";
+    }
+  }
+  return "NONE";
+}
+
+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::onCooperateStatus, this, _1));
+  client_rtc_commands_ = create_client<CooperateCommands>(
+    "/api/external/set/rtc_commands", rmw_qos_profile_services_default);
+}
+
+void RTCReplayerNode::onCooperateStatus(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);
+      std::cerr << "uuid: " << uuid_string << " module: " << getModuleName(cc.module.type)
+                << " status: " << getModuleStatus(cmd_status) << std::endl;
+    }
+    // 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)