From 96e32d7e471d1eb93ad94be942d75910da71b306 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 1/7] 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 00000000..877c283b --- /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 00000000..e758edfd --- /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 00000000..49d0d9d6 --- /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 00000000..9b832b36 --- /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 00000000..9101a1e8 --- /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 00000000..08f037fb --- /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) From 04a874da222f3c49eeafc73f6ef8087e59a43781 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 26 Apr 2023 13:56:52 +0900 Subject: [PATCH 2/7] refactor(behavior_path_planner): rename pull_over to goal_planner (#3501) Signed-off-by: kosuke55 --- planning/rtc_replayer/src/rtc_replayer_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index 08f037fb..ebc7171f 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -47,8 +47,8 @@ std::string getModuleName(const uint8_t module_type) case Module::AVOIDANCE_RIGHT: { return "avoidance_right"; } - case Module::PULL_OVER: { - return "pull_over"; + case Module::GOAL_PLANNER: { + return "goal_planner"; } case Module::PULL_OUT: { return "pull_out"; From f334d2808725fa6a3e1b54b1dc14fa4ed9056c3d Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 4 May 2023 00:36:31 +0900 Subject: [PATCH 3/7] build: mark autoware_cmake as (#3616) * build: mark autoware_cmake as with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) Signed-off-by: Vincent Richard * style(pre-commit): autofix * chore: fix pre-commit errors Signed-off-by: Kenji Miyake --------- Signed-off-by: Vincent Richard Signed-off-by: Kenji Miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake --- planning/rtc_replayer/package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/rtc_replayer/package.xml b/planning/rtc_replayer/package.xml index 9101a1e8..17c5a760 100644 --- a/planning/rtc_replayer/package.xml +++ b/planning/rtc_replayer/package.xml @@ -13,8 +13,7 @@ Fumiya Watanabe ament_cmake_auto - - autoware_cmake + autoware_cmake rclcpp rclcpp_components From 8e03a12f602c1adf764eaafacb7a94ff44336b6f Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 25 May 2023 15:21:48 +0900 Subject: [PATCH 4/7] build(iron): remove rmw_qos_profile_t (#3809) Signed-off-by: wep21 --- planning/rtc_replayer/src/rtc_replayer_node.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index ebc7171f..333beafc 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -92,8 +92,7 @@ RTCReplayerNode::RTCReplayerNode(const rclcpp::NodeOptions & 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); + client_rtc_commands_ = create_client("/api/external/set/rtc_commands"); } void RTCReplayerNode::onCooperateStatus(const CooperateStatusArray::ConstSharedPtr msg) From c40d366a7a037d803b9e7365647a97caa22309c5 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 7 Jun 2023 20:21:51 +0900 Subject: [PATCH 5/7] refactor(start_planner): rename pull out to start planner (#3908) Signed-off-by: kosuke55 --- planning/rtc_replayer/src/rtc_replayer_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index 333beafc..8bed9ec4 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -50,8 +50,8 @@ std::string getModuleName(const uint8_t module_type) case Module::GOAL_PLANNER: { return "goal_planner"; } - case Module::PULL_OUT: { - return "pull_out"; + case Module::START_PLANNER: { + return "start_planner"; } case Module::TRAFFIC_LIGHT: { return "traffic_light"; From cf020bdd4835978e1f17e2296e1cb43d284cc4cf Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 7 Oct 2023 17:20:47 +0900 Subject: [PATCH 6/7] feat(rtc_auto_mode_manager): eliminate rtc auto mode manager (#5235) * change namespace of auto_mode Signed-off-by: kyoichi-sugahara * delete RTC auto mode manager package Signed-off-by: kyoichi-sugahara * delete rtc_replayer.param Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * fix typo Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/rtc_replayer/launch/rtc_replayer.launch.xml | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml index 9b832b36..70d7baf2 100644 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -1,7 +1,3 @@ - - - - - + From c673f438547818635901b6936fc2f6e1fea7e597 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 6 Jun 2024 18:41:12 +0900 Subject: [PATCH 7/7] rename to autoware_rtc_replayer Signed-off-by: Mamoru Sobue --- .../{rtc_replayer => autoware_rtc_replayer}/CMakeLists.txt | 4 ++-- planning/{rtc_replayer => autoware_rtc_replayer}/README.md | 0 .../include/rtc_replayer/rtc_replayer_node.hpp | 4 ++-- .../autoware_rtc_replayer/launch/rtc_replayer.launch.xml | 3 +++ .../{rtc_replayer => autoware_rtc_replayer}/package.xml | 4 ++-- .../src/autoware_rtc_replayer_node.cpp} | 6 +++--- planning/rtc_replayer/launch/rtc_replayer.launch.xml | 3 --- 7 files changed, 12 insertions(+), 12 deletions(-) rename planning/{rtc_replayer => autoware_rtc_replayer}/CMakeLists.txt (86%) rename planning/{rtc_replayer => autoware_rtc_replayer}/README.md (100%) rename planning/{rtc_replayer => autoware_rtc_replayer}/include/rtc_replayer/rtc_replayer_node.hpp (96%) create mode 100644 planning/autoware_rtc_replayer/launch/rtc_replayer.launch.xml rename planning/{rtc_replayer => autoware_rtc_replayer}/package.xml (89%) rename planning/{rtc_replayer/src/rtc_replayer_node.cpp => autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp} (96%) delete mode 100644 planning/rtc_replayer/launch/rtc_replayer.launch.xml diff --git a/planning/rtc_replayer/CMakeLists.txt b/planning/autoware_rtc_replayer/CMakeLists.txt similarity index 86% rename from planning/rtc_replayer/CMakeLists.txt rename to planning/autoware_rtc_replayer/CMakeLists.txt index 877c283b..f8f456e6 100644 --- a/planning/rtc_replayer/CMakeLists.txt +++ b/planning/autoware_rtc_replayer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(rtc_replayer) +project(autoware_rtc_replayer) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -17,7 +17,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "rtc_replayer::RTCReplayerNode" + PLUGIN "autoware::rtc_replayer::RTCReplayerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/rtc_replayer/README.md b/planning/autoware_rtc_replayer/README.md similarity index 100% rename from planning/rtc_replayer/README.md rename to planning/autoware_rtc_replayer/README.md diff --git a/planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp b/planning/autoware_rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp similarity index 96% rename from planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp rename to planning/autoware_rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp index 49d0d9d6..92ddec72 100644 --- a/planning/rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp +++ b/planning/autoware_rtc_replayer/include/rtc_replayer/rtc_replayer_node.hpp @@ -30,7 +30,7 @@ #include #include -namespace rtc_replayer +namespace autoware::rtc_replayer { using std::placeholders::_1; using std::placeholders::_2; @@ -54,6 +54,6 @@ class RTCReplayerNode : public rclcpp::Node std::map prev_cmd_status_; }; -} // namespace rtc_replayer +} // 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/rtc_replayer/package.xml b/planning/autoware_rtc_replayer/package.xml similarity index 89% rename from planning/rtc_replayer/package.xml rename to planning/autoware_rtc_replayer/package.xml index 17c5a760..c93b9a81 100644 --- a/planning/rtc_replayer/package.xml +++ b/planning/autoware_rtc_replayer/package.xml @@ -1,9 +1,9 @@ - rtc_replayer + autoware_rtc_replayer 0.1.0 - The rtc_replayer package + The autoware_rtc_replayer package Fumiya Watanabe Taiki Tanaka diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp similarity index 96% rename from planning/rtc_replayer/src/rtc_replayer_node.cpp rename to planning/autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp index 8bed9ec4..98afa937 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/autoware_rtc_replayer/src/autoware_rtc_replayer_node.cpp @@ -16,7 +16,7 @@ #include -namespace rtc_replayer +namespace autoware::rtc_replayer { std::string getModuleStatus(const uint8_t module_status) @@ -124,7 +124,7 @@ void RTCReplayerNode::onCooperateStatus(const CooperateStatusArray::ConstSharedP } } -} // namespace rtc_replayer +} // namespace autoware::rtc_replayer #include -RCLCPP_COMPONENTS_REGISTER_NODE(rtc_replayer::RTCReplayerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::rtc_replayer::RTCReplayerNode) diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml deleted file mode 100644 index 70d7baf2..00000000 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - -