ROS-389 [noetic]: replay-improvments-and-fixes

CHANGELOG.rst

@@ -6,6 +6,9 @@ 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 broadcast
+  of sensor TF transforms.
 
 
 ouster_ros v0.13.0

launch/common.launch

@@ -12,6 +12,10 @@
   <arg name="imu_frame" doc="
     sets name of choice for the os_imu tf frame, value can not be empty"/>
   <arg name="point_cloud_frame" doc="
+    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="pub_static_tf" doc="
     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."/>
@@ -56,6 +60,7 @@
       <param name="~/lidar_frame" value="$(arg lidar_frame)"/>
       <param name="~/imu_frame" value="$(arg imu_frame)"/>
       <param name="~/point_cloud_frame" value="$(arg point_cloud_frame)"/>
+      <param name="~/pub_static_tf" value="$(arg pub_static_tf)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
       <param name="~/dynamic_transforms_broadcast" type="bool"
         value="$(arg dynamic_transforms_broadcast)"/>

launch/driver.launch

@@ -46,6 +46,10 @@
     doc="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" doc="
+    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="no_bond" default="false"
     doc="request no bond setup when nodelets are created"/>
@@ -119,6 +123,7 @@
       <param name="~/lidar_frame" value="$(arg lidar_frame)"/>
       <param name="~/imu_frame" value="$(arg imu_frame)"/>
       <param name="~/point_cloud_frame" value="$(arg point_cloud_frame)"/>
+      <param name="~/pub_static_tf" value="$(arg pub_static_tf)"/>
       <param name="~/proc_mask" value="$(arg proc_mask)"/>
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/point_type" value="$(arg point_type)"/>

launch/record.launch

@@ -44,6 +44,10 @@
     doc="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" doc="
+    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="dynamic_transforms_broadcast" default="false"
     doc="static or dynamic transforms broadcast"/>
@@ -123,6 +127,7 @@
     <arg name="lidar_frame" value="$(arg lidar_frame)"/>
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
+    <aeg name="pub_static_tf" value="$(arg pub_static_tf)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
     <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="dynamic_transforms_broadcast"

launch/replay.launch

@@ -3,9 +3,13 @@
   <param name="use_sim_time" value="true"/>
 
   <arg name="loop" default="false" doc="request loop playback"/>
+  <arg name="play_delay" default="0" doc="playback start delay in seconds"/>
+  <arg name="play_rate" default="1.0"/>
+
   <arg if="$(arg loop)" name="_loop" value="--loop"/>
   <arg unless="$(arg loop)" name="_loop" value=" "/>
 
+  <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"/>
 
@@ -35,6 +39,10 @@
     doc="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" doc="
+    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="dynamic_transforms_broadcast" default="false"
     doc="static or dynamic transforms broadcast"/>
@@ -94,6 +102,7 @@
     <arg name="lidar_frame" value="$(arg lidar_frame)"/>
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
+    <arg name="pub_static_tf" value="$(arg pub_static_tf)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
     <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="dynamic_transforms_broadcast"
@@ -111,9 +120,10 @@
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>
 
-  <node if="$(arg _use_bag_file_name)" pkg="rosbag" type="play" name="rosbag_play_recording"
-    launch-prefix="bash -c 'sleep 3; $0 $@' "
+  <node if="$(arg _use_bag_file_name)" pkg="rosbag" type="play"
+    name="rosbag_play_recording"
+    launch-prefix="bash -c 'sleep $(arg play_delay); $0 $@' "
     output="screen" required="true"
-    args="--clock $(arg bag_file) $(arg _loop)"/>
+    args="--clock $(arg bag_file) $(arg _loop) --rate $(arg play_rate)"/>
 
 </launch>

launch/replay_pcap.launch

@@ -1,8 +1,13 @@
 <launch>
 
-  <!-- NOTE: pcap replay node does not implement clock-->
+  <!-- NOTE: pcap replay node does not implement clock -->
   <param name="use_sim_time" value="false"/>
 
