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

CHANGELOG.rst

@@ -6,6 +6,7 @@ Changelog
 [unreleased]
 ============
 * [BUGFIX]: correctly align timestamps to the generated point cloud
+* Added support to enable **loop** for pcap replay + other replay config
 
 
 ouster_ros v0.13.0

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"/>
 
@@ -111,9 +115,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"/>
@@ -71,10 +76,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>
 

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;