From e615b3160a71ab6a0f00cfbec109830f36125cc8 Mon Sep 17 00:00:00 2001
From: iche033 <ichen@osrfoundation.org>
Date: Thu, 14 May 2020 13:46:27 -0700
Subject: [PATCH] Reduce /<robot_name>/pose topic bandwidth (#402)

* check topic heartbeat and log /X*/pose topics

* use pose_v and subt to pose_static

* cleanup

* Rename parameters

* move heartbeat check into its own thread

Co-authored-by: Nate Koenig <nate@openrobotics.org>
---
 ign_migration_scripts/launch/preview.ign      |  6 +++
 .../scripts/spawn_preview_model.py            |  3 ++
 .../launch/spawner.rb                         |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../launch/spawner.rb                         |  3 ++
 .../launch/vehicle_topics.launch              | 10 +++-
 .../launch/spawner.rb                         |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../launch/spawner.rb                         |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../launch/spawner.rb                         |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../ssci_x2_sensor_config_1/launch/spawner.rb |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../ssci_x4_sensor_config_1/launch/spawner.rb |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../ssci_x4_sensor_config_2/launch/spawner.rb |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../x1_sensor_config_6/launch/spawner.rb      |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../x2_sensor_config_7/launch/spawner.rb      |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../x3_sensor_config_5/launch/spawner.rb      |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 .../x4_sensor_config_6/launch/spawner.rb      |  3 ++
 .../launch/vehicle_topics.launch              |  9 +++-
 subt_ign/launch/cave_circuit.ign              | 12 +++++
 subt_ign/launch/cloudsim_sim.ign              | 12 +++++
 subt_ign/launch/competition.ign               | 12 +++++
 subt_ign/launch/competition_no_ros.ign        | 12 +++++
 subt_ign/launch/tunnel_circuit_practice.ign   | 12 +++++
 subt_ign/launch/urban_circuit.ign             | 12 +++++
 subt_ign/launch/validate_connections.ign      |  3 ++
 subt_ign/launch/virtual_stix.ign              | 12 +++++
 subt_ign/launch/virtual_stix_headless.ign     | 12 +++++
 subt_ign/test/test.ign                        | 12 +++++
 subt_ros/launch/vehicle_topics.launch         |  9 +++-
 subt_ros/src/BridgeLogger.cc                  | 51 +++++++++++++++++++
 subt_ros/src/pose_tf_broadcaster.cpp          | 21 ++++++--
 39 files changed, 327 insertions(+), 19 deletions(-)

diff --git a/ign_migration_scripts/launch/preview.ign b/ign_migration_scripts/launch/preview.ign
index 56ac8797..e0c13ca0 100644
--- a/ign_migration_scripts/launch/preview.ign
+++ b/ign_migration_scripts/launch/preview.ign
@@ -210,6 +210,9 @@
             <publish_collision_pose>false</publish_collision_pose>
             <publish_visual_pose>false</publish_visual_pose>
             <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+            <use_pose_vector_msg>true</use_pose_vector_msg>
+            <static_publisher>true</static_publisher>
+            <static_update_frequency>1</static_update_frequency>
           </plugin>
           <!-- Battery plugin -->
         </include>
@@ -256,6 +259,9 @@
             <publish_collision_pose>false</publish_collision_pose>
             <publish_visual_pose>false</publish_visual_pose>
             <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+            <use_pose_vector_msg>true</use_pose_vector_msg>
+            <static_publisher>true</static_publisher>
+            <static_update_frequency>1</static_update_frequency>
           </plugin>
           <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
             name="ignition::gazebo::systems::MulticopterMotorModel">
diff --git a/ign_migration_scripts/scripts/spawn_preview_model.py b/ign_migration_scripts/scripts/spawn_preview_model.py
index 7d8eebfc..5d64a928 100755
--- a/ign_migration_scripts/scripts/spawn_preview_model.py
+++ b/ign_migration_scripts/scripts/spawn_preview_model.py
@@ -74,6 +74,9 @@ def spawn_preview_model(args):
     etree.SubElement(plugin, 'publish_collision_pose').text = 'false'
     etree.SubElement(plugin, 'publish_visual_pose').text = 'false'
     etree.SubElement(plugin, 'publish_nested_model_pose').text = 'true'
+    etree.SubElement(plugin, 'use_pose_vector_msg').text = 'true'
+    etree.SubElement(plugin, 'static_publisher').text = 'true'
+    etree.SubElement(plugin, 'static_update_frequency').text = '1'
 
     sdf_esc = etree.tostring(sdf, encoding="unicode",
                              with_tail=False).replace('"', r'\"').replace('\n', '')
diff --git a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb
index 0b481bc2..ca4c1fec 100644
--- a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb
@@ -30,6 +30,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
diff --git a/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
index 98c19afc..a4246a61 100644
--- a/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
@@ -10,9 +10,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
 
     <node
       pkg="subt_ros"
diff --git a/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb b/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb
index c7b2ae5f..7ab3fb0f 100644
--- a/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb
@@ -30,6 +30,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
diff --git a/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch
index 0c16d16b..d7a3174a 100644
--- a/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch
@@ -10,10 +10,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
-
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="subt_ros"
       type="pose_tf_broadcaster"
diff --git a/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb b/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb
index e8268354..eb7243e3 100644
--- a/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb
@@ -29,6 +29,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
   "      <publish_collision_pose>false</publish_collision_pose>\n"\
   "      <publish_visual_pose>false</publish_visual_pose>\n"\
   "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+  "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+  "      <static_publisher>true</static_publisher>\n"\
+  "      <static_update_frequency>1</static_update_frequency>\n"\
   "    </plugin>\n"\
   "    <!-- Battery plugin -->\n"\
   "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
diff --git a/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
index 36a32246..ba429f04 100644
--- a/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -24,9 +24,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb
index 1e3c4ae7..6d7fdfe0 100644
--- a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb
@@ -32,6 +32,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
diff --git a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/vehicle_topics.launch
index abcf95e0..85116ced 100644
--- a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -18,9 +18,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb
index b654bed8..6ead02b8 100644
--- a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb
@@ -21,6 +21,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
         name="ignition::gazebo::systems::MulticopterMotorModel">
diff --git a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
index ad1c2784..fe30a5eb 100644
--- a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
@@ -18,9 +18,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb b/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb
index aa5fbcca..18e94253 100755
--- a/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb
@@ -44,6 +44,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
           <publish_collision_pose>false</publish_collision_pose>
           <publish_visual_pose>false</publish_visual_pose>
           <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+          <use_pose_vector_msg>true</use_pose_vector_msg>
+          <static_publisher>true</static_publisher>
+          <static_update_frequency>1</static_update_frequency>
         </plugin>
         <!-- Battery plugin -->
         <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"
diff --git a/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
index af476d2d..3b820aeb 100755
--- a/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -36,9 +36,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb b/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb
index c9524512..8ff44c83 100755
--- a/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb
@@ -43,6 +43,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
           <publish_collision_pose>false</publish_collision_pose>
           <publish_visual_pose>false</publish_visual_pose>
           <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+          <use_pose_vector_msg>true</use_pose_vector_msg>
+          <static_publisher>true</static_publisher>
+          <static_update_frequency>1</static_update_frequency>
         </plugin>
         <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
           name="ignition::gazebo::systems::MulticopterMotorModel">
diff --git a/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch
index 25758838..9e82ad3f 100755
--- a/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch
@@ -34,9 +34,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb b/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb
index fadc986c..b1155949 100755
--- a/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb
+++ b/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb
@@ -38,6 +38,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
           <publish_collision_pose>false</publish_collision_pose>
           <publish_visual_pose>false</publish_visual_pose>
           <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+          <use_pose_vector_msg>true</use_pose_vector_msg>
+          <static_publisher>true</static_publisher>
+          <static_update_frequency>1</static_update_frequency>
         </plugin>
         <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
           name="ignition::gazebo::systems::MulticopterMotorModel">
diff --git a/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch b/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch
index 04651d28..a59e43f0 100755
--- a/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch
+++ b/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch
@@ -34,9 +34,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/x1_sensor_config_6/launch/spawner.rb b/submitted_models/x1_sensor_config_6/launch/spawner.rb
index 66e45104..87ccf702 100644
--- a/submitted_models/x1_sensor_config_6/launch/spawner.rb
+++ b/submitted_models/x1_sensor_config_6/launch/spawner.rb
@@ -30,6 +30,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
diff --git a/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch b/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch
index fa93959c..9d90f2e2 100644
--- a/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch
+++ b/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch
@@ -56,9 +56,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/x2_sensor_config_7/launch/spawner.rb b/submitted_models/x2_sensor_config_7/launch/spawner.rb
index 06d2e34d..90c8acfc 100644
--- a/submitted_models/x2_sensor_config_7/launch/spawner.rb
+++ b/submitted_models/x2_sensor_config_7/launch/spawner.rb
@@ -30,6 +30,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
diff --git a/submitted_models/x2_sensor_config_7/launch/vehicle_topics.launch b/submitted_models/x2_sensor_config_7/launch/vehicle_topics.launch
index 0fb7a05e..f5539211 100644
--- a/submitted_models/x2_sensor_config_7/launch/vehicle_topics.launch
+++ b/submitted_models/x2_sensor_config_7/launch/vehicle_topics.launch
@@ -56,9 +56,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/x3_sensor_config_5/launch/spawner.rb b/submitted_models/x3_sensor_config_5/launch/spawner.rb
index 8c4f8a14..74547351 100644
--- a/submitted_models/x3_sensor_config_5/launch/spawner.rb
+++ b/submitted_models/x3_sensor_config_5/launch/spawner.rb
@@ -19,6 +19,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
         name="ignition::gazebo::systems::MulticopterMotorModel">
diff --git a/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch b/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch
index 69dd96fc..e8fda481 100644
--- a/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch
+++ b/submitted_models/x3_sensor_config_5/launch/vehicle_topics.launch
@@ -24,9 +24,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/submitted_models/x4_sensor_config_6/launch/spawner.rb b/submitted_models/x4_sensor_config_6/launch/spawner.rb
index abbf4de7..7b201847 100644
--- a/submitted_models/x4_sensor_config_6/launch/spawner.rb
+++ b/submitted_models/x4_sensor_config_6/launch/spawner.rb
@@ -19,6 +19,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
           <publish_collision_pose>false</publish_collision_pose>
           <publish_visual_pose>false</publish_visual_pose>
           <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+          <use_pose_vector_msg>true</use_pose_vector_msg>
+          <static_publisher>true</static_publisher>
+          <static_update_frequency>1</static_update_frequency>
         </plugin>
         <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
           name="ignition::gazebo::systems::MulticopterMotorModel">
diff --git a/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch b/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch
index 69dd96fc..e8fda481 100644
--- a/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch
+++ b/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch
@@ -24,9 +24,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign
index 7f8d845e..20b0b572 100644
--- a/subt_ign/launch/cave_circuit.ign
+++ b/subt_ign/launch/cave_circuit.ign
@@ -487,6 +487,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -567,6 +570,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -633,6 +639,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -829,6 +838,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign
index 95e6faba..4c45e4bb 100644
--- a/subt_ign/launch/cloudsim_sim.ign
+++ b/subt_ign/launch/cloudsim_sim.ign
@@ -553,6 +553,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -612,6 +615,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -660,6 +666,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -835,6 +844,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign
index 90fd87dc..c9ed0698 100644
--- a/subt_ign/launch/competition.ign
+++ b/subt_ign/launch/competition.ign
@@ -432,6 +432,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -502,6 +505,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -558,6 +564,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -744,6 +753,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/competition_no_ros.ign b/subt_ign/launch/competition_no_ros.ign
index f829fe12..66f02656 100644
--- a/subt_ign/launch/competition_no_ros.ign
+++ b/subt_ign/launch/competition_no_ros.ign
@@ -431,6 +431,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -495,6 +498,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -545,6 +551,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -725,6 +734,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/tunnel_circuit_practice.ign b/subt_ign/launch/tunnel_circuit_practice.ign
index e1779cce..ff687404 100644
--- a/subt_ign/launch/tunnel_circuit_practice.ign
+++ b/subt_ign/launch/tunnel_circuit_practice.ign
@@ -424,6 +424,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -494,6 +497,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -550,6 +556,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -736,6 +745,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/urban_circuit.ign b/subt_ign/launch/urban_circuit.ign
index e9afb7d4..3b521d2d 100644
--- a/subt_ign/launch/urban_circuit.ign
+++ b/subt_ign/launch/urban_circuit.ign
@@ -448,6 +448,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -528,6 +531,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -594,6 +600,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -790,6 +799,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/validate_connections.ign b/subt_ign/launch/validate_connections.ign
index 4befbf4a..d0ed7fbe 100644
--- a/subt_ign/launch/validate_connections.ign
+++ b/subt_ign/launch/validate_connections.ign
@@ -385,6 +385,9 @@ ign service -s /connection/prev \
           <publish_collision_pose>false</publish_collision_pose>
           <publish_visual_pose>false</publish_visual_pose>
           <publish_nested_model_pose>true</publish_nested_model_pose>
+          <use_pose_vector_msg>true</use_pose_vector_msg>
+          <static_publisher>true</static_publisher>
+          <static_update_frequency>1</static_update_frequency>
         </plugin>
       </model>
     </sdf>
diff --git a/subt_ign/launch/virtual_stix.ign b/subt_ign/launch/virtual_stix.ign
index 9e692d1b..ffb03255 100644
--- a/subt_ign/launch/virtual_stix.ign
+++ b/subt_ign/launch/virtual_stix.ign
@@ -394,6 +394,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -464,6 +467,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -520,6 +526,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -706,6 +715,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/launch/virtual_stix_headless.ign b/subt_ign/launch/virtual_stix_headless.ign
index 21ef5ac9..d831873c 100644
--- a/subt_ign/launch/virtual_stix_headless.ign
+++ b/subt_ign/launch/virtual_stix_headless.ign
@@ -306,6 +306,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -376,6 +379,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -432,6 +438,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -618,6 +627,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ign/test/test.ign b/subt_ign/test/test.ign
index fcbdd917..d814e78f 100644
--- a/subt_ign/test/test.ign
+++ b/subt_ign/test/test.ign
@@ -325,6 +325,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -388,6 +391,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <!-- Battery plugin -->\n"\
     "    <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
@@ -437,6 +443,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
@@ -565,6 +574,9 @@
     "      <publish_collision_pose>false</publish_collision_pose>\n"\
     "      <publish_visual_pose>false</publish_visual_pose>\n"\
     "      <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
+    "      <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
+    "      <static_publisher>true</static_publisher>\n"\
+    "      <static_update_frequency>1</static_update_frequency>\n"\
     "    </plugin>\n"\
     "    <plugin filename=\"libignition-gazebo-multicopter-motor-model-system.so\"\n"\
     "      name=\"ignition::gazebo::systems::MulticopterMotorModel\">\n"\
diff --git a/subt_ros/launch/vehicle_topics.launch b/subt_ros/launch/vehicle_topics.launch
index 81c59326..8e76f2f8 100644
--- a/subt_ros/launch/vehicle_topics.launch
+++ b/subt_ros/launch/vehicle_topics.launch
@@ -23,9 +23,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
diff --git a/subt_ros/src/BridgeLogger.cc b/subt_ros/src/BridgeLogger.cc
index afea780c..492a9ebe 100644
--- a/subt_ros/src/BridgeLogger.cc
+++ b/subt_ros/src/BridgeLogger.cc
@@ -16,6 +16,7 @@
 */
 #include <chrono>
 #include <mutex>
+#include <thread>
 #include <fstream>
 #include <ros/ros.h>
 #include <std_msgs/String.h>
@@ -51,6 +52,9 @@ class BridgeLogger
   private: void OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg,
                             const std::string &_topic);
 