+  <arg name="loop" default="false" doc="request loop playback"/>
+  <arg name="play_delay" default="0" doc="playback start delay in seconds"/>
+  <arg name="progress_update_freq" default="3.0"
+    doc="playback progress update frequency per second"/>
+
   <arg name="ouster_ns" default="ouster" doc="Override the default namespace of all ouster nodes"/>
   <arg name="metadata" doc="path to read metadata file when replaying sensor data"/>
   <arg name="pcap_file" doc="file name to use for the recorded pcap file"/>
@@ -29,6 +34,10 @@
     doc="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" doc="
+    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="dynamic_transforms_broadcast" default="false"
     doc="static or dynamic transforms broadcast"/>
@@ -71,10 +80,12 @@
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet"
       name="os_node" output="screen" required="true"
-      launch-prefix="bash -c 'sleep 3; $0 $@' "
+      launch-prefix="bash -c 'sleep $(arg play_delay); $0 $@' "
       args="load ouster_ros/OusterPcap os_nodelet_mgr">
       <param name="~/metadata" value="$(arg metadata)"/>
       <param name="~/pcap_file" value="$(arg pcap_file)"/>
+      <param name="~/loop" value="$(arg loop)"/>
+      <param name="~/progress_update_freq" value="$(arg progress_update_freq)"/>
     </node>
   </group>
 
@@ -87,6 +98,7 @@
     <arg name="lidar_frame" value="$(arg lidar_frame)"/>
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
+    <arg name="pub_static_tf" value="$(arg pub_static_tf)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
     <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="dynamic_transforms_broadcast"

launch/sensor.launch

@@ -47,6 +47,10 @@
     doc="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" doc="
+    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="no_bond" default="false"
     doc="request no bond setup when nodelets are created"/>
@@ -140,6 +144,7 @@
     <arg name="lidar_frame" value="$(arg lidar_frame)"/>
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
+    <arg name="pub_static_tf" value="$(arg pub_static_tf)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
     <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="_no_bond" value="$(arg _no_bond)"/>

launch/sensor_mtp.launch

@@ -51,6 +51,10 @@
     doc="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" doc="
+    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="no_bond" default="false"
     doc="request no bond setup when nodelets are created"/>
@@ -146,6 +150,7 @@
     <arg name="lidar_frame" value="$(arg lidar_frame)"/>
     <arg name="imu_frame" value="$(arg imu_frame)"/>
     <arg name="point_cloud_frame" value="$(arg point_cloud_frame)"/>
+    <arg name="pub_static_tf" value="$(arg pub_static_tf)"/>
     <arg name="timestamp_mode" value="$(arg timestamp_mode)"/>
     <arg name="ptp_utc_tai_offset" value="$(arg ptp_utc_tai_offset)"/>
     <arg name="_no_bond" value="$(arg _no_bond)"/>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.13.1</version>
+  <version>0.13.2</version>
   <description>Ouster ROS driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

src/os_cloud_nodelet.cpp

@@ -76,20 +76,22 @@ class OusterCloud : public nodelet::Nodelet {
             dynamic_transforms_rate = 1.0;
         }
 
