From e7947f5b2a44eff858eab2e15253bfe08cf1d616 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 6 Oct 2022 17:51:38 +0900 Subject: [PATCH] feat(rtc_reaplyer): add rtc_replayer (#1993) * feat(rtc_replayer): add rtc_reaplayer Signed-off-by: tanaka3 * chore: small fix Signed-off-by: tanaka3 * doc: update Signed-off-by: taikitanaka3 * chore: add debug print Signed-off-by: tanaka3 * fix: fix * fix: build fail * style: spell check Signed-off-by: tanaka3 Signed-off-by: tanaka3 Signed-off-by: taikitanaka3 --- planning/rtc_replayer/CMakeLists.txt | 27 ++++ planning/rtc_replayer/README.md | 49 +++++++ .../rtc_replayer/rtc_replayer_node.hpp | 59 ++++++++ .../launch/rtc_replayer.launch.xml | 7 + planning/rtc_replayer/package.xml | 29 ++++ .../rtc_replayer/src/rtc_replayer_node.cpp | 131 ++++++++++++++++++ 6 files changed, 302 insertions(+) create mode 100644 planning/rtc_replayer/CMakeLists.txt create mode 100644 planning/rtc_replayer/README.md create mode 100644 planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp create mode 100644 planning/rtc_replayer/launch/rtc_replayer.launch.xml create mode 100644 planning/rtc_replayer/package.xml create mode 100644 planning/rtc_replayer/src/rtc_replayer_node.cpp diff --git a/planning/rtc_replayer/CMakeLists.txt b/planning/rtc_replayer/CMakeLists.txt new file mode 100644 index 0000000000000..877c283b41ea9 --- /dev/null +++ b/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 +) diff --git a/planning/rtc_replayer/README.md b/planning/rtc_replayer/README.md new file mode 100644 index 0000000000000..e758edfdb38b1 --- /dev/null +++ b/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. diff --git a/planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp b/planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp new file mode 100644 index 0000000000000..49d0d9d60c93a --- /dev/null +++ b/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 + +#include +#include +#include +#include + +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::SharedPtr sub_statuses_; + rclcpp::Client::SharedPtr client_rtc_commands_; + std::map prev_cmd_status_; +}; + +} // namespace rtc_replayer + +#endif // RTC_REPLAYER__RTC_REPLAYER_NODE_HPP_ diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml new file mode 100644 index 0000000000000..9b832b369c77c --- /dev/null +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/planning/rtc_replayer/package.xml b/planning/rtc_replayer/package.xml new file mode 100644 index 0000000000000..9101a1e86633a --- /dev/null +++ b/planning/rtc_replayer/package.xml @@ -0,0 +1,29 @@ + + + + rtc_replayer + 0.1.0 + The rtc_replayer package + + Fumiya Watanabe + Taiki Tanaka + + Apache License 2.0 + + Fumiya Watanabe + + ament_cmake_auto + + autoware_cmake + + rclcpp + rclcpp_components + tier4_rtc_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp new file mode 100644 index 0000000000000..08f037fbfc390 --- /dev/null +++ b/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 + +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( + "/debug/rtc_status", 1, std::bind(&RTCReplayerNode::onCooperateStatus, this, _1)); + client_rtc_commands_ = create_client( + "/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(); + 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(rtc_replayer::RTCReplayerNode)