ROS-389 [rolling/humble/iron/jazzy]: replay-improvments-and-fixes

CHANGELOG.rst

@@ -5,6 +5,10 @@ Changelog
 [unreleased]
 ============
 * [BUGFIX]: correctly align timestamps to the generated point cloud.
+* Added support to enable **loop** for pcap replay + other replay config.
+* Added a new launch file parameter ``pub_static_tf`` that allows users to turn off the braodcast
+  of sensor TF transforms.
+
 
 ouster_ros v0.13.2
 ==================

ouster-ros/config/driver_params.yaml

@@ -56,7 +56,11 @@ ouster/os_driver:
     # point_cloud_frame[optional]: which frame of reference to use when
     # generating PointCloud2 or LaserScan messages, select between the values of
     # lidar_frame and sensor_frame.
-    point_cloud_frame: os_lidar 
+    point_cloud_frame: os_lidar
+    # pub_static_tf[optional]: when this flag is set to True, the driver will
+    # broadcast the TF transforms for the imu/sensor/lidar frames. Prevent the
+    # driver from broadcasting TF transforms by setting this parameter to False.
+    pub_static_tf: true
     # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and
     # SCAN to enable or disable their respective messages.
     proc_mask: IMU|PCL|SCAN|IMG|RAW

ouster-ros/config/os_sensor_cloud_image_params.yaml

@@ -27,6 +27,7 @@ ouster/os_cloud:
     lidar_frame: os_lidar
     imu_frame: os_imu
     point_cloud_frame: os_lidar
+    pub_static_tf: true
     timestamp_mode: ''  # this value needs to match os_sensor/timestamp_mode
     ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588
     proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options

ouster-ros/include/ouster_ros/os_sensor_node_base.h

@@ -7,21 +7,23 @@
  *
  */
 
-#include <ouster/types.h>
+#include <chrono>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <lifecycle_msgs/srv/change_state.hpp>
 #include <std_msgs/msg/string.hpp>
 
 #include "ouster_sensor_msgs/srv/get_metadata.hpp"
 
+#include <ouster/types.h>
+
 namespace ouster_ros {
 
 class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
    protected:
     explicit OusterSensorNodeBase(const std::string& name,
-                                  const rclcpp::NodeOptions& options)
-        : rclcpp_lifecycle::LifecycleNode(name, options) {}
+                                  const rclcpp::NodeOptions& options);
 
    protected:
     bool is_arg_set(const std::string& arg) const {
@@ -41,7 +43,20 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
     static bool write_text_to_file(const std::string& file_path,
                                    const std::string& text);
 
+    static std::string transition_id_to_string(uint8_t transition_id);
+
+    template <typename CallbackT, typename... CallbackT_Args>
+    bool change_state(std::uint8_t transition_id, CallbackT callback,
+                      CallbackT_Args... callback_args,
+                      std::chrono::seconds time_out = std::chrono::seconds{3});
+
+    void execute_transitions_sequence(std::vector<uint8_t> transitions_sequence,
+                                      size_t at);
+
+
    protected:
+    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> change_state_client;
+
     ouster::sensor::sensor_info info;
     rclcpp::Service<ouster_sensor_msgs::srv::GetMetadata>::SharedPtr get_metadata_srv;
     std::string cached_metadata;

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

@@ -55,6 +55,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="true"
     description="Use the default system QoS settings"/>
@@ -125,6 +129,7 @@
         <param name="lidar_frame" value="$(var lidar_frame)"/>
         <param name="imu_frame" value="$(var imu_frame)"/>
         <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="pub_static_tf" value="$(var pub_static_tf)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
         <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

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

@@ -3,6 +3,9 @@
   <set_parameter name="use_sim_time" value="true" />
 
   <arg name="loop" default="false" description="request loop playback"/>
+  <arg name="play_delay" default="0" description="playback start delay in seconds"/>
+  <arg name="play_rate" default="1.0"/>
+
   <let if="$(var loop)" name="_loop" value="--loop"/>
   <let unless="$(var loop)" name="_loop" value=" "/>
 
@@ -37,6 +40,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
 
@@ -77,12 +84,14 @@
     </node>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterCloud" name="os_cloud">
+        <remap from="/os_node/metadata" to="/ouster/metadata"/>
         <remap from="/os_node/imu_packets" to="/ouster/imu_packets"/>
         <remap from="/os_node/lidar_packets" to="/ouster/lidar_packets"/>
         <param name="sensor_frame" value="$(var sensor_frame)"/>
         <param name="lidar_frame" value="$(var lidar_frame)"/>
         <param name="imu_frame" value="$(var imu_frame)"/>
         <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="pub_static_tf" value="$(var pub_static_tf)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
         <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
@@ -95,6 +104,7 @@
         <param name="max_range" value="$(var max_range)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
+        <remap from="/os_node/metadata" to="/ouster/metadata"/>
         <remap from="/os_node/lidar_packets" to="/ouster/lidar_packets"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
         <param name="proc_mask" value="$(var proc_mask)"/>
@@ -119,9 +129,9 @@
   <let name="_use_bag_file_name" value="$(eval '\'$(var bag_file)\' != \'b\'')"/>
 
   <executable if="$(var _use_bag_file_name)" output="screen"
-    launch-prefix="bash -c 'sleep 3; $0 $@'"
+    launch-prefix="bash -c 'sleep $(var play_delay); $0 $@'"
     cmd="ros2 bag play $(var bag_file) --clock $(var _loop)
-      --qos-profile-overrides-path
+      --rate $(var play_rate) --qos-profile-overrides-path
       $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/>
 
 </launch>

ouster-ros/launch/replay_pcap.launch.xml

@@ -1,8 +1,13 @@
 <launch>
 
-  <!-- NOTE: pcap replay node does not implement clock-->
+  <!-- NOTE: pcap replay node does not implement clock -->
   <set_parameter name="use_sim_time" value="false" />
 
+  <arg name="loop" default="false" description="request loop playback"/>
+  <arg name="play_delay" default="0" description="playback start delay in seconds"/>
+  <arg name="progress_update_freq" default="1.0"
+    description="playback preogress update frequency per second"/>
+
   <arg name="ouster_ns" default="ouster"
     description="Override the default namespace of all ouster nodes"/>
   <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC"
@@ -31,6 +36,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
 
@@ -65,9 +74,12 @@
 
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
-    <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_pcap" name="os_pcap" output="screen">
+    <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_pcap" name="os_pcap"
+        launch-prefix="bash -c 'sleep $(var play_delay); $0 $@'" output="screen">
       <param name="metadata" value="$(var metadata)"/>
       <param name="pcap_file" value="$(var pcap_file)"/>
+      <param name="loop" value="$(var loop)"/>
+      <param name="progress_update_freq" value="$(var progress_update_freq)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
     </node>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
@@ -76,6 +88,7 @@
         <param name="lidar_frame" value="$(var lidar_frame)"/>
         <param name="imu_frame" value="$(var imu_frame)"/>
         <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="pub_static_tf" value="$(var pub_static_tf)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
         <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
@@ -94,15 +107,6 @@
     </node_container>
   </group>
 
-  <!-- HACK: configure and activate the replay node via a process execute since state
-    transition is currently not availabe through launch.xml format -->
-  <executable if="$(var _use_metadata_file)"
-    cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_pcap configure"
-    launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
-  <executable if="$(var _use_metadata_file)"
-    cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_pcap activate"
-    launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
-
   <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)"/>

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