-        if (!dynamic_transforms) {
-            NODELET_INFO("OusterCloud: using static transforms broadcast");
-            tf_bcast.broadcast_transforms(info);
-        } else {
-            NODELET_INFO_STREAM(
-                "OusterCloud: dynamic transforms broadcast enabled with "
-                "broadcast rate of: "
-                << dynamic_transforms_rate << " Hz");
-            timer_.stop();
-            timer_ = getNodeHandle().createTimer(
-                ros::Duration(1.0 / dynamic_transforms_rate),
-                [this, info](const ros::TimerEvent&) {
-                    tf_bcast.broadcast_transforms(info, last_msg_ts);
-                });
+        if (tf_bcast.publish_static_tf()) {
+            if (!dynamic_transforms) {
+                NODELET_INFO("OusterCloud: using static transforms broadcast");
+                tf_bcast.broadcast_transforms(info);
+            } else {
+                NODELET_INFO_STREAM(
+                    "OusterCloud: dynamic transforms broadcast enabled with "
+                    "broadcast rate of: "
+                    << dynamic_transforms_rate << " Hz");
+                timer_.stop();
+                timer_ = getNodeHandle().createTimer(
+                    ros::Duration(1.0 / dynamic_transforms_rate),
+                    [this, info](const ros::TimerEvent&) {
+                        tf_bcast.broadcast_transforms(info, last_msg_ts);
+                    });
+            }
         }
 
         create_handlers(info);

src/os_driver_nodelet.cpp

@@ -62,7 +62,9 @@ class OusterDriver : public OusterSensor {
         // for OusterDriver we are going to always assume static broadcast
         // at least for now
         tf_bcast.parse_parameters(getPrivateNodeHandle());
-        tf_bcast.broadcast_transforms(info);
+        if (tf_bcast.publish_static_tf()) {
+            tf_bcast.broadcast_transforms(info);
+        }
         create_handlers();
     }
 

src/os_pcap_nodelet.cpp

@@ -35,6 +35,10 @@ class OusterPcap : public OusterSensorNodeletBase {
     virtual void onInit() override {
         auto meta_file = get_meta_file();
         auto pcap_file = get_pcap_file();
+        loop = getPrivateNodeHandle().param("loop", false);
+        progress_update_freq = getPrivateNodeHandle().param("progress_update_freq", 1.0);
+        if (progress_update_freq < 0.001)
+            progress_update_freq = 0.001;
         create_metadata_pub();
         load_metadata_from_file(meta_file);
         allocate_buffers();
@@ -104,10 +108,12 @@ class OusterPcap : public OusterSensorNodeletBase {
         packet_read_active = true;
         packet_read_thread = std::make_unique<std::thread>([this]() {
             auto& pf = sensor::get_format(info);
-            while (packet_read_active) {
+            do {
                 read_packets(*pcap, pf);
-            }
+                pcap->reset();
+            } while (ros::ok() && packet_read_active && loop);
             NODELET_DEBUG("packet_read_thread done.");
+            ros::shutdown();
         });
     }
 
@@ -125,9 +131,9 @@ class OusterPcap : public OusterSensorNodeletBase {
         auto file_start = packet_info.timestamp;
         auto last_update = file_start;
         using namespace std::chrono_literals;
-        const auto UPDATE_PERIOD = duration_cast<microseconds>(1s / 3);
+        const auto UPDATE_PERIOD = duration_cast<microseconds>(1s / progress_update_freq);
 
-        while (ros::ok() && payload_size) {
+        while (ros::ok() && packet_read_active && payload_size) {
             auto start = high_resolution_clock::now();
             if (packet_info.dst_port == info.config.udp_port_imu) {
                 std::memcpy(imu_packet.buf.data(), pcap.current_data(),
@@ -167,6 +173,8 @@ class OusterPcap : public OusterSensorNodeletBase {
     PacketMsg imu_packet;
     ros::Publisher lidar_packet_pub;
     ros::Publisher imu_packet_pub;
+    bool loop;
+    double progress_update_freq;
 
     std::atomic<bool> packet_read_active = {false};
     std::unique_ptr<std::thread> packet_read_thread;

src/os_transforms_broadcaster.h

@@ -32,6 +32,7 @@ class OusterTransformsBroadcaster {
         lidar_frame = pnh.param("lidar_frame", std::string{"os_lidar"});
         imu_frame = pnh.param("imu_frame", std::string{"os_imu"});
         point_cloud_frame = pnh.param("point_cloud_frame", std::string{});
+        pub_static_tf = pnh.param("pub_static_tf", true);
 
         if (!is_arg_set(sensor_frame) || !is_arg_set(lidar_frame) ||
             !is_arg_set(imu_frame)) {
@@ -100,6 +101,8 @@ class OusterTransformsBroadcaster {
         return point_cloud_frame == sensor_frame;
     }
 
+    bool publish_static_tf() const { return pub_static_tf; }
+
    private:
     std::string node_name;
     tf2_ros::StaticTransformBroadcaster static_tf_bcast;
@@ -108,6 +111,7 @@ class OusterTransformsBroadcaster {
     std::string lidar_frame;
     std::string sensor_frame;
     std::string point_cloud_frame;
+    bool pub_static_tf;
 };
 
 }  // namespace ouster_ros