feat: add service

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

system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml

@@ -6,6 +6,8 @@
   <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="enabele_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"/>
 
   <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 +16,7 @@
     <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 enabele_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
   </node>
 </launch>

system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml

@@ -6,6 +6,8 @@
   <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="enabele_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"/>
 
   <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 +16,7 @@
     <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 enabele_relay_control)"/>
+    <param name="srv_name" value="$(var srv_name)"/>
   </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

@@ -15,7 +15,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<std::string>("topic");
@@ -24,6 +24,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<bool>("enable_relay_control");
+  node_param_.srv_name = declare_parameter<std::string>("srv_name");
 
   if (node_param_.is_transform) {
     node_param_.frame_id = declare_parameter<std::string>("frame_id");
@@ -32,6 +34,17 @@ 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;
+        response->status.success = true;
+      });
+  }
+
   // Subscriber
   rclcpp::QoS qos = rclcpp::QoS{node_param_.qos};
   if (node_param_.transient_local) {
@@ -51,7 +64,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);
           }
         }
@@ -64,7 +78,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<rclcpp::SerializedMessage> msg) {
-        pub_topic_->publish(*msg);
+        if (is_relaying_) pub_topic_->publish(*msg);
       });
   }
 }

system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp

@@ -19,6 +19,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <tier4_system_msgs/srv/change_topic_relay_control.hpp>
 
 namespace autoware::topic_relay_controller
 {
@@ -33,6 +34,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
@@ -51,6 +54,12 @@ class TopicRelayController : public rclcpp::Node
   // 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_;
+
+  // State
+  bool is_relaying_;
 };
 }  // namespace autoware::topic_relay_controller