Port configurable point cloud feature to ros1

CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog
 =========
 
+[unreleased]
+============
+* added the ability to customize the published point clouds(s) to velodyne point cloud format and
+  other common pcl point types.
+* ouster_image_nodelet can operate separately from ouster_cloud_nodelet.
+
+
 ouster_ros v0.10.0
 ==================
 

CMakeLists.txt

@@ -89,7 +89,13 @@ add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp)
 
 # ==== Test ====
 if(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(${PROJECT_NAME}_test tests/ring_buffer_test.cpp)
+  catkin_add_gtest(${PROJECT_NAME}_test
+    tests/ring_buffer_test.cpp
+    src/os_ros.cpp
+    tests/point_accessor_test.cpp
+    tests/point_transform_test.cpp
+    tests/point_cloud_compose_test.cpp
+  )
   target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES})
 endif()
 

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

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

include/ouster_ros/os_ros.h

@@ -29,8 +29,6 @@
 namespace ouster_ros {
 
 namespace sensor = ouster::sensor;
-using Cloud = pcl::PointCloud<Point>;
-using ns = std::chrono::nanoseconds;
 
 /**
  * Checks sensor_info if it currently represents a legacy udp lidar profile
@@ -40,13 +38,13 @@ using ns = std::chrono::nanoseconds;
 bool is_legacy_lidar_profile(const sensor::sensor_info& info);
 
 /**
- * Gets the number of point cloud returns that this sensor_info object
- * represents
+ * Gets the number of point cloud returns that this sensor_info object represents
  * @param[in] info sensor_info
  * @return number of returns
  */
 int get_n_returns(const sensor::sensor_info& info);
 
+
 /**
  * Gets the number beams based on supplied sensor_info
  * @param[in] info sensor_info
@@ -88,74 +86,6 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm,
                                    const std::string& frame,
                                    const sensor::packet_format& pf);
 
-/**
- * Populate a PCL point cloud from a LidarScan.
- * @param[in, out] points The points parameters is used to store the results of
- * the cartesian product before it gets packed into the cloud object.
- * @param[in] lut_direction the direction of the xyz lut (with single precision)
- * @param[in] lut_offset the offset of the xyz lut (with single precision)
- * @param[in] scan_ts scan start used to caluclate relative timestamps for
- * points.
- * @param[in] lidar_scan input lidar data
- * @param[out] cloud output pcl pointcloud to populate
- * @param[in] return_index index of return desired starting at 0
- */
-[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f(
-    ouster::PointsF& points, const ouster::PointsF& lut_direction,
-    const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts,
-    const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud,
-    int return_index);
-
-/**
- * Populate a PCL point cloud from a LidarScan.
- * @param[in, out] points The points parameters is used to store the results of
- * the cartesian product before it gets packed into the cloud object.
- * @param[in] lut_direction the direction of the xyz lut (with single precision)
- * @param[in] lut_offset the offset of the xyz lut (with single precision)
- * @param[in] scan_ts scan start used to caluclate relative timestamps for
- * points
- * @param[in] lidar_scan input lidar data
- * @param[out] cloud output pcl pointcloud to populate
- * @param[in] return_index index of return desired starting at 0
- */
-void scan_to_cloud_f(ouster::PointsF& points,
-                     const ouster::PointsF& lut_direction,
-                     const ouster::PointsF& lut_offset, uint64_t scan_ts,
-                     const ouster::LidarScan& lidar_scan,
-                     ouster_ros::Cloud& cloud, int return_index);
-
-/**
- * Populate a destaggered PCL point cloud from a LidarScan
- * @param[out] cloud output pcl pointcloud to populate
- * @param[in, out] points The points parameters is used to store the results of
- * the cartesian product before it gets packed into the cloud object.
- * @param[in] lut_direction the direction of the xyz lut (with single precision)
- * @param[in] lut_offset the offset of the xyz lut (with single precision)
- * @param[in] scan_ts scan start used to caluclate relative timestamps for
- * points
- * @param[in] lidar_scan input lidar data
- * @param[in] pixel_shift_by_row pixel shifts by row
- * @param[in] return_index index of return desired starting at 0
- */
-void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud,
-                     ouster::PointsF& points,
-                     const ouster::PointsF& lut_direction,
-                     const ouster::PointsF& lut_offset, uint64_t scan_ts,
-                     const ouster::LidarScan& ls,
-                     const std::vector<int>& pixel_shift_by_row,
-                     int return_index);
-
-/**
- * Serialize a PCL point cloud to a ROS message
- * @param[in] cloud the PCL point cloud to convert
- * @param[in] timestamp the timestamp to apply to the resulting ROS message
- * @param[in] frame the frame to set in the resulting ROS message
- * @return a ROS message containing the point cloud
- */
-sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud,
-                                            const ros::Time& timestamp,
-                                            const std::string& frame);
-
 /**
  * Convert transformation matrix return by sensor to ROS transform
  * @param[in] mat transformation matrix return by sensor
@@ -179,9 +109,12 @@ geometry_msgs::TransformStamped transform_to_tf_msg(
  * @return ROS message suitable for publishing as a LaserScan
  */
 sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg(
-    const ouster::LidarScan& ls, const ros::Time& timestamp,
-    const std::string& frame, const ouster::sensor::lidar_mode lidar_mode,
-    const uint16_t ring, const int return_index);
+    const ouster::LidarScan& ls,
+    const ros::Time& timestamp,
+    const std::string &frame,
+    const ouster::sensor::lidar_mode lidar_mode,
+    const uint16_t ring,
+    const int return_index);
 
 namespace impl {
 sensor::ChanField suitable_return(sensor::ChanField input_field, bool second);
@@ -197,10 +130,7 @@ struct read_and_cast {
 template <typename T>
 inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField field,
                                          const ouster::LidarScan& ls) {
-    if (!ls.field_type(field)) {
-        return ouster::img_t<T>::Zero(ls.h, ls.w);
-    }
-
+    if (!ls.field_type(field)) return ouster::img_t<T>::Zero(ls.h, ls.w);
     ouster::img_t<T> result{ls.h, ls.w};
     ouster::impl::visit_field(ls, field, read_and_cast(), result);
     return result;
@@ -221,6 +151,13 @@ inline ros::Time ts_to_ros_time(uint64_t ts) {
     return t;
 }
 
-}  // namespace impl
+std::set<std::string> parse_tokens(const std::string& input, char delim);
+
+inline bool check_token(const std::set<std::string>& tokens,
+                        const std::string& token) {
+    return tokens.find(token) != tokens.end();
+}
+
+} // namespace impl
 
 }  // namespace ouster_ros