From 4b1037197b431422949f2b28f4a0de1522c4c587 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 16:19:01 +0900 Subject: [PATCH 01/26] feat: add node base Signed-off-by: TetsuKawa --- .../CMakeLists.txt | 20 ++++++++++++ .../autoware_topic_relay_controller/README.md | 15 +++++++++ .../config/topic_relay_controller.param.yaml | 4 +++ .../launch/topic_relay_controller.launch.xml | 4 +++ .../package.xml | 22 +++++++++++++ .../schema/topic_relay_controller.schema.json | 30 +++++++++++++++++ .../src/topic_relay_controller_node.cpp | 25 +++++++++++++++ .../src/topic_relay_controller_node.hpp | 32 +++++++++++++++++++ 8 files changed, 152 insertions(+) create mode 100644 system/autoware_topic_relay_controller/CMakeLists.txt create mode 100644 system/autoware_topic_relay_controller/README.md create mode 100644 system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml create mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml create mode 100644 system/autoware_topic_relay_controller/package.xml create mode 100644 system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json create mode 100644 system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp create mode 100644 system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt new file mode 100644 index 0000000000000..7b035b2b62c23 --- /dev/null +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_topic_relay_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/topic_relay_controller_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::topic_relay_controller::TopicRelayController" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md new file mode 100644 index 0000000000000..1976a75dc6d48 --- /dev/null +++ b/system/autoware_topic_relay_controller/README.md @@ -0,0 +1,15 @@ +# topic_relay_controller + +## Purpose + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +TBD. diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml new file mode 100644 index 0000000000000..3a506213f054d --- /dev/null +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -0,0 +1,4 @@ +--- +/**: + ros__parameters: + tmp: 0 \ No newline at end of file diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml new file mode 100644 index 0000000000000..3f152f40d12df --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml new file mode 100644 index 0000000000000..c67a52d52e84c --- /dev/null +++ b/system/autoware_topic_relay_controller/package.xml @@ -0,0 +1,22 @@ + + + + autoware_topic_relay_controller + 0.1.0 + The topic_relay_controller ROS 2 package + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json b/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json new file mode 100644 index 0000000000000..d51b0ae4ed1b6 --- /dev/null +++ b/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for topic relay controller", + "type": "object", + "definitions": { + "topic_rely_controller": { + "type": "object", + "properties": { + }, + "required": [ + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/topic_rely_controller" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false + } + \ No newline at end of file diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp new file mode 100644 index 0000000000000..74370ca306aac --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -0,0 +1,25 @@ +// Copyright 2025 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 "topic_relay_controller_node.hpp" + +namespace autoware::topic_relay_controller +{ +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) +{ + RCLCPP_INFO(get_logger(), "topic_relay_controller_node started."); +} +} // namespace autoware::topic_relay_controller + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp new file mode 100644 index 0000000000000..c5ea006b13966 --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -0,0 +1,32 @@ +// Copyright 2025 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 AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ +#define AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ + +// ROS 2 core +#include + +namespace autoware::topic_relay_controller +{ +class TopicRelayController : public rclcpp::Node +{ +public: + explicit TopicRelayController(const rclcpp::NodeOptions & options); + +private: +}; +} // namespace autoware::topic_relay_controller + +#endif // AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file From 960000d9cea5ed827585c8a78ef8c3fc7fe11482 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 18:11:45 +0900 Subject: [PATCH 02/26] modify: include guard Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index c5ea006b13966..fca4dfdec31b4 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ -#define AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ +#ifndef TOPIC_RELAY_CONTROLLER_HPP_ +#define TOPIC_RELAY_CONTROLLER_HPP_ // ROS 2 core #include @@ -29,4 +29,4 @@ class TopicRelayController : public rclcpp::Node }; } // namespace autoware::topic_relay_controller -#endif // AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file +#endif // TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file From e92fe98e2d37a0273dec8bfff899e61c400a06f4 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 15:45:47 +0900 Subject: [PATCH 03/26] feat: delete schema Signed-off-by: TetsuKawa --- .../schema/topic_relay_controller.schema.json | 30 ------------------- 1 file changed, 30 deletions(-) delete mode 100644 system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json diff --git a/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json b/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json deleted file mode 100644 index d51b0ae4ed1b6..0000000000000 --- a/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json +++ /dev/null @@ -1,30 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for topic relay controller", - "type": "object", - "definitions": { - "topic_rely_controller": { - "type": "object", - "properties": { - }, - "required": [ - ], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/topic_rely_controller" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false - } - \ No newline at end of file From 27355a05976a628fd03063294810fe4faca3d38a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 15:44:27 +0900 Subject: [PATCH 04/26] feat: delete config Signed-off-by: TetsuKawa --- system/autoware_topic_relay_controller/CMakeLists.txt | 1 - .../config/topic_relay_controller.param.yaml | 4 ---- 2 files changed, 5 deletions(-) delete mode 100644 system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt index 7b035b2b62c23..f244bf8122f82 100644 --- a/system/autoware_topic_relay_controller/CMakeLists.txt +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -16,5 +16,4 @@ rclcpp_components_register_node(${PROJECT_NAME} ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml deleted file mode 100644 index 3a506213f054d..0000000000000 --- a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ ---- -/**: - ros__parameters: - tmp: 0 \ No newline at end of file From 66cbd5851e0e959707683d328688fbd8756b3b2e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 16:29:05 +0900 Subject: [PATCH 05/26] feat: add subscriber Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 16 ++++++- .../topic_relay_controller_tf.launch.xml | 18 ++++++++ .../package.xml | 1 + .../src/topic_relay_controller_node.cpp | 45 ++++++++++++++++++- .../src/topic_relay_controller_node.hpp | 21 +++++++++ 5 files changed, 99 insertions(+), 2 deletions(-) create mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 3f152f40d12df..49672c0a044ab 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -1,4 +1,18 @@ - + + + + + + + + + + + + + + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml new file mode 100644 index 0000000000000..6dfd1464715bc --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml index c67a52d52e84c..101c40b373048 100644 --- a/system/autoware_topic_relay_controller/package.xml +++ b/system/autoware_topic_relay_controller/package.xml @@ -12,6 +12,7 @@ rclcpp rclcpp_components + tf2_msgs ament_lint_auto autoware_lint_common diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 74370ca306aac..4dd51cf821e1d 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -17,7 +17,50 @@ namespace autoware::topic_relay_controller { TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) { - RCLCPP_INFO(get_logger(), "topic_relay_controller_node started."); + // Parameter + node_param_.topic = declare_parameter("topic"); + node_param_.remap_topic = declare_parameter("remap_topic"); + node_param_.qos = declare_parameter("qos", 1); + node_param_.transient_local = declare_parameter("transient_local", false); + node_param_.best_effort = declare_parameter("best_effort", false); + node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); + + if (node_param_.is_transform) { + node_param_.frame_id = declare_parameter("frame_id"); + node_param_.child_frame_id = declare_parameter("child_frame_id"); + } else { + node_param_.topic_type = declare_parameter("topic_type"); + } + + // Subscriber + rclcpp::QoS qos = rclcpp::QoS{node_param_.qos}; + if (node_param_.transient_local) { + qos.transient_local(); + } + if (node_param_.best_effort) { + qos.best_effort(); + } + + if (node_param_.is_transform) { + sub_transform_ = this->create_subscription( + node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + for (const auto & transform : msg->transforms) { + if ( + transform.header.frame_id == node_param_.frame_id && + transform.child_frame_id == node_param_.child_frame_id) { + RCLCPP_INFO ( + this->get_logger(), "Received transform from %s to %s", + transform.header.frame_id.c_str(), transform.child_frame_id.c_str()); + } + } + }); + } else { + sub_topic_ = this->create_generic_subscription( + node_param_.topic, node_param_.topic_type, qos, + [this]([[maybe_unused]] std::shared_ptr msg) { + RCLCPP_INFO(this->get_logger(), "Received message"); + }); + } } } // namespace autoware::topic_relay_controller diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index fca4dfdec31b4..fe4a789f6c5c4 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -18,14 +18,35 @@ // ROS 2 core #include +#include + namespace autoware::topic_relay_controller { +struct NodeParam +{ + std::string topic; + std::string remap_topic; + std::string topic_type; + size_t qos; + std::string frame_id; + std::string child_frame_id; + bool transient_local; + bool best_effort; + bool is_transform; +}; + class TopicRelayController : public rclcpp::Node { public: explicit TopicRelayController(const rclcpp::NodeOptions & options); private: + // Parameter + NodeParam node_param_; + + // Subscriber + rclcpp::GenericSubscription::SharedPtr sub_topic_; + rclcpp::Subscription::SharedPtr sub_transform_; }; } // namespace autoware::topic_relay_controller From 8d404053c76c4dabfa68143ccb613d31963ffa49 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 16:02:53 +0900 Subject: [PATCH 06/26] modify: add include Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 3 +++ .../src/topic_relay_controller_node.hpp | 2 ++ 2 files changed, 5 insertions(+) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 4dd51cf821e1d..6584a373e3881 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -13,6 +13,9 @@ #include "topic_relay_controller_node.hpp" +#include +#include + namespace autoware::topic_relay_controller { TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index fe4a789f6c5c4..699e9a2a66ace 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -20,6 +20,8 @@ #include +#include + namespace autoware::topic_relay_controller { struct NodeParam From dda7a93741336f8927d9a331a41d4206a6f63af9 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 16:34:06 +0900 Subject: [PATCH 07/26] feat: add publisher Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 14 ++++++++++---- .../src/topic_relay_controller_node.hpp | 4 ++++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 6584a373e3881..e70bced3ebf96 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -45,23 +45,29 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) } if (node_param_.is_transform) { + // Publisher + pub_transform_ = this->create_publisher( + node_param_.remap_topic, qos); + sub_transform_ = this->create_subscription( node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { for (const auto & transform : msg->transforms) { if ( transform.header.frame_id == node_param_.frame_id && transform.child_frame_id == node_param_.child_frame_id) { - RCLCPP_INFO ( - this->get_logger(), "Received transform from %s to %s", - transform.header.frame_id.c_str(), transform.child_frame_id.c_str()); + pub_transform_->publish(*msg); } } }); } else { + // Publisher + pub_topic_ = this->create_generic_publisher( + node_param_.remap_topic, node_param_.topic_type, qos); + sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { - RCLCPP_INFO(this->get_logger(), "Received message"); + pub_topic_->publish(*msg); }); } } diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 699e9a2a66ace..7ee647d2893f0 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -49,6 +49,10 @@ class TopicRelayController : public rclcpp::Node // Subscriber rclcpp::GenericSubscription::SharedPtr sub_topic_; rclcpp::Subscription::SharedPtr sub_transform_; + + // Publisher + rclcpp::GenericPublisher::SharedPtr pub_topic_; + rclcpp::Publisher::SharedPtr pub_transform_; }; } // namespace autoware::topic_relay_controller From 70988cbdc51a4bef476e76a3f772b37ed430570d Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 17:25:13 +0900 Subject: [PATCH 08/26] feat: add service Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 4 ++++ .../topic_relay_controller_tf.launch.xml | 4 ++++ .../package.xml | 1 + .../src/topic_relay_controller_node.cpp | 20 ++++++++++++++++--- .../src/topic_relay_controller_node.hpp | 9 +++++++++ 5 files changed, 35 insertions(+), 3 deletions(-) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 49672c0a044ab..7dbc806ee6424 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -6,6 +6,8 @@ + + @@ -14,5 +16,7 @@ + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index 6dfd1464715bc..801357c03f8e3 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -6,6 +6,8 @@ + + @@ -14,5 +16,7 @@ + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml index 101c40b373048..537774e67b998 100644 --- a/system/autoware_topic_relay_controller/package.xml +++ b/system/autoware_topic_relay_controller/package.xml @@ -13,6 +13,7 @@ rclcpp rclcpp_components tf2_msgs + tier4_system_msgs ament_lint_auto autoware_lint_common diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index e70bced3ebf96..78820e2129988 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -18,7 +18,7 @@ namespace autoware::topic_relay_controller { -TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): Node("topic_relay_controller", options), is_relaying_(true) { // Parameter node_param_.topic = declare_parameter("topic"); @@ -27,6 +27,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.transient_local = declare_parameter("transient_local", false); node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); + node_param_.enable_relay_control = declare_parameter("enable_relay_control"); + node_param_.srv_name = declare_parameter("srv_name"); if (node_param_.is_transform) { node_param_.frame_id = declare_parameter("frame_id"); @@ -35,6 +37,17 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.topic_type = declare_parameter("topic_type"); } + // Service + if (node_param_.enable_relay_control) { + srv_change_relay_control_ = create_service( + node_param_.srv_name, + [this](const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, + tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { + is_relaying_ = request->relay_on; + response->status.success = true; + }); + } + // Subscriber rclcpp::QoS qos = rclcpp::QoS{node_param_.qos}; if (node_param_.transient_local) { @@ -54,7 +67,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) for (const auto & transform : msg->transforms) { if ( transform.header.frame_id == node_param_.frame_id && - transform.child_frame_id == node_param_.child_frame_id) { + transform.child_frame_id == node_param_.child_frame_id && + is_relaying_) { pub_transform_->publish(*msg); } } @@ -67,7 +81,7 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { - pub_topic_->publish(*msg); + if (is_relaying_) pub_topic_->publish(*msg); }); } } diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 7ee647d2893f0..23d17ce3b7c21 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include @@ -35,6 +36,8 @@ struct NodeParam bool transient_local; bool best_effort; bool is_transform; + bool enable_relay_control; + std::string srv_name; }; class TopicRelayController : public rclcpp::Node @@ -53,6 +56,12 @@ class TopicRelayController : public rclcpp::Node // Publisher rclcpp::GenericPublisher::SharedPtr pub_topic_; rclcpp::Publisher::SharedPtr pub_transform_; + + // Service + rclcpp::Service::SharedPtr srv_change_relay_control_; + + // State + bool is_relaying_; }; } // namespace autoware::topic_relay_controller From 56828bc2ce613166d8eb20401d8bf3eabbde3467 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 15:40:40 +0900 Subject: [PATCH 09/26] modify: typo Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 4 ++-- .../launch/topic_relay_controller_tf.launch.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 7dbc806ee6424..4165cf7ea741b 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -6,7 +6,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index 801357c03f8e3..8a497982dd813 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -6,7 +6,7 @@ - + @@ -16,7 +16,7 @@ - + From 4d88d50ce7cfa79f5b48e22a8e2f0e9386f4b87a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jan 2025 09:13:48 +0000 Subject: [PATCH 10/26] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 20 +++++++++---------- .../src/topic_relay_controller_node.hpp | 9 +++++---- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 78820e2129988..ace592f3c875e 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -18,12 +18,13 @@ namespace autoware::topic_relay_controller { -TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): Node("topic_relay_controller", options), is_relaying_(true) +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) +: Node("topic_relay_controller", options), is_relaying_(true) { // Parameter node_param_.topic = declare_parameter("topic"); node_param_.remap_topic = declare_parameter("remap_topic"); - node_param_.qos = declare_parameter("qos", 1); + node_param_.qos = declare_parameter("qos", 1); node_param_.transient_local = declare_parameter("transient_local", false); node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); @@ -41,7 +42,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): if (node_param_.enable_relay_control) { srv_change_relay_control_ = create_service( node_param_.srv_name, - [this](const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, + [this]( + const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { is_relaying_ = request->relay_on; response->status.success = true; @@ -59,24 +61,22 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): if (node_param_.is_transform) { // Publisher - pub_transform_ = this->create_publisher( - node_param_.remap_topic, qos); + pub_transform_ = this->create_publisher(node_param_.remap_topic, qos); sub_transform_ = this->create_subscription( node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { for (const auto & transform : msg->transforms) { if ( transform.header.frame_id == node_param_.frame_id && - transform.child_frame_id == node_param_.child_frame_id && - is_relaying_) { + transform.child_frame_id == node_param_.child_frame_id && is_relaying_) { pub_transform_->publish(*msg); } } }); } else { // Publisher - pub_topic_ = this->create_generic_publisher( - node_param_.remap_topic, node_param_.topic_type, qos); + pub_topic_ = + this->create_generic_publisher(node_param_.remap_topic, node_param_.topic_type, qos); sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, @@ -85,7 +85,7 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): }); } } -} // namespace autoware::topic_relay_controller +} // namespace autoware::topic_relay_controller #include RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 23d17ce3b7c21..14ac259bbee6e 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TOPIC_RELAY_CONTROLLER_HPP_ -#define TOPIC_RELAY_CONTROLLER_HPP_ +#ifndef TOPIC_RELAY_CONTROLLER_NODE_HPP_ +#define TOPIC_RELAY_CONTROLLER_NODE_HPP_ // ROS 2 core #include @@ -58,11 +58,12 @@ class TopicRelayController : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_transform_; // Service - rclcpp::Service::SharedPtr srv_change_relay_control_; + rclcpp::Service::SharedPtr + srv_change_relay_control_; // State bool is_relaying_; }; } // namespace autoware::topic_relay_controller -#endif // TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file +#endif // TOPIC_RELAY_CONTROLLER_NODE_HPP_ From f6250b63069a142259f0f02b3e318f9d0b7abdf9 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Tue, 14 Jan 2025 23:13:20 +0900 Subject: [PATCH 11/26] modify: add include memory Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 14ac259bbee6e..7491ef4662021 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -22,6 +22,7 @@ #include #include +#include namespace autoware::topic_relay_controller { From 4380ba3ecbe51f64182ed603a43d0110a1a2e265 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 22:21:49 +0900 Subject: [PATCH 12/26] modify: add qos setting Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller_tf.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index 8a497982dd813..d23ef5f26d91e 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -2,6 +2,7 @@ + @@ -12,6 +13,7 @@ + From 5461237d05b30f9f1ed9a4458185b7c325bbc9e5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 14:15:30 +0000 Subject: [PATCH 13/26] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 7491ef4662021..1bdcb435e01c4 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include namespace autoware::topic_relay_controller { From 5117744b9c67ada137a523a6df70baf21b26e322 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 22:20:41 +0900 Subject: [PATCH 14/26] feat: add enable_keep_publishing Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 4 ++ .../topic_relay_controller_tf.launch.xml | 4 ++ .../src/topic_relay_controller_node.cpp | 44 ++++++++++++++++--- .../src/topic_relay_controller_node.hpp | 7 +++ 4 files changed, 53 insertions(+), 6 deletions(-) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 4165cf7ea741b..ae642dad39f93 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -8,6 +8,8 @@ + + @@ -18,5 +20,7 @@ + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index d23ef5f26d91e..648d53cf4e1b7 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -9,6 +9,8 @@ + + @@ -20,5 +22,7 @@ + + diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index ace592f3c875e..afc1261e201a7 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -30,6 +30,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); node_param_.enable_relay_control = declare_parameter("enable_relay_control"); node_param_.srv_name = declare_parameter("srv_name"); + node_param_.enable_keep_publishing = declare_parameter("enable_keep_publishing"); + node_param_.update_rate = declare_parameter("update_rate"); if (node_param_.is_transform) { node_param_.frame_id = declare_parameter("frame_id"); @@ -46,6 +48,7 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { is_relaying_ = request->relay_on; + RCLCPP_INFO(get_logger(), "relay control: %s", is_relaying_ ? "ON" : "OFF"); response->status.success = true; }); } @@ -64,15 +67,21 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) pub_transform_ = this->create_publisher(node_param_.remap_topic, qos); sub_transform_ = this->create_subscription( - node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::SharedPtr msg) { for (const auto & transform : msg->transforms) { if ( - transform.header.frame_id == node_param_.frame_id && - transform.child_frame_id == node_param_.child_frame_id && is_relaying_) { + transform.header.frame_id != node_param_.frame_id || + transform.child_frame_id != node_param_.child_frame_id || + !is_relaying_) return; + + if (node_param_.enable_keep_publishing) { + last_tf_topic_ = msg; + } else { pub_transform_->publish(*msg); } } - }); + } + ); } else { // Publisher pub_topic_ = @@ -81,8 +90,31 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { - if (is_relaying_) pub_topic_->publish(*msg); - }); + if (!is_relaying_) return; + + if (node_param_.enable_keep_publishing) { + last_topic_ = msg; + } else { + pub_topic_->publish(*msg); + } + } + ); + } + + // Timer + if (node_param_.enable_keep_publishing) { + const auto update_period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, [this]() { + if (!is_relaying_) return; + + if (node_param_.is_transform) { + if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); + } else { + if (last_topic_) pub_topic_->publish(*last_topic_); + } + } + ); } } } // namespace autoware::topic_relay_controller diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 1bdcb435e01c4..c5e2ac08203c7 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -39,6 +39,8 @@ struct NodeParam bool is_transform; bool enable_relay_control; std::string srv_name; + bool enable_keep_publishing; + int update_rate; }; class TopicRelayController : public rclcpp::Node @@ -62,8 +64,13 @@ class TopicRelayController : public rclcpp::Node rclcpp::Service::SharedPtr srv_change_relay_control_; + // Timer + rclcpp::TimerBase::SharedPtr timer_; + // State bool is_relaying_; + tf2_msgs::msg::TFMessage::SharedPtr last_tf_topic_; + std::shared_ptr last_topic_; }; } // namespace autoware::topic_relay_controller From b16d790ea496a7a83d4b9da5f8477850dbacd8c3 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 13:26:28 +0000 Subject: [PATCH 15/26] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index afc1261e201a7..b68d8262de24d 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -71,17 +71,16 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) for (const auto & transform : msg->transforms) { if ( transform.header.frame_id != node_param_.frame_id || - transform.child_frame_id != node_param_.child_frame_id || - !is_relaying_) return; - + transform.child_frame_id != node_param_.child_frame_id || !is_relaying_) + return; + if (node_param_.enable_keep_publishing) { last_tf_topic_ = msg; } else { pub_transform_->publish(*msg); } } - } - ); + }); } else { // Publisher pub_topic_ = @@ -91,30 +90,27 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { if (!is_relaying_) return; - + if (node_param_.enable_keep_publishing) { last_topic_ = msg; } else { pub_topic_->publish(*msg); } - } - ); + }); } // Timer if (node_param_.enable_keep_publishing) { const auto update_period_ns = rclcpp::Rate(node_param_.update_rate).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), update_period_ns, [this]() { - if (!is_relaying_) return; + timer_ = rclcpp::create_timer(this, get_clock(), update_period_ns, [this]() { + if (!is_relaying_) return; - if (node_param_.is_transform) { - if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); - } else { - if (last_topic_) pub_topic_->publish(*last_topic_); - } + if (node_param_.is_transform) { + if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); + } else { + if (last_topic_) pub_topic_->publish(*last_topic_); } - ); + }); } } } // namespace autoware::topic_relay_controller From bf6c3c131e772647ead15763943eb214533fe647 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 20 Jan 2025 12:30:57 +0900 Subject: [PATCH 16/26] feat: add readme Signed-off-by: TetsuKawa --- .../autoware_topic_relay_controller/README.md | 30 +++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md index 1976a75dc6d48..5bce3cdfcdf2a 100644 --- a/system/autoware_topic_relay_controller/README.md +++ b/system/autoware_topic_relay_controller/README.md @@ -1,15 +1,41 @@ # topic_relay_controller ## Purpose +The node subscribes to a specified topic, remaps it, and republishes it. Additionally, it has the capability to continue publishing the last received value if the subscription stops. ## Inputs / Outputs ### Input +| Name | Type | Description | +| ------------- | ------------------------------- | ------------------------------------------------------------------------- | +| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | +| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | + ### Output +| Name | Type | Description | +| ------------- | ------------------------------- | ------------------------------------------------------------------------- | +| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | + + ## Parameters +- **`topic`** (string): The name of the input topic to subscribe to. +- **`remap_topic`** (string): The name of the output topic to publish to. +- **`topic_type`** (string): The type of messages being relayed. +- **`qos`** (integer, default: `1`): QoS profile to use for subscriptions and publications. +- **`transient_local`** (boolean, default: `false`): Enables transient local QoS for subscribers. +- **`best_effort`** (boolean, default: `false`): Enables best-effort QoS for subscribers. +- **`enable_relay_control`** (boolean, default: `true`): Allows dynamic relay control via a service. +- **`srv_name`** (string): The service name for relay control when `enable_relay_control` is `true`. +- **`enable_keep_publishing`** (boolean, default: `false`): Keeps publishing the last received topic value when not subscribed. +- **`update_rate`** (integer, default: `10`): The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true`. +- **`frame_id`** (string, optional): Frame ID for transform messages when subscribing to `/tf` or `/tf_static`. +- **`child_frame_id`** (string, optional): Child frame ID for transform messages when subscribing to `/tf` or `/tf_static`. ## Assumptions / Known limits - -TBD. +- The node assumes that the specified `topic` and `remap_topic` are valid and accessible within the ROS 2 environment. +- If `enable_keep_publishing` is `true`, the node continuously republishes the last received value even if no new messages are being received. +- For `/tf` and `/tf_static`, additional parameters like `frame_id` and `child_frame_id` are required for selective transformation relays. +- QoS settings must be carefully chosen to match the requirements of the subscribed and published topics. From 4f353b716fd5c0df58dde9273e3471cc45f7cd93 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 Jan 2025 03:33:45 +0000 Subject: [PATCH 17/26] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../autoware_topic_relay_controller/README.md | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md index 5bce3cdfcdf2a..e3c6a4075aa04 100644 --- a/system/autoware_topic_relay_controller/README.md +++ b/system/autoware_topic_relay_controller/README.md @@ -1,26 +1,27 @@ # topic_relay_controller ## Purpose + The node subscribes to a specified topic, remaps it, and republishes it. Additionally, it has the capability to continue publishing the last received value if the subscription stops. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ------------- | ------------------------------- | ------------------------------------------------------------------------- | -| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | -| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | +| Name | Type | Description | +| ------------ | -------------------------- | -------------------------------------------------------------------- | +| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | +| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | ### Output -| Name | Type | Description | -| ------------- | ------------------------------- | ------------------------------------------------------------------------- | -| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | - +| Name | Type | Description | +| ---------------- | -------------------------- | ----------------------------------------------------------------------------- | +| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | ## Parameters + - **`topic`** (string): The name of the input topic to subscribe to. - **`remap_topic`** (string): The name of the output topic to publish to. - **`topic_type`** (string): The type of messages being relayed. @@ -35,6 +36,7 @@ The node subscribes to a specified topic, remaps it, and republishes it. Additio - **`child_frame_id`** (string, optional): Child frame ID for transform messages when subscribing to `/tf` or `/tf_static`. ## Assumptions / Known limits + - The node assumes that the specified `topic` and `remap_topic` are valid and accessible within the ROS 2 environment. - If `enable_keep_publishing` is `true`, the node continuously republishes the last received value even if no new messages are being received. - For `/tf` and `/tf_static`, additional parameters like `frame_id` and `child_frame_id` are required for selective transformation relays. From 7213b341c3c79bfff299722bdb9892855024f0c5 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 21:21:58 +0900 Subject: [PATCH 18/26] feat: change qos name and add parameter type Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 16 ++++++++++++---- .../src/topic_relay_controller_node.hpp | 2 +- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index b68d8262de24d..655c46b08d043 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -24,9 +24,9 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) // Parameter node_param_.topic = declare_parameter("topic"); node_param_.remap_topic = declare_parameter("remap_topic"); - node_param_.qos = declare_parameter("qos", 1); - node_param_.transient_local = declare_parameter("transient_local", false); - node_param_.best_effort = declare_parameter("best_effort", false); + node_param_.qos_depth = declare_parameter("qos_depth", 1); + node_param_.transient_local = declare_parameter("transient_local", false); + node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); node_param_.enable_relay_control = declare_parameter("enable_relay_control"); node_param_.srv_name = declare_parameter("srv_name"); @@ -54,7 +54,15 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) } // Subscriber - rclcpp::QoS qos = rclcpp::QoS{node_param_.qos}; + rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(); + if (node_param_.qos_depth > 0) { + size_t qos_depth = static_cast(node_param_.qos_depth); + qos.keep_last(qos_depth); + } else { + RCLCPP_ERROR(get_logger(), "qos_depth must be greater than 0"); + return; + } + if (node_param_.transient_local) { qos.transient_local(); } diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index c5e2ac08203c7..446b9f156456a 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -31,7 +31,7 @@ struct NodeParam std::string topic; std::string remap_topic; std::string topic_type; - size_t qos; + int qos_depth; std::string frame_id; std::string child_frame_id; bool transient_local; From f8a6bb5869169683386f6d9bfbc41dd948902437 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 21:35:54 +0900 Subject: [PATCH 19/26] feat: add config and delete arg from launch Signed-off-by: TetsuKawa --- .../CMakeLists.txt | 1 + .../config/topic_relay_controller.param.yaml | 14 ++++++++++ .../launch/topic_relay_controller.launch.xml | 24 ++-------------- .../topic_relay_controller_tf.launch.xml | 28 ------------------- .../src/topic_relay_controller_node.cpp | 4 +-- 5 files changed, 20 insertions(+), 51 deletions(-) create mode 100644 system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml delete mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt index f244bf8122f82..768affd7fd121 100644 --- a/system/autoware_topic_relay_controller/CMakeLists.txt +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -15,5 +15,6 @@ rclcpp_components_register_node(${PROJECT_NAME} ) ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml new file mode 100644 index 0000000000000..e2edae0487568 --- /dev/null +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + topic: "/default" + topic_type: "std_msgs/msg/String" # neccessary for /tf or /tf_static + remap_topic: "/remap_default" + #frame_id: neccessary for /tf or /tf_static + #child_frame_id: neccessary for /tf or /tf_static + qos_depth: 1 + transient_local: false + best_effort: false + enable_relay_control: true + srv_name: "/system/topic_relay_controller_defalt/operate" # neccessary for enable_relay_control is true + enable_keep_publishing: true + update_rate: 10 #neccessary for enable_keep_publishing is true diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index ae642dad39f93..f4c6fa43a8d61 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -1,26 +1,8 @@ - - - - - - - - - - - + + - - - - - - - - - - + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml deleted file mode 100644 index 648d53cf4e1b7..0000000000000 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 655c46b08d043..fa1efe6c77d62 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -29,9 +29,9 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); node_param_.enable_relay_control = declare_parameter("enable_relay_control"); - node_param_.srv_name = declare_parameter("srv_name"); + if(node_param_.enable_relay_control) node_param_.srv_name = declare_parameter("srv_name"); node_param_.enable_keep_publishing = declare_parameter("enable_keep_publishing"); - node_param_.update_rate = declare_parameter("update_rate"); + if(node_param_.enable_keep_publishing) node_param_.update_rate = declare_parameter("update_rate"); if (node_param_.is_transform) { node_param_.frame_id = declare_parameter("frame_id"); From f3e6a03c5a80db4986fc5151654e4ab2598fc501 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 21:46:15 +0900 Subject: [PATCH 20/26] modify: typo Signed-off-by: TetsuKawa --- .../config/topic_relay_controller.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml index e2edae0487568..84a7f9bc09f59 100644 --- a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -9,6 +9,6 @@ transient_local: false best_effort: false enable_relay_control: true - srv_name: "/system/topic_relay_controller_defalt/operate" # neccessary for enable_relay_control is true - enable_keep_publishing: true - update_rate: 10 #neccessary for enable_keep_publishing is true + srv_name: "/system/topic_relay_controller_default/operate" # neccessary for enable_relay_control is true + enable_keep_publishing: false + #update_rate: #neccessary for enable_keep_publishing is true From 777f0e59ff0d8811d63956b008c46f09a673ef36 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Jan 2025 12:51:27 +0000 Subject: [PATCH 21/26] style(pre-commit): autofix --- .../src/topic_relay_controller_node.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index fa1efe6c77d62..88056c271cb40 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -29,9 +29,11 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); node_param_.enable_relay_control = declare_parameter("enable_relay_control"); - if(node_param_.enable_relay_control) node_param_.srv_name = declare_parameter("srv_name"); + if (node_param_.enable_relay_control) + node_param_.srv_name = declare_parameter("srv_name"); node_param_.enable_keep_publishing = declare_parameter("enable_keep_publishing"); - if(node_param_.enable_keep_publishing) node_param_.update_rate = declare_parameter("update_rate"); + if (node_param_.enable_keep_publishing) + node_param_.update_rate = declare_parameter("update_rate"); if (node_param_.is_transform) { node_param_.frame_id = declare_parameter("frame_id"); From 6f46373927e13d8401ee23cdfcbcb6391110109a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 22:12:52 +0900 Subject: [PATCH 22/26] modify: add comment Signed-off-by: TetsuKawa --- .../config/topic_relay_controller.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml index 84a7f9bc09f59..f4b6cccfec944 100644 --- a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -6,8 +6,8 @@ #frame_id: neccessary for /tf or /tf_static #child_frame_id: neccessary for /tf or /tf_static qos_depth: 1 - transient_local: false - best_effort: false + transient_local: false # Change qos to transient_local. Default is volatile. + best_effort: false # Change qos to best_effort. Default is reliable. enable_relay_control: true srv_name: "/system/topic_relay_controller_default/operate" # neccessary for enable_relay_control is true enable_keep_publishing: false From caedca6e6695c76c2f8c18c4300b340430a559a6 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 22:13:39 +0900 Subject: [PATCH 23/26] modify: modify comment Signed-off-by: TetsuKawa --- .../config/topic_relay_controller.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml index f4b6cccfec944..204679e4be11a 100644 --- a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: topic: "/default" - topic_type: "std_msgs/msg/String" # neccessary for /tf or /tf_static + topic_type: "std_msgs/msg/String" # unneccessary for /tf or /tf_static remap_topic: "/remap_default" #frame_id: neccessary for /tf or /tf_static #child_frame_id: neccessary for /tf or /tf_static From 1004a06c067e0ce1012c8616f0d8e3bd5ee38d3e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 23:41:21 +0900 Subject: [PATCH 24/26] feat: add maintainer Signed-off-by: TetsuKawa --- system/autoware_topic_relay_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml index 537774e67b998..771c7e632f49a 100644 --- a/system/autoware_topic_relay_controller/package.xml +++ b/system/autoware_topic_relay_controller/package.xml @@ -4,6 +4,7 @@ autoware_topic_relay_controller 0.1.0 The topic_relay_controller ROS 2 package + Makoto Kurihara Tetsuhiro Kawaguchi Apache License 2.0 From e3f5d058265775654f3ac8a9cdbb55ac7e351337 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 22 Jan 2025 23:42:11 +0900 Subject: [PATCH 25/26] modift: change readme param format Signed-off-by: TetsuKawa --- .../autoware_topic_relay_controller/README.md | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md index e3c6a4075aa04..0863d59564f60 100644 --- a/system/autoware_topic_relay_controller/README.md +++ b/system/autoware_topic_relay_controller/README.md @@ -22,18 +22,20 @@ The node subscribes to a specified topic, remaps it, and republishes it. Additio ## Parameters -- **`topic`** (string): The name of the input topic to subscribe to. -- **`remap_topic`** (string): The name of the output topic to publish to. -- **`topic_type`** (string): The type of messages being relayed. -- **`qos`** (integer, default: `1`): QoS profile to use for subscriptions and publications. -- **`transient_local`** (boolean, default: `false`): Enables transient local QoS for subscribers. -- **`best_effort`** (boolean, default: `false`): Enables best-effort QoS for subscribers. -- **`enable_relay_control`** (boolean, default: `true`): Allows dynamic relay control via a service. -- **`srv_name`** (string): The service name for relay control when `enable_relay_control` is `true`. -- **`enable_keep_publishing`** (boolean, default: `false`): Keeps publishing the last received topic value when not subscribed. -- **`update_rate`** (integer, default: `10`): The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true`. -- **`frame_id`** (string, optional): Frame ID for transform messages when subscribing to `/tf` or `/tf_static`. -- **`child_frame_id`** (string, optional): Child frame ID for transform messages when subscribing to `/tf` or `/tf_static`. +| Variable | Type | Description | +|----------|------|-------------| +| topic | string | The name of the input topic to subscribe to | +| remap_topic | string | The name of the output topic to publish to | +| topic_type | string | The type of messages being relayed | +| qos | integer | QoS profile to use for subscriptions and publications (default: `1`) | +| transient_local | boolean | Enables transient local QoS for subscribers (default: `false`) | +| best_effort | boolean | Enables best-effort QoS for subscribers (default: `false`) | +| enable_relay_control | boolean | Allows dynamic relay control via a service (default: `true`) | +| srv_name | string | The service name for relay control when `enable_relay_control` is `true` | +| enable_keep_publishing | boolean | Keeps publishing the last received topic value when not subscribed (default: `false`) | +| update_rate | integer | The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true` (optional) | +| frame_id | string | Frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | +| child_frame_id | string | Child frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | ## Assumptions / Known limits From 9430a164c29c2ac62be2b7289fc9abf41d87a349 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Jan 2025 14:45:08 +0000 Subject: [PATCH 26/26] style(pre-commit): autofix --- .../autoware_topic_relay_controller/README.md | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md index 0863d59564f60..a5651ff32fbd2 100644 --- a/system/autoware_topic_relay_controller/README.md +++ b/system/autoware_topic_relay_controller/README.md @@ -22,20 +22,20 @@ The node subscribes to a specified topic, remaps it, and republishes it. Additio ## Parameters -| Variable | Type | Description | -|----------|------|-------------| -| topic | string | The name of the input topic to subscribe to | -| remap_topic | string | The name of the output topic to publish to | -| topic_type | string | The type of messages being relayed | -| qos | integer | QoS profile to use for subscriptions and publications (default: `1`) | -| transient_local | boolean | Enables transient local QoS for subscribers (default: `false`) | -| best_effort | boolean | Enables best-effort QoS for subscribers (default: `false`) | -| enable_relay_control | boolean | Allows dynamic relay control via a service (default: `true`) | -| srv_name | string | The service name for relay control when `enable_relay_control` is `true` | -| enable_keep_publishing | boolean | Keeps publishing the last received topic value when not subscribed (default: `false`) | -| update_rate | integer | The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true` (optional) | -| frame_id | string | Frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | -| child_frame_id | string | Child frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | +| Variable | Type | Description | +| ---------------------- | ------- | ---------------------------------------------------------------------------------------------------- | +| topic | string | The name of the input topic to subscribe to | +| remap_topic | string | The name of the output topic to publish to | +| topic_type | string | The type of messages being relayed | +| qos | integer | QoS profile to use for subscriptions and publications (default: `1`) | +| transient_local | boolean | Enables transient local QoS for subscribers (default: `false`) | +| best_effort | boolean | Enables best-effort QoS for subscribers (default: `false`) | +| enable_relay_control | boolean | Allows dynamic relay control via a service (default: `true`) | +| srv_name | string | The service name for relay control when `enable_relay_control` is `true` | +| enable_keep_publishing | boolean | Keeps publishing the last received topic value when not subscribed (default: `false`) | +| update_rate | integer | The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true` (optional) | +| frame_id | string | Frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | +| child_frame_id | string | Child frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | ## Assumptions / Known limits