SW-5466: Support velodyne point type in the ros driver (foxy) (#255)

* Port changes from ros2 main branch to the foxy branch
ouster-lidar · Nov 15, 2023 · df7918a · df7918a
1 parent 87e6ce9
commit df7918a
Show file tree

Hide file tree

Showing 29 changed files with 1,968 additions and 587 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -8,9 +8,14 @@ Changelog
 * introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds
   to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode
   is used.
+  * [BREAKING]: the default value of ``ptp_utc_tai_offset`` is set to ``-37.0``. To retain the same
+    time offset for an existing system, users need to set ``ptp_utc_tai_offset`` to ``0.0``.
 * fix: destagger columns timestamp when generating destaggered point clouds.
 * shutdown the driver when unable to connect to the sensor on startup
-
+* [BREAKING]: rename ouster_msgs to ouster_sensor_msgs
+* added the ability to customize the published point clouds(s) to velodyne point cloud format and
+  other common pcl point types.
+* ouster_image_compoenent can operate separately from ouster_cloud_component.
 
 ouster_ros v0.10.0
 ==================

diff --git a/README.md b/README.md
@@ -12,24 +12,26 @@
 | 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 (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)
 
-- [Overview](#overview)
-- [Requirements](#requirements)
-  - [Linux](#linux)
-  - [Windows](#windows)
-  - [Mac](#mac)
-- [Getting Started](#getting-started)
-- [Usage](#usage)
-  - [Launching Nodes](#launching-nodes)
-    - [Sensor Mode](#sensor-mode)
-    - [Recording Mode](#recording-mode)
-    - [Replay Mode](#replay-mode)
-    - [Multicast Mode (experimental)](#multicast-mode-experimental)
-  - [Invoking Services](#invoking-services)
-    - [GetMetadata](#getmetadata)
-    - [GetConfig](#getconfig)
-    - [SetConfig](#setconfig)
-    - [Reset](#reset)
-- [License](#license)
+- [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors)
+  - [Overview](#overview)
+  - [Requirements](#requirements)
+    - [Linux](#linux)
+    - [Windows](#windows)
+    - [Mac](#mac)
+  - [Getting Started](#getting-started)
+  - [Usage](#usage)
+    - [Launching Nodes](#launching-nodes)
+      - [Sensor Mode](#sensor-mode)
+      - [Recording Mode](#recording-mode)
+      - [Replay Mode](#replay-mode)
+      - [Multicast Mode (experimental)](#multicast-mode-experimental)
+    - [Invoking Services](#invoking-services)
+      - [GetMetadata](#getmetadata)
+      - [GetConfig](#getconfig)
+      - [SetConfig](#setconfig)
+      - [Reset](#reset)
+    - [Driver Parameters](#driver-parameters)
+  - [License](#license)
 
 
 ## Overview
@@ -65,7 +67,7 @@ sudo apt install -y             \
     ros-$ROS_DISTRO-tf2-eigen   \
     ros-$ROS_DISTRO-rviz2
 ```
-where `$ROS_DISTRO` is ``foxy``.
+where `$ROS_DISTRO` is ``foxy`` or ``galactic``.
 
 > **Note**  
 > Installing `ros-$ROS_DISTRO-rviz` package is optional in case you didn't need to visualize the
@@ -107,7 +109,7 @@ git clone -b ros2-foxy --recurse-submodules https://github.com/ouster-lidar/oust
 
 Next to compile the driver you need to source the ROS environemt into the active termainl:
 ```bash
-source /opt/ros/<ros-distro>/setup.bash # replace ros-distro with 'foxy'
+source /opt/ros/<ros-distro>/setup.bash # replace ros-distro with 'foxy' or 'galactic'
 ```
 
 Finally, invoke `colcon build` command from within the catkin workspace as shown below:
@@ -130,7 +132,7 @@ source ros2_ws/install/setup.bash
 The package supports three modes of interaction, you can connect to a _live sensor_, _replay_ a recorded
 bag or _record_ a new bag file using the corresponding launch files. Recently, we have added a new mode
 that supports multicast. The commands are listed below, for convenience we do provide both launch file
-formats (xml and python) but the python format is the preferred method.
+formats (xml and python) but the python format is the preferred method:
 
 #### Sensor Mode
 To connect to a live sensor you use the following launch file
@@ -241,8 +243,32 @@ connection, reset the sensor and reconnect again.
 > **Note**
 > Changing settings is not yet fully support during a reset operation (more on this)
 
-TBD: For further detailed instructions refer to the [main guide](./docs/index.rst)
-
+### Driver Parameters
+The driver has several parameters that allow you to customize its behavior, all of
+these parameters are defined with the `driver_params.yaml` file found under `config`
+folder. The only required parameter is `sensor_hostname` which sets the sensor
+hostname or ip that you want to connect to through ouster-ros driver.
+
+Other notable parameters include:
+* **point_type**: This parameter allows to customize the point cloud that the
+  driver produces through its `/ouster/points` topics. Choose one of the following
+  values:
+  - `original`: This uses the original point representation `ouster_ros::Point`
+           of the ouster-ros driver.
+  - `native`: directly maps all fields as published by the sensor to an
+           equivalent point cloud representation with the additon of ring
+           and timestamp fields.
+  - `xyz`: the simplest point type, only has {x, y, z}
+  - `xyzi`: same as xyz point type but adds intensity (signal) field. this
+           type is not compatible with the low data profile.
+  - `xyzir`: same as xyzi type but adds ring (channel) field.
+          this type is same as Velodyne point cloud type
+          this type is not compatible with the low data profile.
+
+This is not a comprehenisve list of all the parameters that the driver supports
+for more detailed list please refer to the `config/driver_params.yaml` file.
+
+For further detailed instructions about the driver refer to the [main guide](./docs/index.rst)
 
 ## License
 [License File](./LICENSE)
diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt
@@ -154,7 +154,7 @@ rclcpp_components_register_node(os_image_component
   EXECUTABLE os_image
 )
 
-# ==== os_sensor_component ====
+# ==== os_driver_component ====
 create_ros2_component(os_driver_component
   "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp"
   "std_srvs"
@@ -168,11 +168,23 @@ rclcpp_components_register_node(os_driver_component
 # ==== Test ====
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
-  ament_add_gtest(${PROJECT_NAME}_test test/ring_buffer_test.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test
+    test/ring_buffer_test.cpp
+    src/os_ros.cpp
+    test/point_accessor_test.cpp
+    test/point_transform_test.cpp
+    test/point_cloud_compose_test.cpp
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test
+    rclcpp
+    ouster_sensor_msgs
+  )
   target_include_directories(${PROJECT_NAME}_test PUBLIC
+    ${_ouster_ros_INCLUDE_DIRS}
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
   )
+  target_link_libraries(${PROJECT_NAME}_test ouster_ros_library)
 endif()
 
 

diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml
@@ -67,3 +67,22 @@ ouster/os_driver:
     # data QoS. This is preferrable for production but default QoS is needed for
     # rosbag. See: https://github.com/ros2/rosbag2/issues/125
     use_system_default_qos: false
+    # point_type[optional]: choose from: {original, native, xyz, xyzi, xyzir}
+    # Here is a breif description of each option:
+    #  - original: This uses the original point representation ouster_ros::Point
+    #          of the ouster-ros driver.
+    #  - native: directly maps all fields as published by the sensor to an
+    #          equivalent point cloud representation with the additon of ring
+    #          and timestamp fields.
+    #  - xyz: the simplest point type, only has {x, y, z}
+    #  - xyzi: same as xyz point type but adds intensity (signal) field. this
+    #          type is not compatible with the low data profile.
+    #  - xyzir: same as xyzi type but adds ring (channel) field.
+    #          this type is same as Velodyne point cloud type
+    #          this type is not compatible with the low data profile.
+    # for more details about the fields of each point type and their data refer
+    # to the following header files:
+    # - ouster_ros/os_point.h
+    # - ouster_ros/sensor_point_types.h
+    # - ouster_ros/common_point_types.h.
+    point_type: original
diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml
@@ -27,5 +27,6 @@ ouster/os_cloud:
     use_system_default_qos: False # needs to match the value defined for os_sensor node
     scan_ring: 0  # Use this parameter in conjunction with the SCAN flag and choose a
                   # value the range [0, sensor_beams_count)
+    point_type: original # choose from: {original, native, xyz, xyzi, xyzir}
 ouster/os_image:
     use_system_default_qos: False # needs to match the value defined for os_sensor node
diff --git a/ouster-ros/include/ouster_ros/common_point_types.h b/ouster-ros/include/ouster_ros/common_point_types.h
@@ -0,0 +1,72 @@
+/**
+ * Copyright (c) 2018-2023, Ouster, Inc.
+ * All rights reserved.
+ *
+ * @file common_point_types.h
+ * @brief common PCL point datatype for use with ouster sensors
+ */
+
+#pragma once
+
+#include <pcl/point_types.h>
+
+namespace ouster_ros {
+
+/* The following are pcl point representations that are common/standard point
+   representation that we readily support.
+ */
+
+/*
+ * Same as Velodyne point cloud type
+ * @remark XYZIR point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA
+ * udp lidar profile.
+ */
+struct EIGEN_ALIGN16 _PointXYZIR {
+    PCL_ADD_POINT4D;
+    float intensity;
+    uint16_t ring;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct PointXYZIR : public _PointXYZIR {
+
+    inline PointXYZIR(const _PointXYZIR& pt)
+    {
+      x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
+      intensity = pt.intensity; ring = pt.ring;
+    }
+
+    inline PointXYZIR()
+    {
+      x = y = z = 0.0f; data[3] = 1.0f;
+      intensity = 0.0f; ring = 0;
+    }
+
+    inline const auto as_tuple() const {
+        return std::tie(x, y, z, intensity, ring);
+    }
+
+    inline auto as_tuple() {
+        return std::tie(x, y, z, intensity, ring);
+    }
+
+    template<size_t I>
+    inline auto& get() {
+        return std::get<I>(as_tuple());
+    }
+};
+
+}   // namespace ouster_ros
+
+// clang-format off
+
+/* common point types */
+POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZIR,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, intensity, intensity)
+    (std::uint16_t, ring, ring)
+)
+
+// clang-format on
diff --git a/ouster-ros/include/ouster_ros/os_point.h b/ouster-ros/include/ouster_ros/os_point.h
@@ -10,36 +10,79 @@
 
 #include <pcl/point_types.h>
 
-#include <Eigen/Core>
-#include <chrono>
-
-#include <ouster/lidar_scan.h>
-
 namespace ouster_ros {
 
-struct EIGEN_ALIGN16 Point {
+// The default/original represntation of the point cloud since the driver
+// inception. This shouldn't be confused with Point_LEGACY which provides exact
+// mapping of the fields of Ouster LidarScan of the Legacy Profile, copying the 
+// the same order and using the same bit representation. For example, this Point
+// struct uses float data type to represent intensity (aka signal); however, the
+// sensor sends the signal channel either as UINT16 or UINT32 depending on the
+// active udp lidar profile.
+struct EIGEN_ALIGN16 _Point {
     PCL_ADD_POINT4D;
-    float intensity;
+    float intensity;        // equivalent to signal
     uint32_t t;
     uint16_t reflectivity;
-    uint16_t ring;
-    uint16_t ambient;
+    uint16_t ring;          // equivalent to channel
+    uint16_t ambient;       // equivalent to near_ir
     uint32_t range;
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
-}  // namespace ouster_ros
+
+struct Point : public _Point {
+
+    inline Point(const _Point& pt)
+    {
+      x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
+      intensity = pt.intensity;
+      t = pt.t;
+      reflectivity = pt.reflectivity; 
+      ring = pt.ring;
+      ambient = pt.ambient;
+      range = pt.range;
+    }
+
+    inline Point()
+    {
+      x = y = z = 0.0f; data[3] = 1.0f;
+      intensity = 0.0f;
+      t = 0;
+      reflectivity = 0;
+      ring = 0;
+      ambient = 0;
+      range = 0;
+    }
+
+    inline const auto as_tuple() const {
+        return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range);
+    }
+
+    inline auto as_tuple() {
+        return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range);
+    }
+
+    template<size_t I>
+    inline auto& get() {
+        return std::get<I>(as_tuple());
+    }
+};
+
+}   // namespace ouster_ros
 
 // clang-format off
+
+// DEFAULT/ORIGINAL
 POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
     (float, x, x)
     (float, y, y)
     (float, z, z)
     (float, intensity, intensity)
-    // use std::uint32_t to avoid conflicting with pcl::uint32_t
     (std::uint32_t, t, t)
     (std::uint16_t, reflectivity, reflectivity)
     (std::uint16_t, ring, ring)
     (std::uint16_t, ambient, ambient)
     (std::uint32_t, range, range)
 )
+
 // clang-format on