+  /// \brief Check heartbeat for all topics monitored
+  private: void CheckHeartbeat();
+
   /// \brief ROS node handler.
   private: ros::NodeHandle n;
 
@@ -69,6 +73,14 @@ class BridgeLogger
 
   /// \brief Timer that triggers the update function.
   private: ros::Timer updateTimer;
+
+  /// \brief A map of topic names and time when their last heartbeat was
+  /// received.
+  private: std::map<std::string,
+      std::chrono::time_point<std::chrono::steady_clock>> topicTimestamp;
+
+  /// \brief Check heartbeat in separate thread
+  private: std::thread heartbeatThread;
 };
 
 /////////////////////////////////////////////////
@@ -79,6 +91,8 @@ BridgeLogger::BridgeLogger()
 
   this->updateTimer = this->n.createTimer(ros::Duration(10.0),
       &BridgeLogger::Update, this);
+
+  this->heartbeatThread = std::thread(&BridgeLogger::CheckHeartbeat, this);
 }
 
 /////////////////////////////////////////////////
@@ -211,6 +225,9 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg,
     return;
   }
 
+  // log msg time received in wall clock time
+  this->topicTimestamp[_topic] = systemTime;
+
   // Check if a message was missed.
   if (this->prevSeq[_topic] + 1 != seq && this->prevSeq[_topic] >= 0)
     this->streams[_topic] << "***Error: Missed message(s) ***\n";
