Repository: ouster-lidar/ouster-ros (language: cpp)

Issue:
High TF Broadcast Rate
I noticed in this commit (https://github.com/ouster-lidar/ouster-ros/commit/600c55cae143f98f62e283fa54eac8964177e695) that the transform was changed to support working with ROS time. Maybe I am misunderstanding, but I'm confused why a static transform (from metadata) needs to send an TF2 message for every received IMU and Lidar packet. I notice a large quantity of TF2 messages are being sent but never with dynamically changing data. Could this be throttled in anyway or is it necessary to send the TF2 message at this rate?

We have observed this with all of our sensors:
OS-0 and OS-1
Commit: https://github.com/ouster-lidar/ouster-ros/commit/3c2b6a1bb0eca0b400106cce4ea978deff18a46d



Discussion / hints:

Fix patch:
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 1c82e7e6..6c0d739d 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -29,6 +29,7 @@ ouster_ros(2)
   launch parameters.
 * bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components
   in the same process.
+* switch to using static transform publisher for the ros2 driver.
 
 ouster_client
 -------------
diff --git a/ouster-ros/launch/record.independent.launch.xml b/ouster-ros/launch/record.independent.launch.xml
index 16a234d8..7c03f590 100644
--- a/ouster-ros/launch/record.independent.launch.xml
+++ b/ouster-ros/launch/record.independent.launch.xml
@@ -74,7 +74,6 @@
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>
-    <arg name="_enable_static_tf_publishers" value="true"/>
   </include>
 
   <let name="_use_bag_file_name" value="$(eval '\'$(var bag_file)\' != \'b\'')"/>
diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml
index 1038a738..787ffac6 100644
--- a/ouster-ros/launch/replay.independent.launch.xml
+++ b/ouster-ros/launch/replay.independent.launch.xml
@@ -51,7 +51,6 @@
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>
-    <arg name="_enable_static_tf_publishers" value="true"/>
     <arg name="sensor_frame" value="$(var sensor_frame)"/>
     <arg name="lidar_frame" value="$(var lidar_frame)"/>
     <arg name="imu_frame" value="$(var imu_frame)"/>
diff --git a/ouster-ros/launch/rviz.launch.py b/ouster-ros/launch/rviz.launch.py
index 2d753728..bf8ca655 100644
--- a/ouster-ros/launch/rviz.launch.py
+++ b/ouster-ros/launch/rviz.launch.py
@@ -24,30 +24,6 @@ def generate_launch_description():
     ouster_ns_arg = DeclareLaunchArgument(
         'ouster_ns', default_value='ouster')
 
-    enable_static_tf = LaunchConfiguration('_enable_static_tf_publishers')
-    enable_static_tf_arg = DeclareLaunchArgument(
-        '_enable_static_tf_publishers', default_value='false')
-
-    # NOTE: the two static tf publishers are rather a workaround to let rviz2
-    #     get going and not complain while waiting for the actual sensor frames
-    #     to be published that is when running rviz2 using a parent launch file
-    # TODO: need to be able to propagate the modified frame names from the
-    #       parameters file to RVIZ launch py.
-    sensor_imu_tf = Node(
-        package="tf2_ros",
-        executable="static_transform_publisher",
-        name="stp_sensor_imu",
-        namespace=ouster_ns,
-        condition=IfCondition(enable_static_tf),
-        arguments=["0", "0", "0", "0", "0", "0", "os_sensor", "os_imu"])
-    sensor_ldr_tf = Node(
-        package="tf2_ros",
-        executable="static_transform_publisher",
-        name="stp_sensor_lidar",
-        namespace=ouster_ns,
-        condition=IfCondition(enable_static_tf),
-        arguments=["0", "0", "0", "0", "0", "0", "os_sensor", "os_lidar"])
-
     rviz_node = Node(
         package='rviz2',
         namespace=ouster_ns,
@@ -59,8 +35,5 @@ def generate_launch_description():
     return LaunchDescription([
         ouster_ns_arg,
         rviz_config_arg,
-        enable_static_tf_arg,
-        sensor_imu_tf,
-        sensor_ldr_tf,
         rviz_node
     ])
diff --git a/ouster-ros/launch/rviz.launch.xml b/ouster-ros/launch/rviz.launch.xml
index 82197418..54b490b7 100644
--- a/ouster-ros/launch/rviz.launch.xml
+++ b/ouster-ros/launch/rviz.launch.xml
@@ -4,8 +4,6 @@
     description="Override the default namespace of all ouster nodes"/>
   <arg name="rviz_config" default="$(find-pkg-share ouster_ros)/config/viz.rviz"
     description="optional rviz config file"/>
-  <arg name="_enable_static_tf_publishers" default="false"
-    description="boolean value to enable static tf publishers"/>
 
   <arg name="sensor_frame" default="os_sensor" description="value can not be empty"/>
   <arg name="lidar_frame" default="os_lidar" description="value can not be empty"/>
@@ -15,15 +13,6 @@
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen"
       launch-prefix="bash -c 'sleep 5; $0 $@'" args="-d $(var rviz_config)"/>
-    <!-- NOTE: this is rather a workaround to let rviz2 get going and not complain while waiting
-      for the actual sensor frames to be published that is when when using the same launch file
-      to run rviz2 -->
-    <node if="$(var _enable_static_tf_publishers)"
-      pkg="tf2_ros" exec="static_transform_publisher" name="stp_sensor_imu"
-      args="0 0 0 0 0 0 $(var sensor_frame) $(var imu_frame)"/>
-    <node  if="$(var _enable_static_tf_publishers)"
-      pkg="tf2_ros" exec="static_transform_publisher" name="stp_sensor_lidar"
-      args="0 0 0 0 0 0 $(var sensor_frame) $(var lidar_frame)"/>
   </group>
 
 </launch>
\ No newline at end of file
diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py
index a6e00e7f..0db299ac 100644
--- a/ouster-ros/launch/sensor.composite.launch.py
+++ b/ouster-ros/launch/sensor.composite.launch.py
@@ -77,8 +77,7 @@ def generate_launch_description():
         Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py'
     rviz_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([str(rviz_launch_file_path)]),
-        condition=IfCondition(rviz_enable),
-        launch_arguments={'_enable_static_tf_publishers': 'true'}.items()
+        condition=IfCondition(rviz_enable)
     )
 
     # HACK: to configure and activate the the sensor since state transition
diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml
index 208f7082..717cabd0 100644
--- a/ouster-ros/launch/sensor.composite.launch.xml
+++ b/ouster-ros/launch/sensor.composite.launch.xml
@@ -79,7 +79,6 @@
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>
-    <arg name="_enable_static_tf_publishers" value="true"/>
     <arg name="sensor_frame" value="$(var sensor_frame)"/>
     <arg name="lidar_frame" value="$(var lidar_frame)"/>
     <arg name="imu_frame" value="$(var imu_frame)"/>
diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py
index 632a619a..8ef67d4f 100644
--- a/ouster-ros/launch/sensor.independent.launch.py
+++ b/ouster-ros/launch/sensor.independent.launch.py
@@ -105,8 +105,7 @@ def generate_launch_description():
         Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py'
     rviz_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([str(rviz_launch_file_path)]),
-        condition=IfCondition(rviz_enable),
-        launch_arguments={'_enable_static_tf_publishers': 'true'}.items()
+        condition=IfCondition(rviz_enable)
     )
 
     return launch.LaunchDescription([
diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml
index 62a1f110..1bb6f464 100644
--- a/ouster-ros/launch/sensor.independent.launch.xml
+++ b/ouster-ros/launch/sensor.independent.launch.xml
@@ -77,7 +77,6 @@
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>
-    <arg name="_enable_static_tf_publishers" value="true"/>
     <arg name="sensor_frame" value="$(var sensor_frame)"/>
     <arg name="lidar_frame" value="$(var lidar_frame)"/>
     <arg name="imu_frame" value="$(var imu_frame)"/>
diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml
index 26772709..77af0335 100644
--- a/ouster-ros/launch/sensor_mtp.launch.xml
+++ b/ouster-ros/launch/sensor_mtp.launch.xml
@@ -87,7 +87,6 @@
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
     <arg name="ouster_ns" value="$(var ouster_ns)"/>
     <arg name="rviz_config" value="$(var rviz_config)"/>
-    <arg name="_enable_static_tf_publishers" value="true"/>
     <arg name="sensor_frame" value="$(var sensor_frame)"/>
     <arg name="lidar_frame" value="$(var lidar_frame)"/>
     <arg name="imu_frame" value="$(var imu_frame)"/>
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index 9935bb60..0d52fe14 100644
--- a/ouster-ros/package.xml
+++ b/ouster-ros/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.8.5</version>
+  <version>0.8.6</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp
index 0a1b7d5d..3bdcb241 100644
--- a/ouster-ros/src/os_cloud_node.cpp
+++ b/ouster-ros/src/os_cloud_node.cpp
@@ -14,7 +14,7 @@
 
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <algorithm>
@@ -95,6 +95,7 @@ class OusterCloud : public OusterProcessingNodeBase {
         RCLCPP_INFO(get_logger(),
                     "OusterCloud: retrieved new sensor metadata!");
         info = sensor::parse_metadata(metadata_msg->data);
+        send_static_transforms();
         n_returns = get_n_returns();
         create_lidarscan_objects();
         compute_scan_ts = [this](const auto& ts_v) {
@@ -104,6 +105,13 @@ class OusterCloud : public OusterProcessingNodeBase {
         create_subscriptions();
     }
 
+    void send_static_transforms() {
+        tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg(
+            info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now()));
+        tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg(
+            info.imu_to_sensor_transform, sensor_frame, imu_frame, now()));
+    }
+
     void declare_parameters() {
         declare_parameter("sensor_frame");
         declare_parameter("lidar_frame");
@@ -195,9 +203,6 @@ class OusterCloud : public OusterProcessingNodeBase {
             pc_msg.header.frame_id = sensor_frame;
             lidar_pubs[i]->publish(pc_msg);
         }
-
-        tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg(
-            info.lidar_to_sensor_transform, sensor_frame, lidar_frame, msg_ts));
     }
 
     uint64_t impute_value(int last_scan_last_nonzero_idx,
@@ -316,8 +321,6 @@ class OusterCloud : public OusterProcessingNodeBase {
         auto imu_msg =
             ouster_ros::packet_to_imu_msg(*packet, msg_ts, imu_frame, pf);
         imu_pub->publish(imu_msg);
-        tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg(
-            info.imu_to_sensor_transform, sensor_frame, imu_frame, msg_ts));
     };
 
     static inline rclcpp::Time to_ros_time(uint64_t ts) {
@@ -346,7 +349,7 @@ class OusterCloud : public OusterProcessingNodeBase {
     std::string imu_frame;
     std::string lidar_frame;
 
-    tf2_ros::TransformBroadcaster tf_bcast;
+    tf2_ros::StaticTransformBroadcaster tf_bcast;
 
     bool use_ros_time;
 


Test patch: