diff --git a/planning/autoware_rtc_replayer/CMakeLists.txt b/planning/autoware_rtc_replayer/CMakeLists.txt new file mode 100644 index 00000000..f8f456e6 --- /dev/null +++ b/planning/autoware_rtc_replayer/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_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 "autoware::rtc_replayer::RTCReplayerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/planning/autoware_rtc_replayer/README.md b/planning/autoware_rtc_replayer/README.md new file mode 100644 index 00000000..e758edfd --- /dev/null +++ b/planning/autoware_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/autoware_rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp b/planning/autoware_rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp new file mode 100644 index 00000000..92ddec72 --- /dev/null +++ b/planning/autoware_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 autoware::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 autoware::rtc_replayer + +#endif // RTC_REPLAYER__RTC_REPLAYER_NODE_HPP_ diff --git a/planning/autoware_rtc_replayer/launch/rtc_replayer.launch.xml b/planning/autoware_rtc_replayer/launch/rtc_replayer.launch.xml new file mode 100644 index 00000000..15bf6bea --- /dev/null +++ b/planning/autoware_rtc_replayer/launch/rtc_replayer.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_rtc_replayer/package.xml b/planning/autoware_rtc_replayer/package.xml new file mode 100644 index 00000000..c93b9a81 --- /dev/null +++ b/planning/autoware_rtc_replayer/package.xml @@ -0,0 +1,28 @@ + + + + autoware_rtc_replayer + 0.1.0 + The autoware_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/autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp b/planning/autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp new file mode 100644 index 00000000..98afa937 --- /dev/null +++ b/planning/autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp @@ -0,0 +1,130 @@ +// 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 autoware::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::GOAL_PLANNER: { + return "goal_planner"; + } + case Module::START_PLANNER: { + return "start_planner"; + } + 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"); +} + +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 autoware::rtc_replayer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::rtc_replayer::RTCReplayerNode)