SW-4972: merge switching to static transform publisher contribution (#…

…124)

* use static tf broadcaster for ros2 (#112)
* use separate params for tf frames
* send static transforms once
* Disable static transform publishers and update changelog and package version
* Disable rviz static transform publisher
* Remove rviz static transform publisher hack
* Remove left out variables
---------
Co-authored-by: Adam Aposhian <[email protected]>

CHANGELOG.rst

@@ -26,6 +26,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
 -------------

ouster-ros/launch/record.composite.launch.xml

@@ -81,7 +81,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)"/>

ouster-ros/launch/replay.composite.launch.xml

@@ -53,7 +53,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)"/>

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=["--frame-id", "os_sensor", "--child-frame-id", "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=["--frame-id", "os_sensor", "--child-frame-id", "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
     ])

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="--frame-id $(var sensor_frame) --child-frame-id $(var imu_frame)"/>
-    <node  if="$(var _enable_static_tf_publishers)"
-      pkg="tf2_ros" exec="static_transform_publisher" name="stp_sensor_lidar"
-      args="--frame-id $(var sensor_frame) --child-frame-id $(var lidar_frame)"/>
   </group>
 
 </launch>

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

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

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([

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

ouster-ros/launch/sensor_mtp.launch.xml

@@ -85,7 +85,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)"/>

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.3</version>
+  <version>0.8.4</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

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>
@@ -118,6 +118,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) {
@@ -127,6 +128,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 create_lidarscan_objects() {
         // The ouster_ros drive currently only uses single precision when it
         // produces the point cloud. So it isn't of a benefit to compute point
@@ -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;