@@ -53,6 +53,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="false"
     description="Use the default system QoS settings"/>
@@ -119,6 +123,7 @@
       <param name="lidar_frame" value="$(var lidar_frame)"/>
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+      <param name="pub_static_tf" value="$(var pub_static_tf)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

ouster-ros/launch/sensor.independent.launch.xml

@@ -53,6 +53,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="false"
     description="Use the default system QoS settings"/>
@@ -122,6 +126,7 @@
       <param name="lidar_frame" value="$(var lidar_frame)"/>
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+      <param name="pub_static_tf" value="$(var pub_static_tf)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

ouster-ros/launch/sensor_mtp.launch.xml

@@ -61,6 +61,10 @@
     description="which frame to be used when publishing PointCloud2 or LaserScan messages.
     Choose between the value of sensor_frame or lidar_frame, leaving this value empty
     would set lidar_frame to be the frame used when publishing these messages."/>
+  <arg name="pub_static_tf" default="true"
+    description="when this flag is set to True, the driver will broadcast the TF transforms
+    for the imu/sensor/lidar frames. Prevent the driver from broadcasting TF transforms by
+    setting this parameter to False."/>
 
   <arg name="use_system_default_qos" default="false"
     description="Use the default system QoS settings"/>
@@ -117,6 +121,7 @@
       <param name="lidar_frame" value="$(var lidar_frame)"/>
       <param name="imu_frame" value="$(var imu_frame)"/>
       <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+      <param name="pub_static_tf" value="$(var pub_static_tf)"/>
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

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.13.3</version>
+  <version>0.13.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

@@ -72,7 +72,9 @@ class OusterCloud : public OusterProcessingNodeBase {
         RCLCPP_INFO(get_logger(),
                     "OusterCloud: retrieved new sensor metadata!");
         info = sensor::parse_metadata(metadata_msg->data);
-        tf_bcast.broadcast_transforms(info);
+        if (tf_bcast.publish_static_tf()) {
+            tf_bcast.broadcast_transforms(info);
+        }
         create_publishers_subscriptions(info);
     }
 

ouster-ros/src/os_driver_node.cpp

@@ -53,7 +53,9 @@ class OusterDriver : public OusterSensor {
 
     virtual void on_metadata_updated(const sensor::sensor_info& info) override {
         OusterSensor::on_metadata_updated(info);
-        tf_bcast.broadcast_transforms(info);
+        if (tf_bcast.publish_static_tf()) {
+          tf_bcast.broadcast_transforms(info);
+        }
     }
 
     virtual void create_publishers() override {