Add qos rules for tf_static topic

ros2 · Oct 24, 2023 · caef6ee · caef6ee
1 parent 689a932
commit caef6ee
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 3 deletions.
diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp
@@ -116,6 +116,7 @@ create_bridge_from_2_to_1(
   const std::string & ros1_type_name,
   const std::string & ros1_topic_name,
   size_t publisher_queue_size,
+  bool publisher_latch,
   rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr);
 
 BridgeHandles

diff --git a/src/bridge.cpp b/src/bridge.cpp
@@ -79,6 +79,7 @@ create_bridge_from_2_to_1(
   rclcpp::PublisherBase::SharedPtr ros2_pub)
 {
   auto subscriber_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(subscriber_queue_size));
+  auto publisher_latch = false;
   return create_bridge_from_2_to_1(
     ros2_node,
     ros1_node,
@@ -88,6 +89,7 @@ create_bridge_from_2_to_1(
     ros1_type_name,
     ros1_topic_name,
     publisher_queue_size,
+    publisher_latch,
     ros2_pub);
 }
 
@@ -101,11 +103,12 @@ create_bridge_from_2_to_1(
   const std::string & ros1_type_name,
   const std::string & ros1_topic_name,
   size_t publisher_queue_size,
+  bool publisher_latch,
   rclcpp::PublisherBase::SharedPtr ros2_pub)
 {
   auto factory = get_factory(ros1_type_name, ros2_type_name);
   auto ros1_pub = factory->create_ros1_publisher(
-    ros1_node, ros1_topic_name, publisher_queue_size);
+    ros1_node, ros1_topic_name, publisher_queue_size, publisher_latch);
 
   auto ros2_sub = factory->create_ros2_subscriber(
     ros2_node, ros2_topic_name, subscriber_qos, ros1_pub, ros2_pub);

diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp
@@ -254,11 +254,20 @@ void update_bridge(
     bridge.ros1_type_name = ros1_type_name;
     bridge.ros2_type_name = ros2_type_name;
 
+    auto ros2_subscriber_qos = rclcpp::QoS(rclcpp::KeepLast(10));
+    auto ros_publisher_latch = false;
+    if (topic_name == "/tf_static") {
+      ros2_subscriber_qos.keep_all();
+      ros2_subscriber_qos.transient_local();
+      ros2_subscriber_qos.reliable();
+      ros_publisher_latch = true;
+    }
+
     try {
       bridge.bridge_handles = ros1_bridge::create_bridge_from_2_to_1(
         ros2_node, ros1_node,
-        bridge.ros2_type_name, topic_name, 10,
-        bridge.ros1_type_name, topic_name, 10);
+        bridge.ros2_type_name, topic_name, ros2_subscriber_qos,
+        bridge.ros1_type_name, topic_name, 10, ros_publisher_latch);
     } catch (std::runtime_error & e) {
       fprintf(
         stderr,