ROS-287: Ouster support for Apollo

README.md

@@ -202,6 +202,8 @@ New launch file parameter:
   - `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.
+  - `xyzit`: same as xyzi type but adds timestamp field.
+          this type is not compatible with the low data profile.
 
 ### Invoking Services
 To execute any of the following service, first you need to open a new terminal

include/ouster_ros/common_point_types.h

@@ -56,11 +56,52 @@ struct PointXYZIR : public _PointXYZIR {
     }
 };
 
+/*
+ * Same as Apollo point cloud type
+ * @remark XYZIT point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA
+ * udp lidar profile.
+ */
+struct EIGEN_ALIGN16 _PointXYZIT {
+    PCL_ADD_POINT4D;
+    uint32_t intensity;
+    uint64_t timestamp;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct PointXYZIT : public _PointXYZIT {
+
+    inline PointXYZIT(const _PointXYZIT& pt)
+    {
+      x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
+      intensity = pt.intensity; timestamp = pt.timestamp;
+    }
+
+    inline PointXYZIT()
+    {
+      x = y = z = 0.0f; data[3] = 1.0f;
+      intensity = 0.0f; timestamp = 0;
+    }
+
+    inline const auto as_tuple() const {
+        return std::tie(x, y, z, intensity, timestamp);
+    }
+
+    inline auto as_tuple() {
+        return std::tie(x, y, z, intensity, timestamp);
+    }
+
+    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)
@@ -69,4 +110,12 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZIR,
     (std::uint16_t, ring, ring)
 )
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZIT,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, intensity, intensity)
+    (std::uint16_t, timestamp, timestamp)
+)
+
 // clang-format on

launch/common.launch

@@ -38,7 +38,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
 

launch/driver.launch

@@ -61,7 +61,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group ns="$(arg ouster_ns)">

launch/record.launch

@@ -64,7 +64,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group ns="$(arg ouster_ns)">

launch/replay.launch

@@ -44,7 +44,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group ns="$(arg ouster_ns)">

launch/sensor.launch

@@ -69,7 +69,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group ns="$(arg ouster_ns)">

launch/sensor_mtp.launch

@@ -73,7 +73,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group ns="$(arg ouster_ns)">

src/point_cloud_processor_factory.h

@@ -97,7 +97,7 @@ class PointCloudProcessorFactory {
    public:
     static bool point_type_requires_intensity(const std::string& point_type) {
         return point_type == "xyzi" || point_type == "xyzir" ||
-               point_type == "original";
+               point_type == "xyzit" || point_type == "original";
     }
 
     static LidarScanProcessor create_point_cloud_processor(
@@ -140,6 +140,10 @@ class PointCloudProcessorFactory {
             return make_point_cloud_procssor<PointXYZIR>(
                 info, frame, apply_lidar_to_sensor_transform,
                 post_processing_fn);
+        } else if (point_type == "xyzit") {
+            return make_point_cloud_procssor<PointXYZIT>(
+                info, frame, apply_lidar_to_sensor_transform,
+                post_processing_fn);
         } else if (point_type == "original") {
             return make_point_cloud_procssor<ouster_ros::Point>(
                 info, frame, apply_lidar_to_sensor_transform,

src/point_transform.h

@@ -19,6 +19,7 @@ DEFINE_MEMBER_CHECKER(x);
 DEFINE_MEMBER_CHECKER(y);
 DEFINE_MEMBER_CHECKER(z);
 DEFINE_MEMBER_CHECKER(t);
+DEFINE_MEMBER_CHECKER(timestamp);
 DEFINE_MEMBER_CHECKER(ring);
 DEFINE_MEMBER_CHECKER(intensity);
 DEFINE_MEMBER_CHECKER(ambient);
@@ -41,6 +42,15 @@ void transform(PointTGT& tgt_pt, const PointSRC& src_pt) {
         tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { tgt_pt.t = 0U; }
     );
 
+    // t: timestamp
+    CondBinaryOp<has_timestamp_v<PointTGT> && has_t_v<PointSRC>>::run(
+        tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.timestamp = src_pt.t; }
+    );
+
+    CondBinaryOp<has_timestamp_v<PointTGT> && !has_t_v<PointSRC>>::run(
+        tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { tgt_pt.timestamp = 0U; }
+    );
+
     // ring
     CondBinaryOp<has_ring_v<PointTGT> && has_ring_v<PointSRC>>::run(
         tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.ring = src_pt.ring; }