From af70a816f9289ee10386350ce540568a5b1f00f5 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Mon, 3 Jun 2024 10:02:18 -0700 Subject: [PATCH] Support FUSA dual returns udp profile [NOETIC] (#334) * Add support for FUSA profile in the driver * Set xyz values of individual points in the PointCloud to NaNs when range is zero * Mark the change for xyz changed to NaNs as breaking * mention the timestamp unit --- CHANGELOG.rst | 4 +- include/ouster_ros/sensor_point_types.h | 96 +++++++++++++++++++++++-- launch/driver.launch | 3 +- launch/record.launch | 3 +- launch/sensor.launch | 3 +- launch/sensor_mtp.launch | 3 +- package.xml | 2 +- src/impl/cartesian.h | 64 +++++++++++++++++ src/os_cloud_nodelet.cpp | 6 +- src/os_driver_nodelet.cpp | 6 +- src/os_ros.cpp | 9 +-- src/point_cloud_processor.h | 4 +- src/point_cloud_processor_factory.h | 52 +++++++++++--- 13 files changed, 227 insertions(+), 28 deletions(-) create mode 100644 src/impl/cartesian.h diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c2cf516d..16f70070 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -12,7 +12,9 @@ Changelog * [BUGFIX]: LaserScan is not properly aligned with generated point cloud * address an issue where LaserScan appeared different on FW prior to 2.4 * [BUGFIX]: LaserScan does not work when using dual mode -* [BUGFIX]: Implement lock free ring buffer with throttling +* [BUGFIX]: Implement lock free ring buffer with throttling to avoid generating partial frames +* add support for FUSA udp profile ``FUSA_RNG15_RFL8_NIR8_DUAL``. +* [BREAKING] Set xyz values of individual points in the PointCloud to NaNs when range is zero. ouster_ros v0.10.0 diff --git a/include/ouster_ros/sensor_point_types.h b/include/ouster_ros/sensor_point_types.h index 7d73e6a1..fa2a9a13 100644 --- a/include/ouster_ros/sensor_point_types.h +++ b/include/ouster_ros/sensor_point_types.h @@ -46,7 +46,7 @@ static constexpr ChanFieldTable<4> Profile_LEGACY{{ // auto=LEGACY struct EIGEN_ALIGN16 _Point_LEGACY { PCL_ADD_POINT4D; - uint32_t t; // timestamp relative to frame + uint32_t t; // timestamp in nanoseconds relative to frame start uint16_t ring; // equivalent to channel uint32_t range; uint32_t signal; // equivalent to intensity @@ -133,7 +133,7 @@ static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETUR // auto=RNG19_RFL8_SIG16_NIR16_DUAL struct EIGEN_ALIGN16 _Point_RNG19_RFL8_SIG16_NIR16_DUAL { PCL_ADD_POINT4D; - uint32_t t; // timestamp relative to frame start time + uint32_t t; // timestamp in nanoseconds relative to frame start uint16_t ring; // equivalent channel uint32_t range; uint16_t signal; // equivalent to intensity @@ -205,7 +205,7 @@ static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16{{ // auto=RNG19_RFL8_SIG16_NIR16 struct EIGEN_ALIGN16 _Point_RNG19_RFL8_SIG16_NIR16 { PCL_ADD_POINT4D; - uint32_t t; // timestamp relative to frame start time + uint32_t t; // timestamp in nanoseconds relative to frame start uint16_t ring; // equivalent channel uint32_t range; uint16_t signal; // equivalent to intensity @@ -277,7 +277,7 @@ static constexpr ChanFieldTable<3> Profile_RNG15_RFL8_NIR8{{ struct EIGEN_ALIGN16 _Point_RNG15_RFL8_NIR8 { PCL_ADD_POINT4D; // No signal/intensity in low data mode - uint32_t t; // timestamp relative to frame + uint32_t t; // timestamp in nanoseconds relative to frame start uint16_t ring; // equivalent to channel uint32_t range; uint16_t reflectivity; @@ -334,3 +334,91 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG15_RFL8_NIR8, ) // clang-format on + + +namespace ouster_ros { + +// Profile_FUSA_RNG15_RFL8_NIR8_DUAL: aka fusa dual returns +// This profile is definied differently from PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL of how +// the sensor actually sends the data. The actual PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL +// has 5 fields not 3, but this profile is defined differently in ROS because +// we build and publish a point cloud for each return separately. However, it +// might be desireable to some of the users to choose a point cloud +// representation which combines parts of the the two or more returns. This isn't +// something that the current framework could deal with as of now. +static constexpr ChanFieldTable<3> Profile_FUSA_RNG15_RFL8_NIR8_DUAL {{ + {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT8}, + {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, +}}; + +// Note: this is one way to implement the processing of 2nd return +// This should be an exact copy of Profile_FUSA_RNG15_RFL8_NIR8_DUAL with the +// exception of ChanField values for the first three fields. NEAR_IR is same for both +static constexpr ChanFieldTable<3> Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN {{ + {sensor::ChanField::RANGE2, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY2, sensor::ChanFieldType::UINT8}, + {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, +}}; + +// auto=RNG19_RFL8_SIG16_NIR16_DUAL +struct EIGEN_ALIGN16 _Point_FUSA_RNG15_RFL8_NIR8_DUAL { + PCL_ADD_POINT4D; + uint32_t t; // timestamp in nanoseconds relative to frame start + uint16_t ring; // equivalent to channel + uint32_t range; + uint8_t reflectivity; + uint16_t near_ir; // equivalent to ambient + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct Point_FUSA_RNG15_RFL8_NIR8_DUAL : public _Point_FUSA_RNG15_RFL8_NIR8_DUAL { + + inline Point_FUSA_RNG15_RFL8_NIR8_DUAL(const _Point_FUSA_RNG15_RFL8_NIR8_DUAL& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + t = pt.t; ring = pt.ring; + range = pt.range; + reflectivity = pt.reflectivity; + near_ir = pt.near_ir; + } + + inline Point_FUSA_RNG15_RFL8_NIR8_DUAL() + { + x = y = z = 0.0f; data[3] = 1.0f; + t = 0; ring = 0; + range = 0; + reflectivity = 0; + near_ir = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, t, ring, range, reflectivity, near_ir); + } + + inline auto as_tuple() { + return std::tie(x, y, z, t, ring, range, reflectivity, near_ir); + } + + template + inline auto& get() { + return std::get(as_tuple()); + } +}; + +} // namespce ouster_ros + +// clang-format off + +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_FUSA_RNG15_RFL8_NIR8_DUAL, + (float, x, x) + (float, y, y) + (float, z, z) + (std::uint32_t, t, t) + (std::uint16_t, ring, ring) + (std::uint32_t, range, range) + (std::uint8_t, reflectivity, reflectivity) + (std::uint16_t, near_ir, near_ir) +) + +// clang-format on \ No newline at end of file diff --git a/launch/driver.launch b/launch/driver.launch index 0cc23f05..0f6f1002 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -7,9 +7,10 @@ ouster_ros - 0.12.2 + 0.12.3 Ouster ROS driver ouster developers BSD diff --git a/src/impl/cartesian.h b/src/impl/cartesian.h new file mode 100644 index 00000000..ac524faf --- /dev/null +++ b/src/impl/cartesian.h @@ -0,0 +1,64 @@ +#pragma once + +#include + +namespace ouster { + +// TODO: move to the sdk client + +/** + * This is the same function as the cartesianT method defined in the client but + * allows the user choose a specific value for range values of zero. + * + * @param[in, out] points The resulting point cloud, should be pre-allocated and + * have the same dimensions as the direction array. + * @param[in] range a range image in the same format as the RANGE field of a + * LidarScan. + * @param[in] direction the direction of an xyz lut. + * @param[in] offset the offset of an xyz lut. + * @param[in] invalid the value to assign of an xyz lut. + * + * @return Cartesian points where ith row is a 3D point which corresponds + * to ith pixel in LidarScan where i = row * w + col. + */ +template +void cartesianT(PointsT& points, + const Eigen::Ref>& range, + const PointsT& direction, const PointsT& offset, + T invalid) { + assert(points.rows() == direction.rows() && + "points & direction row count mismatch"); + assert(points.rows() == offset.rows() && + "points & offset row count mismatch"); + assert(points.rows() == range.size() && + "points and range image size mismatch"); + + const auto pts = points.data(); + const auto* const rng = range.data(); + const auto* const dir = direction.data(); + const auto* const ofs = offset.data(); + + const auto N = range.size(); + const auto col_x = 0 * N; // 1st column of points (x) + const auto col_y = 1 * N; // 2nd column of points (y) + const auto col_z = 2 * N; // 3rd column of points (z) + +#ifdef __OUSTER_UTILIZE_OPENMP__ +#pragma omp parallel for schedule(static) +#endif + for (auto i = 0; i < N; ++i) { + const auto r = rng[i]; + const auto idx_x = col_x + i; + const auto idx_y = col_y + i; + const auto idx_z = col_z + i; + if (r == 0) { + pts[idx_x] = pts[idx_y] = pts[idx_z] = invalid; + } else { + pts[idx_x] = r * dir[idx_x] + ofs[idx_x]; + pts[idx_y] = r * dir[idx_y] + ofs[idx_y]; + pts[idx_z] = r * dir[idx_z] + ofs[idx_z]; + } + } +} + +} // namespace ouster diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index c1f7f3f8..f808ce59 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -136,9 +136,11 @@ class OusterCloud : public nodelet::Nodelet { // warn about profile incompatibility if (PointCloudProcessorFactory::point_type_requires_intensity(point_type) && - info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) { + !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) { NODELET_WARN_STREAM( - "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8"); + "selected point type '" << point_type + << "' is not compatible with the udp profile: " + << to_string(info.format.udp_profile_lidar)); } } diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index bfe84927..8f71217b 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -89,9 +89,11 @@ class OusterDriver : public OusterSensor { // warn about profile incompatibility if (PointCloudProcessorFactory::point_type_requires_intensity(point_type) && - info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) { + !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) { NODELET_WARN_STREAM( - "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8"); + "selected point type '" << point_type + << "' is not compatible with the udp profile: " + << to_string(info.format.udp_profile_lidar)); } } diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 171171b9..ec196b90 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -35,10 +35,11 @@ bool is_legacy_lidar_profile(const sensor::sensor_info& info) { int get_n_returns(const sensor::sensor_info& info) { using sensor::UDPProfileLidar; - return info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; + if (info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL || + info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) + return 2; + + return 1; } size_t get_beams_count(const sensor::sensor_info& info) { diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h index 593e8d7d..e4824439 100644 --- a/src/point_cloud_processor.h +++ b/src/point_cloud_processor.h @@ -16,6 +16,7 @@ #include "point_cloud_compose.h" #include "lidar_packet_handler.h" +#include "impl/cartesian.h" namespace ouster_ros { @@ -78,7 +79,8 @@ class PointCloudProcessor { for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { auto range_channel = static_cast(sensor::ChanField::RANGE + i); auto range = lidar_scan.field(range_channel); - ouster::cartesianT(points, range, lut_direction, lut_offset); + ouster::cartesianT(points, range, lut_direction, lut_offset, + std::numeric_limits::quiet_NaN()); scan_to_cloud_fn(cloud, points, scan_ts, lidar_scan, pixel_shift_by_row, i); diff --git a/src/point_cloud_processor_factory.h b/src/point_cloud_processor_factory.h index 859e92da..ba59f280 100644 --- a/src/point_cloud_processor_factory.h +++ b/src/point_cloud_processor_factory.h @@ -78,13 +78,36 @@ class PointCloudProcessorFactory { pixel_shift_by_row); }; + case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL: + return [](ouster_ros::Cloud& cloud, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { + Point_FUSA_RNG15_RFL8_NIR8_DUAL staging_pt; + if (return_index == 0) { + scan_to_cloud_f_destaggered< + Profile_FUSA_RNG15_RFL8_NIR8_DUAL.size(), + Profile_FUSA_RNG15_RFL8_NIR8_DUAL>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + } else { + scan_to_cloud_f_destaggered< + Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN + .size(), + Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + } + }; + default: throw std::runtime_error("unsupported udp_profile_lidar"); } } template - static LidarScanProcessor make_point_cloud_procssor( + static LidarScanProcessor make_point_cloud_processor( const sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, PointCloudProcessor_PostProcessingFn post_processing_fn) { @@ -100,6 +123,12 @@ class PointCloudProcessorFactory { point_type == "original"; } + static bool profile_has_intensity(UDPProfileLidar profile) { + return profile == UDPProfileLidar::PROFILE_LIDAR_LEGACY || + profile == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL || + profile == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16; + } + static LidarScanProcessor create_point_cloud_processor( const std::string& point_type, const sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, @@ -107,21 +136,26 @@ class PointCloudProcessorFactory { if (point_type == "native") { switch (info.format.udp_profile_lidar) { case UDPProfileLidar::PROFILE_LIDAR_LEGACY: - return make_point_cloud_procssor( + return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL: - return make_point_cloud_procssor< + return make_point_cloud_processor< Point_RNG19_RFL8_SIG16_NIR16_DUAL>( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16: - return make_point_cloud_procssor< + return make_point_cloud_processor< Point_RNG19_RFL8_SIG16_NIR16>( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8: - return make_point_cloud_procssor( + return make_point_cloud_processor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL: + return make_point_cloud_processor< + Point_FUSA_RNG15_RFL8_NIR8_DUAL>( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); default: @@ -129,19 +163,19 @@ class PointCloudProcessorFactory { throw std::runtime_error("unsupported udp_profile_lidar"); } } else if (point_type == "xyz") { - return make_point_cloud_procssor( + return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); } else if (point_type == "xyzi") { - return make_point_cloud_procssor( + return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); } else if (point_type == "xyzir") { - return make_point_cloud_procssor( + return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); } else if (point_type == "original") { - return make_point_cloud_procssor( + return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, post_processing_fn); }