feat: add publisher to topic relay controller

system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml

@@ -6,6 +6,10 @@
   <arg name="qos" default="1" description="QoS profile"/>
   <arg name="transient_local" default="false" description="add transient_local option to subscriber or not"/>
   <arg name="best_effort" default="false" description="add best_effort option to subscriber or not"/>
+  <arg name="enable_relay_control" default="true" description="enable relay control or not"/>
+  <arg name="srv_name" default="/system/topic_relay_controller_$(var node_name_suffix)/operate" description="service name for relay control"/>
+  <arg name="enable_keep_publishing" default="false" description="enable keep publish last topic when not subscribed"/>
+  <arg name="update_rate" default="10" description="update rate for topic publish"/>
 
   <node pkg="autoware_topic_relay_controller" exec="autoware_topic_relay_controller_node" name="topic_relay_controller_$(var node_name_suffix)" output="screen">
     <param name="topic" value="$(var topic)"/>
@@ -14,5 +18,9 @@
     <param name="qos" value="$(var qos)"/>
     <param name="transient_local" value="$(var transient_local)"/>
     <param name="best_effort" value="$(var best_effort)"/>
+    <param name="enable_relay_control" value="$(var enable_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
+    <param name="enable_keep_publishing" value="$(var enable_keep_publishing)"/>
+    <param name="update_rate" value="$(var update_rate)"/>
   </node>
 </launch>

system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml

@@ -7,6 +7,10 @@
   <arg name="child_frame_id" description="child frame id"/>
   <arg name="transient_local" default="false" description="add transient_local option to subscriber or not"/>
   <arg name="best_effort" default="false" description="add best_effort option to subscriber or not"/>
+  <arg name="enable_relay_control" default="true" description="enable relay control or not"/>
+  <arg name="srv_name" default="/system/topic_relay_controller_$(var node_name_suffix)/operate" description="service name for relay control"/>
+  <arg name="enable_keep_publishing" default="false" description="enable keep publish last topic when not subscribed"/>
+  <arg name="update_rate" default="10" description="update rate for tf publish"/>
 
   <node pkg="autoware_topic_relay_controller" exec="autoware_topic_relay_controller_node" name="topic_relay_controller_$(var node_name_suffix)" output="screen">
     <param name="topic" value="$(var topic)"/>
@@ -16,5 +20,9 @@
     <param name="child_frame_id" value="$(var child_frame_id)"/>
     <param name="transient_local" value="$(var transient_local)"/>
     <param name="best_effort" value="$(var best_effort)"/>
+    <param name="enable_relay_control" value="$(var enable_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
+    <param name="enable_keep_publishing" value="$(var enable_keep_publishing)"/>
+    <param name="update_rate" value="$(var update_rate)"/>
   </node>
 </launch>

system/autoware_topic_relay_controller/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2_msgs</depend>
+  <depend>tier4_system_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp

@@ -18,15 +18,20 @@
 
 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<std::string>("topic");
   node_param_.remap_topic = declare_parameter<std::string>("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");
+  node_param_.enable_relay_control = declare_parameter<bool>("enable_relay_control");
+  node_param_.srv_name = declare_parameter<std::string>("srv_name");
+  node_param_.enable_keep_publishing = declare_parameter<bool>("enable_keep_publishing");
+  node_param_.update_rate = declare_parameter<int>("update_rate");
 
   if (node_param_.is_transform) {
     node_param_.frame_id = declare_parameter<std::string>("frame_id");
@@ -35,6 +40,19 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options)
     node_param_.topic_type = declare_parameter<std::string>("topic_type");
   }
 
+  // Service
+  if (node_param_.enable_relay_control) {
+    srv_change_relay_control_ = create_service<tier4_system_msgs::srv::ChangeTopicRelayControl>(
+      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;
+        RCLCPP_INFO(get_logger(), "relay control: %s", is_relaying_ ? "ON" : "OFF");
+        response->status.success = true;
+      });
+  }
+
   // Subscriber
   rclcpp::QoS qos = rclcpp::QoS{node_param_.qos};
   if (node_param_.transient_local) {
@@ -45,27 +63,57 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options)
   }
 
   if (node_param_.is_transform) {
+    // Publisher
+    pub_transform_ = this->create_publisher<tf2_msgs::msg::TFMessage>(node_param_.remap_topic, qos);
+
     sub_transform_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
-      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) {
-            RCLCPP_INFO (
-              this->get_logger(), "Received transform from %s to %s",
-              transform.header.frame_id.c_str(), transform.child_frame_id.c_str());
+            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_ =
+      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<rclcpp::SerializedMessage> msg) {
-        RCLCPP_INFO(this->get_logger(), "Received message");
+        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
+}  // namespace autoware::topic_relay_controller
 
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController)

system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp

@@ -12,13 +12,14 @@
 // 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 <rclcpp/rclcpp.hpp>
 
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <tier4_system_msgs/srv/change_topic_relay_control.hpp>
 
 #include <memory>
 #include <string>
@@ -36,6 +37,10 @@ struct NodeParam
   bool transient_local;
   bool best_effort;
   bool is_transform;
+  bool enable_relay_control;
+  std::string srv_name;
+  bool enable_keep_publishing;
+  int update_rate;
 };
 
 class TopicRelayController : public rclcpp::Node
@@ -50,7 +55,23 @@ class TopicRelayController : public rclcpp::Node
   // Subscriber
   rclcpp::GenericSubscription::SharedPtr sub_topic_;
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_transform_;
+
+  // Publisher
+  rclcpp::GenericPublisher::SharedPtr pub_topic_;
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_transform_;
+
+  // Service
+  rclcpp::Service<tier4_system_msgs::srv::ChangeTopicRelayControl>::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<rclcpp::SerializedMessage> last_topic_;
 };
 }  // namespace autoware::topic_relay_controller
 
-#endif  // TOPIC_RELAY_CONTROLLER_HPP_
+#endif  // TOPIC_RELAY_CONTROLLER_NODE_HPP_