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