[NOETIC] Further reduce overhead in the ros driver (#363)

* Publish raw packets * Use ImuPacket & LidarPacket constructs * Add couple switches to control the generation of organzied point clouds, destaggered/staggered + set min/max range and row step * Add min_max ranges * Disable failing build on warnings * Update CHANGELOG * Expose the parameters through launch files and update changelog * Add the RAW option * Update cartesian method description * Change sdk version to mainline release 0.11.1
ouster-lidar · Sep 19, 2024 · 646beba · 646beba
1 parent e59e9a4
commit 646beba
Show file tree

Hide file tree

Showing 28 changed files with 501 additions and 724 deletions.
diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml
@@ -17,7 +17,7 @@ jobs:
         ros_distro:
           - noetic
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           submodules: true
       - name: Build the Docker image

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -23,7 +23,26 @@ Changelog
 * Added a new launch ``persist_config`` option to request the sensor persist the current config
 * Added a new ``loop`` option to the ``replay.launch`` file.
 * Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg
-  documentation and the associated params to enable.
+  documentation and the associated params to enable. Known Issues:
+  - RVIZ can't handle image resize
+  - Can't handle points cloud resize properly (erroneous or corrupt PointCloud)
+  - Doesn't detect and handle invalid configurations
+* Added a new parameter ``organized`` to request publishing unorganized point cloud
+* Added a new parameter ``destagger`` to request publishing staggered point cloud
+* Added two parameters ``min_range``, ``max_range`` to limit the lidar effective range
+* Updated ouster_client to the release of ``20240425`` [v0.11.1]; changes listed below.
+
+ouster_client
+-------------
+* Added a new buffered UDP source implementation BufferedUDPSource.
+* The method version_of_string is marked as deprecated, use version_from_string
+instead.
+* Added a new method firmware_version_from_metadata which works across firmwares.
+* Added support for return order configuration parameter.
+* Added support for gyro and accelerometer FSR configuration parameters.
+* [BUGFIX] mtp_init_client throws a bad optional access.
+* [BUGFIX] properly handle 32-bit frame IDs from the
+* FUSA_RNG15_RFL8_NIR8_DUAL sensor UDP profile.
 
 
 ouster_ros v0.10.0

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -65,6 +65,7 @@ set(BUILD_SHARED_LIBS OFF)
 
 option(BUILD_VIZ "Enabled for Python build" OFF)
 option(BUILD_PCAP "Enabled for Python build" OFF)
+option(BUILD_OSF "Build OSF library." OFF)
 find_package(OusterSDK REQUIRED)
 
 set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS})
