From b6a9dfd38fb3df93addb3b83074e66af2412bfa3 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 3 Jun 2024 10:19:25 -0700 Subject: [PATCH] Carry over changes to ROS2/foxy --- CHANGELOG.rst | 2 + ouster-ros/config/driver_params.yaml | 3 +- .../include/ouster_ros/sensor_point_types.h | 96 ++++++++++++++++++- .../launch/record.independent.launch.xml | 5 +- ouster-ros/launch/sensor.composite.launch.xml | 5 +- .../launch/sensor.independent.launch.xml | 5 +- ouster-ros/launch/sensor_mtp.launch.xml | 5 +- ouster-ros/package.xml | 2 +- ouster-ros/src/impl/cartesian.h | 64 +++++++++++++ ouster-ros/src/os_cloud_node.cpp | 9 +- ouster-ros/src/os_driver_node.cpp | 9 +- ouster-ros/src/os_ros.cpp | 9 +- ouster-ros/src/point_cloud_processor.h | 4 +- .../src/point_cloud_processor_factory.h | 34 +++++++ 14 files changed, 227 insertions(+), 25 deletions(-) create mode 100644 ouster-ros/src/impl/cartesian.h diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 94dc7152..2c4373d7 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -9,6 +9,8 @@ Changelog * [BUGFIX]: LaserScan does not work when using dual mode * [BUGFIX]: ROS2 crashes when standby mode is set and then set to normal * [BUGFIX]: Implement lock free ring buffer with throttling to reduce 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.12.0 ================== diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index c3882a97..a537ffac 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -31,9 +31,10 @@ ouster/os_driver: ptp_utc_tai_offset: -37.0 # udp_profile_lidar[optional]: lidar packet profile; possible values: # - LEGACY: not recommended - # - RNG19_RFL8_SIG16_NIR16_DUAL # - RNG19_RFL8_SIG16_NIR16 # - RNG15_RFL8_NIR8 + # - RNG19_RFL8_SIG16_NIR16_DUAL + # - FUSA_RNG15_RFL8_NIR8_DUAL udp_profile_lidar: '' # metadata[optional]: path to save metadata file to, if left empty a file # with the sensor hostname or ip will be crearted in ~/.ros folder. diff --git a/ouster-ros/include/ouster_ros/sensor_point_types.h b/ouster-ros/include/ouster_ros/sensor_point_types.h index 7d73e6a1..fa2a9a13 100644 --- a/ouster-ros/include/ouster_ros/sensor_point_types.h +++ b/ouster-ros/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/ouster-ros/launch/record.independent.launch.xml b/ouster-ros/launch/record.independent.launch.xml index a8d07fed..6e6786f4 100644 --- a/ouster-ros/launch/record.independent.launch.xml +++ b/ouster-ros/launch/record.independent.launch.xml @@ -13,9 +13,10 @@ ouster_ros - 0.12.3 + 0.12.4 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/impl/cartesian.h b/ouster-ros/src/impl/cartesian.h new file mode 100644 index 00000000..98b80616 --- /dev/null +++ b/ouster-ros/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 \ No newline at end of file diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 18a0b8ed..91945848 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -128,9 +128,12 @@ class OusterCloud : public OusterProcessingNodeBase { // warn about profile incompatibility if (PointCloudProcessorFactory::point_type_requires_intensity(point_type) && - info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) { - RCLCPP_WARN_STREAM(get_logger(), - "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8"); + !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) { + RCLCPP_WARN_STREAM( + get_logger(), + "selected point type '" << point_type + << "' is not compatible with the udp profile: " + << to_string(info.format.udp_profile_lidar)); } } diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 49971f5c..57d623ac 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -108,9 +108,12 @@ 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) { - RCLCPP_WARN_STREAM(get_logger(), - "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8"); + !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) { + RCLCPP_WARN_STREAM( + get_logger(), + "selected point type '" << point_type + << "' is not compatible with the udp profile: " + << to_string(info.format.udp_profile_lidar)); } } diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index df1d332f..276cd0f0 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -37,10 +37,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/ouster-ros/src/point_cloud_processor.h b/ouster-ros/src/point_cloud_processor.h index 083aae29..558f3bec 100644 --- a/ouster-ros/src/point_cloud_processor.h +++ b/ouster-ros/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/ouster-ros/src/point_cloud_processor_factory.h b/ouster-ros/src/point_cloud_processor_factory.h index fcbb075d..ba59f280 100644 --- a/ouster-ros/src/point_cloud_processor_factory.h +++ b/ouster-ros/src/point_cloud_processor_factory.h @@ -78,6 +78,29 @@ 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"); } @@ -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, @@ -124,6 +153,11 @@ class PointCloudProcessorFactory { 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: // TODO: implement fallback? throw std::runtime_error("unsupported udp_profile_lidar");