SW-5148: back port improvements in ros 2 to ros 1 (#170)

* Add thread_safe_ring_buffer + Switch to using threads for packet processing in os_sensor node +
Add new wreset service + Add metadata service to os_replay
* implemented the refactored os_cloud_nodelet + adding relevant processors
* Backport os_image_nodelet + add image_processor + Add a switch between dynamic and static broadcast
* Refactoring and fixes for the os_cloud_nodelet + Only create publisher when processors are enabled
* Add a new os_driver nodelet + Add a new launch file to utilize os_driver +
Made it is possible to set custom tf frames + Made it possible to set proc_mask and scan_ring
* Add ring_buffer_test + enable building and running within docker
* Adjust the tests run command
* Source the workspace before running tests
* Enable bash as the shell
* Rename nodelets_os namesapce to ouster_ros + Rename nodelets_os.xml to ouster_ros_nodelet.xml +
Wrap all classes with ouster_ros + always default no_bond + Fix Dockerfile build
* Update CHANGELOG.txt
* Update rviz color scheme and changelog
* Update README.md to direct people to use driver.launch instead of sensor.launch
* Add a section describing how to obtain detailed description of launch args

CHANGELOG.rst

@@ -18,15 +18,46 @@ ouster_ros(1)
 * fixed a potential issue of time values within generated point clouds that could result in a value
   overflow
 * added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodelets and
-  save it to the bag file on record
+  save it to the bag file on record.
 * make specifying metadata file optional during record and replay modes as of package version 8.1
 * added a no-bond option to the ``sensor.launch`` file
 * reduce the publish rate of imu tf transforms
+* implemented a new node named ``os_driver`` which combines the functionality of ``os_sensor``,
+  ``os_cloud`` and ``os_image`` into a single node. The new node can be launch via the new 
+  ``driver.launch`` file.
+* introduced a new topic ``/ouster/scan`` which publishes ``sensor_msgs::LaserScan`` messages, the
+  user can pick which beam to be used for the message through the ``scan_ring`` launch argument.
+* added ability to pick which messsages to process and through the new ``proc_mask`` launch file
+  argument.
+* introduced a new parameter ``point_cloud_frame`` to allow users to select which frame to use when
+  publishing the point cloud (choose between ``sensor`` and ``lidar``). The default publishing frame
+  the sensor one which is in line with the current behavior.
+* added the ability to change the names of ``sensor_frame``, ``lidar_frame`` and ``imu_frame``
+* added a placeholder for the ``/ouster/reset`` (not implemented for ROS1).
+* breaking: switched back to using static transforms broadcast but with ability to select the frames
+  to be updated dynamically and at what rate through the two new launch file arguments
+  ``dynamic_transforms_broadcast`` and  ``dynamic_transforms_broadcast_rate``.
+* updated RVIZ color scheme for point clouds to match with the ROS2 version of the driver.
 
 ouster_ros(2)
 -------------
-* MVP ouster-ros targeting ros2 distros (beta release)
+* MVP ouster-ros targeting ros2 distros
 * introduced a ``reset`` service to the ``os_sensor`` node
+* implemented a new node named ``os_driver`` which combines the functionality of ``os_sensor``,
+  ``os_cloud`` and ``os_image`` into a single node.
+* added support to parse the same parameters provided by the ``ros2_ouster_driver``, the parameters
+  are ``lidar_ip``, ``computer_ip``, ``proc_mask`` and ``use_system_default_qos``; the parameters
+  are fully functional and similar to what the ``ros2_ouster_driver`` provides.
+* for convenience introduced a new launch file ``driver_launch.py`` that is compatible with the 
+  ``ros2_ouster_driver`` in terms of parameters it accepts and the name of published topics.
+* introduced a new parameter ``point_cloud_frame`` to allow users to select which frame to use when
+  publishing the point cloud (choose between ``sensor`` and ``lidar``).
+* breaking: ``lidar`` frame is the default frame used when publishing point clouds.
+* added the ability to choose between ``SensorDataQoS`` or ``SystemDefaultQoS`` across all published
+  topics with ``SensorDataQoS`` selected by default for live sensor mode and ``SystemDefaultQoS``
+  enabled for record and replay modes.
+* introduced a new topic ``/ouster/scan`` which publishes ``sensor_msgs::msg::LaserScan`` messages
+
 
 ouster_client
 -------------
@@ -41,53 +72,34 @@ ouster_client
 [20230114]
 ==========
 
-ouster_ros
-----------
-* breaking change: renamed ``ouster_ros/ros.h`` to ``ouster_ros/os_ros.h`` and
-  ``ouster_ros/point.h`` to ``ouster_ros/os_point.h``.
-* breaking change: change the type of the ring field within ``ouster::Point`` from ``uint8_t`` to
-  ``uint16_t``
-* correct LICENSE file installation path.
-* update code files copyrights period.
-* bug fix: ros driver doesn't use correct udp_dest given by user during launch
-* update published TF transforms time with senosr or ros time based on the
-  active timestamp mode.
-* validate lidar and imu port values. warn users when assigning random port numbers.
-* switch to using the cartesianT method when populating pcl point cloud for performance and reduced
-  cpu utilization
-* reduce dynamic memory allocation within the driver for performance and driver stability
-* add ``pcl_ros`` as a dependency to ``package.xml``
+ouster_ros(2)
+-------------
+* MVP ouster-ros targeting ros2 distros (beta release)
+* introduced a ``reset`` service to the ``os_sensor`` node
+* breaking change: updated to ouster sdk release 20230403
+* EOL notice: ouster-ros driver will drop support for ``ROS foxy`` by May 2023.
+* bugfix: Address an issue causing the driver to warn about missing non-legacy fields even they exist
+  in the original metadata file.
+* added a new launch file ``sensor_mtp.launch.xml`` for multicast use case (experimental).
+* added a technique to estimate the the value of the lidar scan timestamp when it is missing packets
+  at the beginning
+* add frame_id to image topics
+* fixed a potential issue of time values within generated point clouds that could result in a value
+  overflow
+* added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it
+  to the bag file on record
+* make specifying metadata file optional during record and replay modes as of package version 8.1
+* replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame``
+  launch parameters.
+* bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components
+  in the same process.
+* switch to using static transform publisher for the ros2 driver.
 
 ouster_client
---------------
-* breaking change: signal multiplier type changed to double to support new FW values of signal
-  multiplier.
-* breaking change: make_xyz_lut takes mat4d beam_to_lidar_transform instead of
-  lidar_origin_to_beam_origin_mm double to accomodate new FWs. Old reference Python implementation
-  was kept, but new reference was also added.
-* address an issue that could cause the processed frame being dropped in favor or the previous
-  frame when the frame_id wraps-around.
-* added a new flag ``CONFIG_FORCE_REINIT`` for ``set_config()`` method, to force the sensor to reinit
-  even when config params have not changed.
-* breaking change: drop defaults parameters from the shortform ``init_client()`` method.
-* added a new method ``init_logger()`` to provide control over the logs emitted by ``ouster_client``.
-* add parsing for new FW 3.0 thermal features shot_limiting and thermal_shutdown statuses and countdowns
-* add frame_status to LidarScan
-* introduced a new method ``cartesianT()`` which speeds up the computation of point projecion from range
-  image, the method also can process the cartesian product with single float precision. A new unit test
-  ``cartesian_test`` which shows achieved speed up gains by the number of valid returns in lidar scan.
-* added ``RAW_HEADERS`` ChanField to LidarScan for packing headers and footer (alpha version, may be
-  changed/removed without notice in the future)
-
-
-[20221004]
-==========
-
-ouster_ros
-----------
-* Moved ouster-ros into separate repo
-* Refresh the docker file
-
-ouster_sdk
-----------
-* Removed ouster_ros
+-------------
+* added a new method ``mtp_init_client`` to init the client with multicast support (experimental).
+* the class ``SensorHttp``  which provides easy access to REST APIs of the sensor has been made public
+  under the ``ouster::sensor::util`` namespace.
+* breaking change: get_metadata defaults to outputting non-legacy metadata
+* add debug five_word profile which will be removed later
+* breaking change: remove deprecations on LidarScan

CMakeLists.txt

@@ -77,20 +77,27 @@ target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build pcl_com
 add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp)
 
 # ==== Executables ====
