ROS2[FOXY] add pcap reader

CHANGELOG.rst

@@ -11,6 +11,8 @@ Changelog
 * [BUGFIX]: Implement lock free ring buffer with throttling to reduce partial frames
 * add support for FUSA udp profile ``FUSA_RNG15_RFL8_NIR8_DUAL``.
 * [BREAKING]: Set xyz values of individual points in the PointCloud to NaNs when range is zero.
+* Added support to replay pcap format direclty from ouster-ros. The feature needs to be enabled
+  explicitly by turning on the ``BUILD_PCAP`` cmake option and having ``libpcap-dev`` installed.
 
 ouster_ros v0.12.0
 ==================

README.md

@@ -25,6 +25,7 @@
       - [Sensor Mode](#sensor-mode)
       - [Recording Mode](#recording-mode)
       - [Replay Mode](#replay-mode)
+        - [PCAP Replay Mode](#pcap-replay-mode)
       - [Multicast Mode (experimental)](#multicast-mode-experimental)
     - [Invoking Services](#invoking-services)
       - [GetMetadata](#getmetadata)
@@ -100,7 +101,8 @@ sudo apt install -y         \
 > You may choose a different _ssl_ backend for the _curl_ library such as `libcurl4-gnutls-dev` or
 > `libcurl4-nss-dev`
 
-
+> **Note**  
+> To use the PCAP replay mode you need to have `libpcap-dev` installed
 
 ### Windows
 TBD
@@ -190,6 +192,15 @@ ros2 launch ouster_ros replay.launch.xml    \
     metadata:=<json file name>              # optional if bag file has /metadata topic
 ```
 
+##### PCAP Replay Mode
+> Note
+> To use this feature you need to compile the driver with `BUILD_PCAP` option enabled
+```bash
+ros2 launch ouster_ros replay_pcap.launch.xml   \
+    pcap_file:=<path to ouster pcap file>       \
+    metadata:=<json file name>              # required
+```
+
 #### Multicast Mode (experimental)
 The multicast launch mode supports configuring the sensor to broadcast lidar packets from the same
 sensor (live) to multiple active clients. You initiate this mode by using `sensor_mtp.launch.xml`

ouster-ros/CMakeLists.txt

@@ -31,9 +31,11 @@ endif()
 option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON)
 
 set(_ouster_ros_INCLUDE_DIRS
-  include
-  ouster-sdk/ouster_client/include
-  ouster-sdk/ouster_client/include/optional-lite
+  "include;ouster-sdk/ouster_client/include;ouster-sdk/ouster_client/include/optional-lite"
+  "include;"
+  "ouster-sdk/ouster_client/include;"
+  "ouster-sdk/ouster_client/include/optional-lite;"
+  "ouster-sdk/ouster_pcap/include"
 )
 
 # ==== Libraries ====
@@ -55,6 +57,11 @@ include_directories(${_ouster_ros_INCLUDE_DIRS})
 # use only MPL-licensed parts of eigen
 add_definitions(-DEIGEN_MPL2_ONLY)
 
+set(OUSTER_TARGET_LINKS ouster_client)
+if (BUILD_PCAP)
+  list(APPEND OUSTER_TARGET_LINKS ouster_pcap)
+endif()
+
 add_library(ouster_ros_library SHARED
   src/os_ros.cpp
 )
@@ -78,7 +85,7 @@ target_link_libraries(ouster_ros_library
     ouster_build
     pcl_common
   # PRIVATE (unsupported)
-  -Wl,--whole-archive ouster_client -Wl,--no-whole-archive
+  -Wl,--whole-archive ${OUSTER_TARGET_LINKS} -Wl,--no-whole-archive
 )
 
 # helper method to construct ouster-ros components
@@ -165,6 +172,18 @@ rclcpp_components_register_node(os_driver_component
 )
 
 
+if (BUILD_PCAP)
+# ==== os_replay_component ====
+create_ros2_component(os_pcap_component
+  "src/os_sensor_node_base.cpp;src/os_pcap_node.cpp"
+  ""
+)
+rclcpp_components_register_node(os_pcap_component
+  PLUGIN "ouster_ros::OusterPcap"
+  EXECUTABLE os_pcap
+)
+endif()
+
 # ==== Test ====
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -191,14 +210,21 @@ endif()
 
 
 # ==== Install ====
+set(OUSTER_INSTALL_TARGETS
+  ouster_ros_library
+  os_sensor_component
+  os_replay_component
+  os_cloud_component
+  os_image_component
+  os_driver_component
+)
+if (BUILD_PCAP)
+  list(APPEND OUSTER_INSTALL_TARGETS os_pcap_component)
+endif()
+
 install(
   TARGETS
-    ouster_ros_library
-    os_sensor_component
-    os_replay_component
-    os_cloud_component
-    os_image_component
-    os_driver_component
+    ${OUSTER_INSTALL_TARGETS}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin

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

@@ -1,5 +1,7 @@
 <launch>
 
+  <set_parameter name="use_sim_time" value="true" />
+
   <arg name="ouster_ns" default="ouster"
     description="Override the default namespace of all ouster nodes"/>
   <!-- TODO: revisit the proper behaviour of allowing users override the default timestamp_mode during replay -->
@@ -46,6 +48,15 @@
     use this parameter in conjunction with the SCAN flag
     and choose a value the range [0, sensor_beams_count)"/>
 
+  <arg name="point_type" default="original" description="point type for the generated point cloud;
+   available options: {
+    original,
+    native,
+    xyz,
+    xyzi,
+    xyzir
+    }"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_replay" name="os_replay" output="screen">
@@ -61,6 +72,7 @@
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>
+      <param name="point_type" value="$(var point_type)"/>
     </node>
     <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

ouster-ros/launch/replay_pcap.launch.xml

@@ -0,0 +1,96 @@
+<launch>
+
+  <!-- NOTE: pcap replay node does not implement clock-->
+  <set_parameter name="use_sim_time" value="false" />
+
+  <arg name="ouster_ns" default="ouster"
+    description="Override the default namespace of all ouster nodes"/>
+  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC"
+    description="method used to timestamp measurements; possible values: {
+    TIME_FROM_INTERNAL_OSC,
+    TIME_FROM_SYNC_PULSE_IN,
+    TIME_FROM_PTP_1588,
+    TIME_FROM_ROS_TIME
+    }"/>
+   <arg name="ptp_utc_tai_offset" default="-37.0"
+    description="UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588"/>
+  <arg name="metadata" description="path to write metadata file when receiving sensor data"/>
+  <arg name="pcap_file" description="file name to use for the recorded bag file"/>
+  <arg name="viz" default="true"
+    description="whether to run a rviz"/>
+  <arg name="rviz_config" default="$(find-pkg-share ouster_ros)/config/viz-reliable.rviz"
+    description="optional rviz config file"/>
+
+  <arg name="sensor_frame" default="os_sensor"
+    description="sets name of choice for the sensor_frame tf frame, value can not be empty"/>
+  <arg name="lidar_frame" default="os_lidar"
+    description="sets name of choice for the os_lidar tf frame, value can not be empty"/>
+  <arg name="imu_frame" default="os_imu"
+      description="sets name of choice for the os_imu tf frame, value can not be empty"/>
+  <arg name="point_cloud_frame" default=""
+    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."/>
+
+  <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
+
+  <arg name="use_system_default_qos" default="true"
+    description="Use the default system QoS settings"/>
+
+  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" description="
+    The IMG flag here is not supported and does not affect anything,
+    to disable image topics you would need to omit the os_image node
+    from the launch file"/>
+
+  <arg name="scan_ring" default="0" description="
+    use this parameter in conjunction with the SCAN flag
+    and choose a value the range [0, sensor_beams_count)"/>
+
+  <arg name="point_type" default="xyz" description="point type for the generated point cloud;
+   available options: {
+    original,
+    native,
+    xyz,
+    xyzi,
+    xyzir
+    }"/>
+
+  <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">
+      <param name="metadata" value="$(var metadata)"/>
+      <param name="pcap_file" value="$(var pcap_file)"/>
+      <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+    </node>
+    <node pkg="ouster_ros" exec="os_cloud" name="os_cloud" output="screen">
+      <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="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)"/>
+      <param name="proc_mask" value="$(var proc_mask)"/>
+      <param name="scan_ring" value="$(var scan_ring)"/>
+      <param name="point_type" value="$(var point_type)"/>
+    </node>
+    <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
+      <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+    </node>
+  </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)"/>
+  </include>
+
+</launch>

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