From 28e4d7fcbd3acd4ddb969b1220e61dcefe888f2d Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Feb 2024 09:15:39 -0800 Subject: [PATCH 1/2] Quick prototype of adding support for supporting XYZIT point type using PointCloud Customization feature --- README.md | 2 + include/ouster_ros/common_point_types.h | 49 +++++++++++++++++++++++++ launch/common.launch | 3 +- launch/driver.launch | 3 +- launch/replay.launch | 3 +- launch/sensor.launch | 3 +- launch/sensor_mtp.launch | 3 +- src/point_cloud_processor_factory.h | 6 ++- src/point_transform.h | 10 +++++ 9 files changed, 76 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 3a4de566..8f3be59f 100644 --- a/README.md +++ b/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 diff --git a/include/ouster_ros/common_point_types.h b/include/ouster_ros/common_point_types.h index 2bfea7a8..37366bce 100644 --- a/include/ouster_ros/common_point_types.h +++ b/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 + inline auto& get() { + return std::get(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 diff --git a/launch/common.launch b/launch/common.launch index ef3725e2..b4682750 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -38,7 +38,8 @@ native, xyz, xyzi, - xyzir + xyzir, + xyzit }"/> diff --git a/launch/driver.launch b/launch/driver.launch index 0cc23f05..4b838e7f 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -61,7 +61,8 @@ native, xyz, xyzi, - xyzir + xyzir, + xyzit }"/> diff --git a/launch/replay.launch b/launch/replay.launch index 85a85896..7903bd57 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -44,7 +44,8 @@ native, xyz, xyzi, - xyzir + xyzir, + xyzit }"/> diff --git a/launch/sensor.launch b/launch/sensor.launch index 003fed6a..4196cd57 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -69,7 +69,8 @@ native, xyz, xyzi, - xyzir + xyzir, + xyzit }"/> diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index db05f5eb..950e3d47 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -73,7 +73,8 @@ native, xyz, xyzi, - xyzir + xyzir, + xyzit }"/> diff --git a/src/point_cloud_processor_factory.h b/src/point_cloud_processor_factory.h index 859e92da..eac39ca4 100644 --- a/src/point_cloud_processor_factory.h +++ b/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( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); + } else if (point_type == "xyzit") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); } else if (point_type == "original") { return make_point_cloud_procssor( info, frame, apply_lidar_to_sensor_transform, diff --git a/src/point_transform.h b/src/point_transform.h index 128fc024..09324ec3 100644 --- a/src/point_transform.h +++ b/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_t_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.timestamp = src_pt.t; } + ); + + CondBinaryOp && !has_t_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { tgt_pt.timestamp = 0U; } + ); + // ring CondBinaryOp && has_ring_v>::run( tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.ring = src_pt.ring; } From 6ae1d9d96e8d823ed24dd4b2c0d0c4d26bed5df6 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Feb 2024 11:26:51 -0800 Subject: [PATCH 2/2] Update record.launch too to include the xyzit option --- launch/record.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/record.launch b/launch/record.launch index ab32e6b5..8788e768 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -64,7 +64,8 @@ native, xyz, xyzi, - xyzir + xyzir, + xyzit }"/>