@@ -107,7 +108,6 @@ if(CATKIN_ENABLE_TESTING)
   catkin_add_gtest(${PROJECT_NAME}_test
     src/os_ros.cpp
     tests/test_main.cpp
-    tests/ring_buffer_test.cpp
     tests/lock_free_ring_buffer_test.cpp
     tests/point_accessor_test.cpp
     tests/point_transform_test.cpp

diff --git a/Dockerfile b/Dockerfile
@@ -50,7 +50,7 @@ FROM build-env
 
 SHELL ["/bin/bash", "-c"]
 
-ENV CXXFLAGS="-Werror -Wno-deprecated-declarations"
+ENV CXXFLAGS="-Wno-deprecated-declarations"
 RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make     \
 -DCMAKE_BUILD_TYPE=Release                      \
 && /opt/ros/$ROS_DISTRO/env.sh catkin_make install

diff --git a/README.md b/README.md
@@ -1,15 +1,15 @@
 # Official ROS1/ROS2 drivers for Ouster sensors
 
 [ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) |
-[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
+[ROS2 (rolling/humble/iron/jazzy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
 [ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
 
 <p style="float: right;"><img width="20%" src="docs/images/logo.png" /></p>
 
 | ROS Version | Build Status (Linux) |
 |:-----------:|:------:|
 | ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
-| ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
+| ROS2 (rolling/humble/iron/jazzy) | [![rolling/humble/iron/jazzy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
 | ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
 
 - [Official ROS1/ROS2 drivers for Ouster sensors](#official-ros1ros2-drivers-for-ouster-sensors)
@@ -28,7 +28,7 @@
     - [Invoking Services](#invoking-services)
       - [GetMetadata](#getmetadata)
       - [GetConfig](#getconfig)
-      - [SetConfig (experimental)](#setconfig-experimental)
+      - [SetConfig](#setconfig)
   - [License](#license)
 
 
@@ -235,7 +235,7 @@ To get the current config of a live sensor, invoke the command:
 rosservice call /ouster/get_config
 ```
 
-#### SetConfig (experimental)
+#### SetConfig
 To change config via a file while connected to a live sensor, invoke the command:
 ```bash
 rosservice call /ouster/set_config "config_file: '<path to sensor config>'"

diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h
@@ -160,6 +160,14 @@ inline bool check_token(const std::set<std::string>& tokens,
 
 ouster::util::version parse_version(const std::string& fw_rev);
 
+template <typename T>
+uint64_t ulround(T value) {
+    T rounded_value = std::round(value);
+    if (rounded_value < 0) return 0ULL;
+    if (rounded_value > ULLONG_MAX) return ULLONG_MAX;
+    return static_cast<uint64_t>(rounded_value);
+}
+
 } // namespace impl
 
 }  // namespace ouster_ros
diff --git a/launch/common.launch b/launch/common.launch
@@ -41,6 +41,11 @@
     xyzir
     }"/>
 
+  <arg name="organized" doc="generate an organzied point cloud"/>
+  <arg name="destagger" doc="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" doc="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" doc="maximum lidar range to consider (meters)"/>
 
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_cloud_node"
@@ -60,6 +65,10 @@
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/ptp_utc_tai_offset" type="double" value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/point_type" value="$(arg point_type)"/>
+      <param name="~/organized" value="$(arg organized)"/>
+      <param name="~/destagger" value="$(arg destagger)"/>
+      <param name="~/min_range" value="$(arg min_range)"/>
+      <param name="~/max_range" value="$(arg max_range)"/>
     </node>
   </group>
 

diff --git a/launch/driver.launch b/launch/driver.launch
@@ -5,22 +5,25 @@
   <arg name="udp_dest" default=" " doc="hostname or IP where the sensor will send data packets"/>
   <arg name="lidar_port" default="0" doc="port to which the sensor should send lidar data"/>
   <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
-  <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
+  <arg name="udp_profile_lidar" default=" "
+    doc="lidar packet profile; possible values: {
     LEGACY,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
     RNG19_RFL8_SIG16_NIR16_DUAL,
     FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
-  <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
+  <arg name="lidar_mode" default=" "
+    doc="resolution and rate; possible values: {
     512x10,
     512x20,
     1024x10,
     1024x20,
     2048x10,
     4096x5
     }"/>
-  <arg name="timestamp_mode" default=" " doc="method used to timestamp measurements; possible values: {
+  <arg name="timestamp_mode" default=" "
+    doc="method used to timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
     TIME_FROM_PTP_1588,
@@ -49,15 +52,15 @@
   <arg if="$(arg no_bond)" name="_no_bond" value="--no-bond"/>
   <arg unless="$(arg no_bond)" name="_no_bond" value=" "/>
 
-  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" doc="
+  <arg name="proc_mask" default="IMU|PCL|SCAN|IMG|RAW" doc="
     use any combination of the 4 flags to enable or disable specific processors"/>
 
   <arg name="scan_ring" default="0" doc="
     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" doc="point type for the generated point cloud;
-   available options: {
+  <arg name="point_type" default="original"
+    doc="point type for the generated point cloud; available options: {
     original,
     native,
     xyz,
@@ -82,6 +85,16 @@
     doc="maximum number of attempts trying to communicate with the sensor.
          Counter resets upon successful connection"/>
 
+  <arg name="organized" default="true"
+    doc="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    doc="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    doc="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="10000.0"
+    doc="maximum lidar range to consider (meters)"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -117,6 +130,10 @@
              value="$(arg dormant_period_between_reconnects)"/>
       <param name="~/max_failed_reconnect_attempts"
              value="$(arg max_failed_reconnect_attempts)"/>
+      <param name="~/organized" value="$(arg organized)"/>
+      <param name="~/destagger" value="$(arg destagger)"/>
+      <param name="~/min_range" value="$(arg min_range)"/>
+      <param name="~/max_range" value="$(arg max_range)"/>
     </node>
   </group>
 

diff --git a/launch/replay.launch b/launch/replay.launch
@@ -12,8 +12,8 @@
   <arg name="ouster_ns" default="ouster" doc="Override the default namespace of all ouster nodes"/>
   <arg name="metadata" default="" doc="path to read metadata file when replaying sensor data"/>
   <arg name="bag_file" doc="file name to use for the recorded bag file"/>
-  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC" doc="A parameter that allows you to override the timestamp measurements;
-    possible values: {
+  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC"
+    doc="A parameter that allows you to override the timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
     TIME_FROM_PTP_1588,
@@ -50,15 +50,25 @@
     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" doc="point type for the generated point cloud;
-   available options: {
+  <arg name="point_type" default="original"
+    doc="point type for the generated point cloud; available options: {
     original,
     native,
     xyz,
     xyzi,
     xyzir
     }"/>
 
+  <arg name="organized" default="true"
+    doc="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    doc="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    doc="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="10000.0"
+    doc="maximum lidar range to consider (meters)"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -93,6 +103,10 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="organized" value="$(arg organized)"/>
+    <arg name="destagger" value="$(arg destagger)"/>
+    <arg name="min_range" value="$(arg min_range)"/>
+    <arg name="max_range" value="$(arg max_range)"/>
   </include>
 
   <arg name="_use_bag_file_name" value="$(eval not (bag_file == ''))"/>

diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch
@@ -6,8 +6,8 @@
   <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"/>
-  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC" doc="A parameter that allows you to override the timestamp measurements;
-    possible values: {
+  <arg name="timestamp_mode" default="TIME_FROM_INTERNAL_OSC"
+    doc="A parameter that allows you to override the timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
     TIME_FROM_PTP_1588,
@@ -44,15 +44,25 @@
     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" doc="point type for the generated point cloud;
-   available options: {
+  <arg name="point_type" default="original"
+    doc="point type for the generated point cloud; available options: {
     original,
     native,
     xyz,
     xyzi,
     xyzir
     }"/>
 
+  <arg name="organized" default="true"
+    doc="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    doc="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    doc="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="10000.0"
+    doc="maximum lidar range to consider (meters)"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -86,6 +96,10 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="organized" value="$(arg organized)"/>
+    <arg name="destagger" value="$(arg destagger)"/>
+    <arg name="min_range" value="$(arg min_range)"/>
+    <arg name="max_range" value="$(arg max_range)"/>
   </include>
 
 

diff --git a/launch/sensor.launch b/launch/sensor.launch
@@ -5,22 +5,25 @@
   <arg name="udp_dest" default=" " doc="hostname or IP where the sensor will send data packets"/>
   <arg name="lidar_port" default="0" doc="port to which the sensor should send lidar data"/>
   <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
-  <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
+  <arg name="udp_profile_lidar" default=" "
+    doc="lidar packet profile; possible values: {
     LEGACY,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
     RNG19_RFL8_SIG16_NIR16_DUAL,
     FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
-  <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
+  <arg name="lidar_mode" default=" "
+    doc="resolution and rate; possible values: {
     512x10,
     512x20,
     1024x10,
     1024x20,
     2048x10,
     4096x5
     }"/>
-  <arg name="timestamp_mode" default=" " doc="method used to timestamp measurements; possible values: {
+  <arg name="timestamp_mode" default=" "
+    doc="method used to timestamp measurements; possible values: {
     TIME_FROM_INTERNAL_OSC,
     TIME_FROM_SYNC_PULSE_IN,
     TIME_FROM_PTP_1588,
@@ -64,8 +67,8 @@
     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" doc="point type for the generated point cloud;
-   available options: {
+  <arg name="point_type" default="original"
+    doc="point type for the generated point cloud; available options: {
     original,
     native,
     xyz,
@@ -90,6 +93,16 @@
     doc="maximum number of attempts trying to communicate with the sensor.
          Counter resets upon successful connection"/>
 
+  <arg name="organized" default="true"
+    doc="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    doc="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    doc="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="10000.0"
+    doc="maximum lidar range to consider (meters)"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -137,6 +150,10 @@
     <arg name="proc_mask" value="$(arg proc_mask)"/>
     <arg name="scan_ring" value="$(arg scan_ring)"/>
     <arg name="point_type" value="$(arg point_type)"/>
+    <arg name="organized" value="$(arg organized)"/>
+    <arg name="destagger" value="$(arg destagger)"/>
+    <arg name="min_range" value="$(arg min_range)"/>
+    <arg name="max_range" value="$(arg max_range)"/>
   </include>
 
 </launch>