@@ -225,6 +242,40 @@ void BridgeLogger::OnSensorMsg(const topic_tools::ShapeShifter::ConstPtr &_msg,
   this->prevSeq[_topic] = seq;
 }
 
+/////////////////////////////////////////////////
+void BridgeLogger::CheckHeartbeat()
+{
+  // check heartbeat
+  using namespace std::chrono_literals;
+  double heartbeatCheckPeriod = 5.0;
+  while (ros::ok)
+  {
+    std::this_thread::sleep_for(5s);
+    auto systemTime = std::chrono::steady_clock::now();
+    {
+      std::lock_guard<std::mutex> lock(this->mutex);
+      // check topic heartbeat
+      for (auto &tIt : this->streams)
+      {
+        std::string topic = tIt.first;
+        auto it = this->topicTimestamp.find(topic);
+        if (it != this->topicTimestamp.end())
+        {
+          auto d = systemTime - it->second;
+          auto seconds =
+              std::chrono::duration_cast<std::chrono::milliseconds>(d).count() /
+              1000.0;
+          if (seconds > heartbeatCheckPeriod)
+          {
+            this->streams[topic] << "***Error: No messages received for more "
+                                 << "than 5 seconds ***\n";
+          }
+        }
+      }
+    }
+  }
+}
+
 /////////////////////////////////////////////////
 int main(int argc, char** argv)
 {
diff --git a/subt_ros/src/pose_tf_broadcaster.cpp b/subt_ros/src/pose_tf_broadcaster.cpp
index 6833bd04..c5242309 100644
--- a/subt_ros/src/pose_tf_broadcaster.cpp
+++ b/subt_ros/src/pose_tf_broadcaster.cpp
@@ -13,8 +13,6 @@
 // limitations under the License.
 
 #include <iostream>
-#include <memory>
-#include <string>
 
 // include ROS 1
 #ifdef __clang__
@@ -27,21 +25,34 @@
 #endif
 
 #include <geometry_msgs/TransformStamped.h>
-#include <tf/transform_broadcaster.h>
+#include <tf2_msgs/TFMessage.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 
-void poseCallback(const geometry_msgs::TransformStamped& msg)
+void poseCallback(const tf2_msgs::TFMessage &_msg)
 {
   static tf2_ros::TransformBroadcaster br;
-  br.sendTransform(msg);
+  br.sendTransform(_msg.transforms);
+}
+
+void poseStaticCallback(const tf2_msgs::TFMessage &_msg)
+{
+   static tf2_ros::StaticTransformBroadcaster brStatic;
+   brStatic.sendTransform(_msg.transforms);
 }
 
 //////////////////////////////////////////////////
 int main(int argc, char * argv[])
 {
   ros::init(argc, argv, "pose_tf_broadcaster");
+
   ros::NodeHandle node;
   ros::Subscriber sub = node.subscribe("pose", 10, &poseCallback);
 
+  ros::Subscriber subStatic = node.subscribe(
+      "pose_static", 10, &poseStaticCallback);
+
   ros::spin();
+
   return 0;
 }