diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt new file mode 100644 index 0000000000000..f244bf8122f82 --- /dev/null +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -0,0 +1,19 @@ +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 +) 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/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml new file mode 100644 index 0000000000000..4165cf7ea741b --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + 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..8a497982dd813 --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml new file mode 100644 index 0000000000000..537774e67b998 --- /dev/null +++ b/system/autoware_topic_relay_controller/package.xml @@ -0,0 +1,24 @@ + + + + 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 + tf2_msgs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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..ace592f3c875e --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -0,0 +1,91 @@ +// 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" + +#include +#include + +namespace autoware::topic_relay_controller +{ +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_.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"); + node_param_.child_frame_id = declare_parameter("child_frame_id"); + } else { + 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) { + qos.transient_local(); + } + if (node_param_.best_effort) { + qos.best_effort(); + } + + 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 && is_relaying_) { + 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) { + if (is_relaying_) pub_topic_->publish(*msg); + }); + } +} +} // 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..14ac259bbee6e --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -0,0 +1,69 @@ +// 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 TOPIC_RELAY_CONTROLLER_NODE_HPP_ +#define TOPIC_RELAY_CONTROLLER_NODE_HPP_ + +// ROS 2 core +#include + +#include +#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; + bool enable_relay_control; + std::string srv_name; +}; + +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_; + + // 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 + +#endif // TOPIC_RELAY_CONTROLLER_NODE_HPP_