Prototype of adding support for supporting XYZIT

 - Imported from ouster-lidar#290
 - Quick Prototype of adding support for supporting XYZIT point type
 using PointCloud Customization feature point type using
 PointCloud Customization feature

# Conflicts:
#	launch/common.launch
#	launch/driver.launch
#	launch/replay.launch
#	launch/sensor.launch
#	launch/sensor_mtp.launch

README.md

@@ -284,6 +284,8 @@ Other notable parameters include:
   - `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.
 
 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.

ouster-ros/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

ouster-ros/launch/record.composite.launch.xml

@@ -72,7 +72,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <arg name="azimuth_window_start" default="0" description="azimuth window start;

ouster-ros/launch/replay.composite.launch.xml

@@ -56,7 +56,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group>

ouster-ros/launch/replay_pcap.launch.xml

@@ -50,7 +50,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <group>

ouster-ros/launch/sensor.composite.launch.xml

@@ -70,7 +70,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <arg name="azimuth_window_start" default="0" description="azimuth window start;

ouster-ros/launch/sensor.independent.launch.xml

@@ -70,7 +70,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <arg name="azimuth_window_start" default="0" description="azimuth window start;

ouster-ros/launch/sensor_mtp.launch.xml

@@ -78,7 +78,8 @@
     native,
     xyz,
     xyzi,
-    xyzir
+    xyzir,
+    xyzit
     }"/>
 
   <arg name="azimuth_window_start" default="0" description="azimuth window start;

ouster-ros/src/point_cloud_processor_factory.h

@@ -120,7 +120,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 bool profile_has_intensity(UDPProfileLidar profile) {
@@ -174,6 +174,10 @@ class PointCloudProcessorFactory {
             return make_point_cloud_processor<PointXYZIR>(
                 info, frame, apply_lidar_to_sensor_transform,
                 post_processing_fn);
+        } else if (point_type == "xyzit") {
+            return make_point_cloud_processor<PointXYZIT>(
+                info, frame, apply_lidar_to_sensor_transform,
+                post_processing_fn);
         } else if (point_type == "original") {
             return make_point_cloud_processor<ouster_ros::Point>(
                 info, frame, apply_lidar_to_sensor_transform,

ouster-ros/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; }