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..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
@@ -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,11 +67,16 @@ 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);
}
}
@@ -81,9 +89,29 @@ 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