feat: add enable_keep_publishing

Signed-off-by: TetsuKawa <[email protected]>

system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml

@@ -8,6 +8,8 @@
   <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)"/>
@@ -18,5 +20,7 @@
     <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

@@ -8,6 +8,8 @@
   <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)"/>
@@ -18,5 +20,7 @@
     <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/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<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");
@@ -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<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 && 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<rclcpp::SerializedMessage> 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

system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp

@@ -38,6 +38,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
@@ -61,8 +63,13 @@ class TopicRelayController : public rclcpp::Node
   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