-add_library(nodelets_os
-  src/os_client_base_nodelet.cpp
+add_library(${PROJECT_NAME}_nodelets
+  src/os_sensor_nodelet_base.cpp
   src/os_sensor_nodelet.cpp
   src/os_replay_nodelet.cpp
   src/os_cloud_nodelet.cpp
-  src/os_image_nodelet.cpp)
-target_link_libraries(nodelets_os ouster_ros ${catkin_LIBRARIES})
-add_dependencies(nodelets_os ${PROJECT_NAME}_gencpp)
+  src/os_image_nodelet.cpp
+  src/os_driver_nodelet.cpp)
+target_link_libraries(${PROJECT_NAME}_nodelets ouster_ros ${catkin_LIBRARIES})
+add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp)
+
+# ==== Test ====
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(${PROJECT_NAME}_test tests/ring_buffer_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES})
+endif()
 
 # ==== Install ====
 install(
   TARGETS
-    ouster_ros
-    nodelets_os
+    ${PROJECT_NAME}
+    ${PROJECT_NAME}_nodelets
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -104,7 +111,7 @@ install(
 install(
   FILES
     LICENSE
-    nodelets_os.xml
+    ${PROJECT_NAME}_nodelets.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 

Dockerfile

@@ -51,10 +51,16 @@ RUN set -xe \
 
 FROM build-env
 
+SHELL ["/bin/bash", "-c"]
+
 ENV CXXFLAGS="-Werror -Wno-deprecated-declarations"
-RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make -DCMAKE_BUILD_TYPE=Release \
+RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make     \
+-DCMAKE_BUILD_TYPE=Release                      \
 && /opt/ros/$ROS_DISTRO/env.sh catkin_make install
 
+RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make --make-args tests \
+&& source ./devel/setup.bash && rosrun ouster_ros ouster_ros_test
+
 # Entrypoint for running Ouster ros:
 #
 # Usage: docker run --rm -it ouster-ros [sensor.launch parameters ..]

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)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
+[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
 [ROS2 (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) | [![rolling/humble](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) | [![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 (foxy) | [![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)
 
 - [Overview](#overview)
@@ -21,6 +21,7 @@
     - [Recording Mode](#recording-mode)
     - [Replay Mode](#replay-mode)
     - [Multicast Mode (experimental)](#multicast-mode-experimental)
+  - [Launch Files Arguments](#launch-files-arguments)
   - [Invoking Services](#invoking-services)
     - [GetMetadata](#getmetadata)
     - [GetConfig](#getconfig)
@@ -30,12 +31,12 @@
 
 ## Overview
 
-This ROS package provide support for all Ouster sensors with FW v2.0 or later. Upon launch the driver
-will configure and connect to the selected sensor device, once connected the driver will handle
-incoming IMU and lidar packets, decode lidar frames and publish corresponding ROS messages on the
-topics of `/ouster/imu` and `/ouster/points`. In the case the sensor supports dual return and it was
-configured to use this capability, then another topic will published named `/ouster/points2` which
-corresponds to the second point cloud.
+This ROS package provide support for all Ouster sensors with FW v2.0 or later. Upon launch the
+driver will configure and connect to the selected sensor device, once connected the driver will
+handle incoming IMU and lidar packets, decode lidar frames and publish corresponding ROS messages
+on the topics of `/ouster/imu` and `/ouster/points`. In the case the used sensor supports dual
+return and it was configured to use this capability, then another topic will published named
+`/ouster/points2` which corresponds to the second point cloud.
 
 ## Requirements
 This package only supports **Melodic** and **Noetic** ROS distros. Please refer to ROS online
@@ -96,11 +97,20 @@ recorded bag or _record_ a new bag file using the corresponding launch files. Re
 added a new mode that supports multicast. The commands are listed below:
 
 #### Sensor Mode
+The driver offers two launch files to connect to an Ouster sensor: `sensor.launch` and
+`driver.launch`; they differ in terms of how the processing of incoming packets is performed.
+`sensor.launch` spawns three nodelets, one to connect to the sensor and publishes raw packets to
+the two other nodelets which handles converting them into **Imu**, **Image** and **PointCloud2**
+messages. Meanwhile, `driver.launch` file spawn a single nodelet that handles all of these tasks.
+You can invoke the two files in the same way. The following line shows how to run the node using
+`driver.launch`:
 ```bash
-roslaunch ouster_ros sensor.launch      \
+roslaunch ouster_ros driver.launch      \
     sensor_hostname:=<sensor host name> \
     metadata:=<json file name>          # optional
 ```
+`driver.launch` offers better performance and reduced overhead on the ROS bus, thus it is preferred
+over `sensor.launch`. `sensor.launch` is mainly provided for backward compatibilty.
 
 #### Recording Mode
 > Note
@@ -154,6 +164,17 @@ roslaunch ouster_ros sensor_mtp.launch      \
 > In both cases the **mtp_dest** is optional and if left unset the client will utilize the first
 available interface.
 
+### Launch Files Arguments
+Each of the previously mentioned launch files include a variety of launch arguments that helps the
+user customize the driver behaivor. To view the arguments that each launch file provides and their
+purpose pass `--ros-args` along with the specific launch file that you are interested in. For
+example, to view launche arguments of the `driver.launch` use the following command:
+```bash
+roslaunch ouster_ros driver.launch --ros-args
+```
+The command should list all available arguments, whether they are optional or required and the
+description and posible values of each argument.
+
 ### Invoking Services
 To execute any of the following service, first you need to open a new terminal
 and source the castkin workspace again by running the command:

config/viz.rviz

@@ -59,17 +59,17 @@ Visualization Manager:
         Min Value: -10
         Value: true
       Axis: Z
-      Channel Name: z
+      Channel Name: range
       Class: rviz/PointCloud2
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
       Enabled: true
       Invert Rainbow: true
       Max Color: 255; 255; 255
-      Max Intensity: 15
+      Max Intensity: 10000
       Min Color: 0; 0; 0
-      Min Intensity: -10
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -89,17 +89,17 @@ Visualization Manager:
         Min Value: -10
         Value: true
       Axis: Z
-      Channel Name: z
+      Channel Name: range
       Class: rviz/PointCloud2
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
       Enabled: true
       Invert Rainbow: true
       Max Color: 255; 255; 255
-      Max Intensity: 15
+      Max Intensity: 10000
       Min Color: 0; 0; 0
-      Min Intensity: -10
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10

include/ouster_ros/os_point.h

@@ -2,7 +2,7 @@
  * Copyright (c) 2018-2023, Ouster, Inc.
  * All rights reserved.
  *
- * @file point.h
+ * @file os_point.h
  * @brief PCL point datatype for use with ouster sensors
  */