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

@@ -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">

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', '')

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"

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"

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"

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"

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"\

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"

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"

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"

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">

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"

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\"

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"

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">

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"

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">

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"

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"

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"

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"

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"

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">

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"

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">

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"