From c828a20933afb85435e1dd811f7f84cb87e2ad9c Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Mon, 13 Nov 2023 11:36:49 -0800 Subject: [PATCH] SW-5466: Support Velodyne and other point types in ouster-ros driver (#216) * Quick protoype of Velodyne point type * Add PointXYZIR point type + other major pcl point types * Include point meta functions and point transform by the ouster_ros namesapce * Wrap point meta functions with a namespace and use shorter names for the functions + * Add a seed test module for the point_cloud_compose methods + Add description for the point_cloud_compose methods + refactor code and add compile time checks. * Propagate error state, warn about potential incompatible profile, propagate error state * Add minimal documentation about the new `point_type` parameter. --- CHANGELOG.rst | 4 +- README.md | 66 ++-- ouster-ros/CMakeLists.txt | 16 +- ouster-ros/config/driver_params.yaml | 19 + .../config/os_sensor_cloud_image_params.yaml | 1 + .../include/ouster_ros/common_point_types.h | 72 ++++ ouster-ros/include/ouster_ros/os_point.h | 65 +++- ouster-ros/include/ouster_ros/os_ros.h | 83 +---- .../include/ouster_ros/sensor_point_types.h | 336 ++++++++++++++++++ ouster-ros/package.xml | 2 +- ouster-ros/src/lidar_packet_handler.h | 22 -- ouster-ros/src/os_cloud_node.cpp | 57 +-- ouster-ros/src/os_driver_node.cpp | 45 ++- ouster-ros/src/os_image_node.cpp | 217 ++++------- ouster-ros/src/os_ros.cpp | 231 +----------- ouster-ros/src/os_sensor_node.cpp | 25 +- ouster-ros/src/point_cloud_compose.h | 161 +++++++++ ouster-ros/src/point_cloud_processor.h | 46 ++- .../src/point_cloud_processor_factory.h | 154 ++++++++ ouster-ros/src/point_meta_helpers.h | 151 ++++++++ ouster-ros/src/point_transform.h | 127 +++++++ ouster-ros/test/point_accessor_test.cpp | 218 ++++++++++++ ouster-ros/test/point_cloud_compose_test.cpp | 74 ++++ ouster-ros/test/point_transform_test.cpp | 292 +++++++++++++++ ouster-sensor-msgs/package.xml | 2 +- 25 files changed, 1912 insertions(+), 574 deletions(-) create mode 100644 ouster-ros/include/ouster_ros/common_point_types.h create mode 100644 ouster-ros/include/ouster_ros/sensor_point_types.h create mode 100644 ouster-ros/src/point_cloud_compose.h create mode 100644 ouster-ros/src/point_cloud_processor_factory.h create mode 100644 ouster-ros/src/point_meta_helpers.h create mode 100644 ouster-ros/src/point_transform.h create mode 100644 ouster-ros/test/point_accessor_test.cpp create mode 100644 ouster-ros/test/point_cloud_compose_test.cpp create mode 100644 ouster-ros/test/point_transform_test.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 2f7c9387..0fa74acf 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -11,7 +11,9 @@ Changelog * fix: destagger columns timestamp when generating destaggered point clouds. * shutdown the driver when unable to connect to the sensor on startup * breaking: rename ouster_msgs to ouster_sensor_msgs - +* added the ability to customize the published point clouds(s) to velodyne point cloud format and + other common pcl point types. +* ouster_image_compoenent can operate separately from ouster_cloud_component. ouster_ros v0.10.0 ================== diff --git a/README.md b/README.md index ca202634..5d034769 100644 --- a/README.md +++ b/README.md @@ -12,24 +12,26 @@ | ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) | ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) -- [Overview](#overview) -- [Requirements](#requirements) - - [Linux](#linux) - - [Windows](#windows) - - [Mac](#mac) -- [Getting Started](#getting-started) -- [Usage](#usage) - - [Launching Nodes](#launching-nodes) - - [Sensor Mode](#sensor-mode) - - [Recording Mode](#recording-mode) - - [Replay Mode](#replay-mode) - - [Multicast Mode (experimental)](#multicast-mode-experimental) - - [Invoking Services](#invoking-services) - - [GetMetadata](#getmetadata) - - [GetConfig](#getconfig) - - [SetConfig](#setconfig) - - [Reset](#reset) -- [License](#license) +- [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors) + - [Overview](#overview) + - [Requirements](#requirements) + - [Linux](#linux) + - [Windows](#windows) + - [Mac](#mac) + - [Getting Started](#getting-started) + - [Usage](#usage) + - [Launching Nodes](#launching-nodes) + - [Sensor Mode](#sensor-mode) + - [Recording Mode](#recording-mode) + - [Replay Mode](#replay-mode) + - [Multicast Mode (experimental)](#multicast-mode-experimental) + - [Invoking Services](#invoking-services) + - [GetMetadata](#getmetadata) + - [GetConfig](#getconfig) + - [SetConfig](#setconfig) + - [Reset](#reset) + - [Driver Parameters](#driver-parameters) + - [License](#license) ## Overview @@ -238,8 +240,32 @@ connection, reset the sensor and reconnect again. > **Note** > Changing settings is not yet fully support during a reset operation (more on this) -TBD: For further detailed instructions refer to the [main guide](./docs/index.rst) - +### Driver Parameters +The driver has several parameters that allow you to customize its behavior, all of +these parameters are defined with the `driver_params.yaml` file found under `config` +folder. The only required parameter is `sensor_hostname` which sets the sensor +hostname or ip that you want to connect to through ouster-ros driver. + +Other notable parameters include: +* **point_type**: This parameter allows to customize the point cloud that the + driver produces through its `/ouster/points` topics. Choose one of the following + values: + - `original`: This uses the original point representation `ouster_ros::Point` + of the ouster-ros driver. + - `native`: directly maps all fields as published by the sensor to an + equivalent point cloud representation with the additon of ring + and timestamp fields. + - `xyz`: the simplest point type, only has {x, y, z} + - `xyzi`: same as xyz point type but adds intensity (signal) field. this + type is not compatible with the low data profile. + - `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. + +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. + +For further detailed instructions about the driver refer to the [main guide](./docs/index.rst) ## License [License File](./LICENSE) diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 0be151ef..b56cf112 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -154,7 +154,7 @@ rclcpp_components_register_node(os_image_component EXECUTABLE os_image ) -# ==== os_sensor_component ==== +# ==== os_driver_component ==== create_ros2_component(os_driver_component "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp" "std_srvs" @@ -168,11 +168,23 @@ rclcpp_components_register_node(os_driver_component # ==== Test ==== if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test test/ring_buffer_test.cpp) + ament_add_gtest(${PROJECT_NAME}_test + test/ring_buffer_test.cpp + src/os_ros.cpp + test/point_accessor_test.cpp + test/point_transform_test.cpp + test/point_cloud_compose_test.cpp + ) + ament_target_dependencies(${PROJECT_NAME}_test + rclcpp + ouster_sensor_msgs + ) target_include_directories(${PROJECT_NAME}_test PUBLIC + ${_ouster_ros_INCLUDE_DIRS} $ $ ) + target_link_libraries(${PROJECT_NAME}_test ouster_ros_library) endif() diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 0520ab0d..c3882a97 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -67,3 +67,22 @@ ouster/os_driver: # data QoS. This is preferrable for production but default QoS is needed for # rosbag. See: https://github.com/ros2/rosbag2/issues/125 use_system_default_qos: false + # point_type[optional]: choose from: {original, native, xyz, xyzi, xyzir} + # Here is a breif description of each option: + # - original: This uses the original point representation ouster_ros::Point + # of the ouster-ros driver. + # - native: directly maps all fields as published by the sensor to an + # equivalent point cloud representation with the additon of ring + # and timestamp fields. + # - xyz: the simplest point type, only has {x, y, z} + # - xyzi: same as xyz point type but adds intensity (signal) field. this + # type is not compatible with the low data profile. + # - 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. + # for more details about the fields of each point type and their data refer + # to the following header files: + # - ouster_ros/os_point.h + # - ouster_ros/sensor_point_types.h + # - ouster_ros/common_point_types.h. + point_type: original \ No newline at end of file diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index 13fdcf58..063774f4 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -27,5 +27,6 @@ ouster/os_cloud: use_system_default_qos: False # needs to match the value defined for os_sensor node scan_ring: 0 # Use this parameter in conjunction with the SCAN flag and choose a # value the range [0, sensor_beams_count) + point_type: original # choose from: {original, native, xyz, xyzi, xyzir} ouster/os_image: use_system_default_qos: False # needs to match the value defined for os_sensor node diff --git a/ouster-ros/include/ouster_ros/common_point_types.h b/ouster-ros/include/ouster_ros/common_point_types.h new file mode 100644 index 00000000..2bfea7a8 --- /dev/null +++ b/ouster-ros/include/ouster_ros/common_point_types.h @@ -0,0 +1,72 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file common_point_types.h + * @brief common PCL point datatype for use with ouster sensors + */ + +#pragma once + +#include + +namespace ouster_ros { + +/* The following are pcl point representations that are common/standard point + representation that we readily support. + */ + +/* + * Same as Velodyne point cloud type + * @remark XYZIR point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA + * udp lidar profile. + */ +struct EIGEN_ALIGN16 _PointXYZIR { + PCL_ADD_POINT4D; + float intensity; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct PointXYZIR : public _PointXYZIR { + + inline PointXYZIR(const _PointXYZIR& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + intensity = pt.intensity; ring = pt.ring; + } + + inline PointXYZIR() + { + x = y = z = 0.0f; data[3] = 1.0f; + intensity = 0.0f; ring = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, intensity, ring); + } + + inline auto as_tuple() { + return std::tie(x, y, z, intensity, ring); + } + + 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) + (float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) +) + +// clang-format on diff --git a/ouster-ros/include/ouster_ros/os_point.h b/ouster-ros/include/ouster_ros/os_point.h index 8b745ac6..e319220d 100644 --- a/ouster-ros/include/ouster_ros/os_point.h +++ b/ouster-ros/include/ouster_ros/os_point.h @@ -10,36 +10,79 @@ #include -#include -#include - -#include - namespace ouster_ros { -struct EIGEN_ALIGN16 Point { +// The default/original represntation of the point cloud since the driver +// inception. This shouldn't be confused with Point_LEGACY which provides exact +// mapping of the fields of Ouster LidarScan of the Legacy Profile, copying the +// the same order and using the same bit representation. For example, this Point +// struct uses float data type to represent intensity (aka signal); however, the +// sensor sends the signal channel either as UINT16 or UINT32 depending on the +// active udp lidar profile. +struct EIGEN_ALIGN16 _Point { PCL_ADD_POINT4D; - float intensity; + float intensity; // equivalent to signal uint32_t t; uint16_t reflectivity; - uint16_t ring; - uint16_t ambient; + uint16_t ring; // equivalent to channel + uint16_t ambient; // equivalent to near_ir uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace ouster_ros + +struct Point : public _Point { + + inline Point(const _Point& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + intensity = pt.intensity; + t = pt.t; + reflectivity = pt.reflectivity; + ring = pt.ring; + ambient = pt.ambient; + range = pt.range; + } + + inline Point() + { + x = y = z = 0.0f; data[3] = 1.0f; + intensity = 0.0f; + t = 0; + reflectivity = 0; + ring = 0; + ambient = 0; + range = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range); + } + + inline auto as_tuple() { + return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range); + } + + template + inline auto& get() { + return std::get(as_tuple()); + } +}; + +} // namespace ouster_ros // clang-format off + +// DEFAULT/ORIGINAL POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) - // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) (std::uint16_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) + // clang-format on diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 2b11365b..c2589104 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -31,8 +30,6 @@ namespace ouster_ros { namespace sensor = ouster::sensor; -using Cloud = pcl::PointCloud; -using ns = std::chrono::nanoseconds; /** * Checks sensor_info if it currently represents a legacy udp lidar profile @@ -90,74 +87,6 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_sensor_msgs::msg::PacketMsg const std::string& frame, const sensor::packet_format& pf); -/** - * Populate a PCL point cloud from a LidarScan. - * @param[in, out] points The points parameters is used to store the results of - * the cartesian product before it gets packed into the cloud object. - * @param[in] lut_direction the direction of the xyz lut (with single precision) - * @param[in] lut_offset the offset of the xyz lut (with single precision) - * @param[in] scan_ts scan start used to caluclate relative timestamps for - * points. - * @param[in] lidar_scan input lidar data - * @param[out] cloud output pcl pointcloud to populate - * @param[in] return_index index of return desired starting at 0 - */ -[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f( - ouster::PointsF& points, const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, - int return_index); - -/** - * Populate a PCL point cloud from a LidarScan. - * @param[in, out] points The points parameters is used to store the results of - * the cartesian product before it gets packed into the cloud object. - * @param[in] lut_direction the direction of the xyz lut (with single precision) - * @param[in] lut_offset the offset of the xyz lut (with single precision) - * @param[in] scan_ts scan start used to caluclate relative timestamps for - * points - * @param[in] lidar_scan input lidar data - * @param[out] cloud output pcl pointcloud to populate - * @param[in] return_index index of return desired starting at 0 - */ -void scan_to_cloud_f(ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& lidar_scan, - ouster_ros::Cloud& cloud, int return_index); - -/** - * Populate a destaggered PCL point cloud from a LidarScan - * @param[out] cloud output pcl pointcloud to populate - * @param[in, out] points The points parameters is used to store the results of - * the cartesian product before it gets packed into the cloud object. - * @param[in] lut_direction the direction of the xyz lut (with single precision) - * @param[in] lut_offset the offset of the xyz lut (with single precision) - * @param[in] scan_ts scan start used to caluclate relative timestamps for - * points - * @param[in] lidar_scan input lidar data - * @param[in] pixel_shift_by_row pixel shifts by row - * @param[in] return_index index of return desired starting at 0 - */ -void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, - ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& ls, - const std::vector& pixel_shift_by_row, - int return_index); - -/** - * Serialize a PCL point cloud to a ROS message - * @param[in] cloud the PCL point cloud to convert - * @param[in] timestamp the timestamp to apply to the resulting ROS message - * @param[in] frame the frame to set in the resulting ROS message - * @return a ROS message containing the point cloud - */ -sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, - const rclcpp::Time& timestamp, - const std::string& frame); - /** * Convert transformation matrix return by sensor to ROS transform * @param[in] mat transformation matrix return by sensor @@ -203,10 +132,7 @@ struct read_and_cast { template inline ouster::img_t get_or_fill_zero(sensor::ChanField field, const ouster::LidarScan& ls) { - if (!ls.field_type(field)) { - return ouster::img_t::Zero(ls.h, ls.w); - } - + if (!ls.field_type(field)) return ouster::img_t::Zero(ls.h, ls.w); ouster::img_t result{ls.h, ls.w}; ouster::impl::visit_field(ls, field, read_and_cast(), result); return result; @@ -221,6 +147,13 @@ inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) { return offset < 0 && ts < static_cast(std::abs(offset)) ? 0 : ts + offset; } +std::set parse_tokens(const std::string& input, char delim); + +inline bool check_token(const std::set& tokens, + const std::string& token) { + return tokens.find(token) != tokens.end(); +} + } // namespace impl diff --git a/ouster-ros/include/ouster_ros/sensor_point_types.h b/ouster-ros/include/ouster_ros/sensor_point_types.h new file mode 100644 index 00000000..7d73e6a1 --- /dev/null +++ b/ouster-ros/include/ouster_ros/sensor_point_types.h @@ -0,0 +1,336 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file sensor_point_types.h + * @brief native sensor point types + * The following are one-to-one mapping of pcl point representatios that could + * fit the data sent by the sensor (with the addition of t and ring fields), + * All of these representation follow the same order of LaserScan fields: + * 1. range + * 2. signal + * 3. refelctivity + * 4. near_ir + * With the exception Point_RNG15_RFL8_NIR8 aka LOW_DATA which does not include + * the signal field +**/ + +#pragma once + +#include +#include + +namespace ouster_ros { + +template +using Table = std::array, N>; + +namespace sensor = ouster::sensor; + +template +using ChanFieldTable = Table; + +} + + +namespace ouster_ros { + +// Profile_LEGACY +static constexpr ChanFieldTable<4> Profile_LEGACY{{ + {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT32}} +}; + +// auto=LEGACY +struct EIGEN_ALIGN16 _Point_LEGACY { + PCL_ADD_POINT4D; + uint32_t t; // timestamp relative to frame + uint16_t ring; // equivalent to channel + uint32_t range; + uint32_t signal; // equivalent to intensity + uint32_t reflectivity; + uint32_t near_ir; // equivalent to ambient + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct Point_LEGACY : public _Point_LEGACY { + + inline Point_LEGACY(const _Point_LEGACY& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + t = pt.t; ring = pt.ring; + range = pt.range; signal = pt.signal; + reflectivity = pt.reflectivity; near_ir = pt.near_ir; + } + + inline Point_LEGACY() + { + x = y = z = 0.0f; data[3] = 1.0f; + t = 0; ring = 0; + range = 0; signal = 0; + reflectivity = 0; near_ir = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); + } + + inline auto as_tuple() { + return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); + } + + template + inline auto& get() { + return std::get(as_tuple()); + } +}; + +} // namespace ouster_ros + +// clang-format off + +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_LEGACY, + (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::uint32_t, signal, signal) + (std::uint32_t, reflectivity, reflectivity) + (std::uint32_t, near_ir, near_ir) +) + +namespace ouster_ros { + +// Profile_RNG19_RFL8_SIG16_NIR16_DUAL: aka dual returns +// This profile is definied differently from RNG19_RFL8_SIG16_NIR16_DUAL of how +// the sensor actually sends the data. The actual RNG19_RFL8_SIG16_NIR16_DUAL +// has 7 fields not 4, 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<4> Profile_RNG19_RFL8_SIG16_NIR16_DUAL {{ + {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT16}, + {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_RNG19_RFL8_SIG16_NIR16_DUAL with the +// exception of ChanField values for the first three fields. NEAR_IR is same for both +static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN {{ + {sensor::ChanField::RANGE2, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL2, sensor::ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY2, sensor::ChanFieldType::UINT8}, + {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, +}}; + +// 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 + uint16_t ring; // equivalent channel + uint32_t range; + uint16_t signal; // equivalent to intensity + uint8_t reflectivity; + uint16_t near_ir; // equivalent to ambient + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct Point_RNG19_RFL8_SIG16_NIR16_DUAL : public _Point_RNG19_RFL8_SIG16_NIR16_DUAL { + + inline Point_RNG19_RFL8_SIG16_NIR16_DUAL(const _Point_RNG19_RFL8_SIG16_NIR16_DUAL& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + t = pt.t; ring = pt.ring; + range = pt.range; signal = pt.signal; + reflectivity = pt.reflectivity; near_ir = pt.near_ir; + } + + inline Point_RNG19_RFL8_SIG16_NIR16_DUAL() + { + x = y = z = 0.0f; data[3] = 1.0f; + t = 0; ring = 0; + range = 0; signal = 0; + reflectivity = 0; near_ir = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); + } + + inline auto as_tuple() { + return std::tie(x, y, z, t, ring, range, signal, 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_RNG19_RFL8_SIG16_NIR16_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::uint16_t, signal, signal) + (std::uint8_t, reflectivity, reflectivity) + (std::uint16_t, near_ir, near_ir) +) + +// clang-format on + +namespace ouster_ros { + +// Profile_RNG19_RFL8_SIG16_NIR16 aka single return +static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16{{ + {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT16}, + {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, +}}; + +// 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 + uint16_t ring; // equivalent channel + uint32_t range; + uint16_t signal; // equivalent to intensity + uint16_t reflectivity; + uint16_t near_ir; // equivalent to ambient + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct Point_RNG19_RFL8_SIG16_NIR16 : public _Point_RNG19_RFL8_SIG16_NIR16 { + + inline Point_RNG19_RFL8_SIG16_NIR16(const _Point_RNG19_RFL8_SIG16_NIR16& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + t = pt.t; ring = pt.ring; + range = pt.range; signal = pt.signal; + reflectivity = pt.reflectivity; near_ir = pt.near_ir; + } + + inline Point_RNG19_RFL8_SIG16_NIR16() + { + x = y = z = 0.0f; data[3] = 1.0f; + t = 0; ring = 0; + range = 0; signal = 0; + reflectivity = 0; near_ir = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); + } + + inline auto as_tuple() { + return std::tie(x, y, z, t, ring, range, signal, reflectivity, near_ir); + } + + template + inline auto& get() { + return std::get(as_tuple()); + } +}; + +} // namespace ouster_ros + +// clang-format off + +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG19_RFL8_SIG16_NIR16, + (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::uint16_t, signal, signal) + (std::uint16_t, reflectivity, reflectivity) + (std::uint16_t, near_ir, near_ir) +) + +// clang-format on + +namespace ouster_ros { + +// Profile_RNG15_RFL8_NIR8 aka LOW_DATA +static constexpr ChanFieldTable<3> Profile_RNG15_RFL8_NIR8{{ + {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT16}, + {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16}, +}}; + +// auto=RNG15_RFL8_NIR8 aka LOW_DATA profile +struct EIGEN_ALIGN16 _Point_RNG15_RFL8_NIR8 { + PCL_ADD_POINT4D; + // No signal/intensity in low data mode + uint32_t t; // timestamp relative to frame + uint16_t ring; // equivalent to channel + uint32_t range; + uint16_t reflectivity; + uint16_t near_ir; // equivalent to ambient + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +struct Point_RNG15_RFL8_NIR8 : public _Point_RNG15_RFL8_NIR8 { + + inline Point_RNG15_RFL8_NIR8(const _Point_RNG15_RFL8_NIR8& 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_RNG15_RFL8_NIR8() + { + 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()); + } +}; + +} // namespace ouster_ros + +// clang-format off + +// Default=RNG15_RFL8_NIR8 aka LOW_DATA profile +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG15_RFL8_NIR8, + (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::uint16_t, reflectivity, reflectivity) + (std::uint16_t, near_ir, near_ir) +) + +// clang-format on diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index dd2ba60b..033faa2d 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.11.0 + 0.11.1 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 6d7cf802..df94b4ef 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -51,28 +51,6 @@ uint64_t ulround(T value) { return static_cast(rounded_value); } -// TODO: move to a separate file -std::set parse_tokens(const std::string& input, char delim) { - std::set tokens; - std::stringstream ss(input); - std::string token; - - while (getline(ss, token, delim)) { - // Remove leading and trailing whitespaces from the token - size_t start = token.find_first_not_of(" "); - size_t end = token.find_last_not_of(" "); - token = token.substr(start, end - start + 1); - if (!token.empty()) tokens.insert(token); - } - - return tokens; -} - -inline bool check_token(const std::set& tokens, - const std::string& token) { - return tokens.find(token) != tokens.end(); -} - } // namespace namespace ouster_ros { diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 616e3629..60d75699 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -6,7 +6,7 @@ * @brief A node to publish point clouds and imu topics */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -15,11 +15,6 @@ #include #include -#include -#include -#include -#include - #include "ouster_sensor_msgs/msg/packet_msg.hpp" #include "ouster_ros/os_processing_node_base.h" #include "ouster_ros/visibility_control.h" @@ -29,12 +24,13 @@ #include "lidar_packet_handler.h" #include "point_cloud_processor.h" #include "laser_scan_processor.h" +#include "point_cloud_processor_factory.h" + +namespace ouster_ros { namespace sensor = ouster::sensor; using ouster_sensor_msgs::msg::PacketMsg; -namespace ouster_ros { - class OusterCloud : public OusterProcessingNodeBase { public: OUSTER_ROS_PUBLIC @@ -58,11 +54,12 @@ class OusterCloud : public OusterProcessingNodeBase { void declare_parameters() { tf_bcast.declare_parameters(); - declare_parameter("timestamp_mode", ""); - declare_parameter("ptp_utc_tai_offset", -37.0); - declare_parameter("proc_mask", "IMU|PCL|SCAN"); - declare_parameter("use_system_default_qos", false); - declare_parameter("scan_ring", 0); + declare_parameter("timestamp_mode", ""); + declare_parameter("ptp_utc_tai_offset", -37.0); + declare_parameter("proc_mask", "IMU|PCL|SCAN"); + declare_parameter("use_system_default_qos", false); + declare_parameter("scan_ring", 0); + declare_parameter("point_type", "original"); } void metadata_handler( @@ -87,9 +84,9 @@ class OusterCloud : public OusterProcessingNodeBase { use_system_default_qos ? system_default_qos : sensor_data_qos; auto proc_mask = get_parameter("proc_mask").as_string(); - auto tokens = parse_tokens(proc_mask, '|'); + auto tokens = impl::parse_tokens(proc_mask, '|'); - if (check_token(tokens, "IMU")) { + if (impl::check_token(tokens, "IMU")) { imu_pub = create_publisher("imu", selected_qos); imu_packet_handler = ImuPacketHandler::create_handler( @@ -106,22 +103,32 @@ class OusterCloud : public OusterProcessingNodeBase { int num_returns = get_n_returns(info); std::vector processors; - if (check_token(tokens, "PCL")) { + if (impl::check_token(tokens, "PCL")) { lidar_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { lidar_pubs[i] = create_publisher( topic_for_return("points", i), selected_qos); } - processors.push_back(PointCloudProcessor::create( - info, tf_bcast.point_cloud_frame_id(), - tf_bcast.apply_lidar_to_sensor_transform(), - [this](PointCloudProcessor::OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) - lidar_pubs[i]->publish(*msgs[i]); - })); + + auto point_type = get_parameter("point_type").as_string(); + processors.push_back( + PointCloudProcessorFactory::create_point_cloud_processor(point_type, info, + tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), + [this](PointCloudProcessor_OutputType msgs) { + for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i]->publish(*msgs[i]); + } + ) + ); + + // 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"); + } } - if (check_token(tokens, "SCAN")) { + if (impl::check_token(tokens, "SCAN")) { scan_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { scan_pubs[i] = create_publisher( @@ -146,7 +153,7 @@ class OusterCloud : public OusterProcessingNodeBase { })); } - if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) { + if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN")) { lidar_packet_handler = LidarPacketHandler::create_handler( info, processors, timestamp_mode, static_cast(ptp_utc_tai_offset * 1e+9)); diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 839d33e6..c3060d2c 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -21,6 +21,7 @@ #include "point_cloud_processor.h" #include "laser_scan_processor.h" #include "image_processor.h" +#include "point_cloud_processor_factory.h" namespace ouster_ros { @@ -33,9 +34,10 @@ class OusterDriver : public OusterSensor { : OusterSensor("os_driver", options), tf_bcast(this) { tf_bcast.declare_parameters(); tf_bcast.parse_parameters(); - declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN"); - declare_parameter("scan_ring", 0); - declare_parameter("ptp_utc_tai_offset", -37.0); + declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN"); + declare_parameter("scan_ring", 0); + declare_parameter("ptp_utc_tai_offset", -37.0); + declare_parameter("point_type", "original"); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { @@ -45,7 +47,7 @@ class OusterDriver : public OusterSensor { virtual void create_publishers() override { auto proc_mask = get_parameter("proc_mask").as_string(); - auto tokens = parse_tokens(proc_mask, '|'); + auto tokens = impl::parse_tokens(proc_mask, '|'); bool use_system_default_qos = get_parameter("use_system_default_qos").as_bool(); @@ -58,7 +60,7 @@ class OusterDriver : public OusterSensor { auto ptp_utc_tai_offset = get_parameter("ptp_utc_tai_offset").as_double(); - if (check_token(tokens, "IMU")) { + if (impl::check_token(tokens, "IMU")) { imu_pub = create_publisher("imu", selected_qos); imu_packet_handler = ImuPacketHandler::create_handler( @@ -69,23 +71,32 @@ class OusterDriver : public OusterSensor { int num_returns = get_n_returns(info); std::vector processors; - if (check_token(tokens, "PCL")) { + if (impl::check_token(tokens, "PCL")) { lidar_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { lidar_pubs[i] = create_publisher( topic_for_return("points", i), selected_qos); } - processors.push_back(PointCloudProcessor::create( - info, tf_bcast.point_cloud_frame_id(), - tf_bcast.apply_lidar_to_sensor_transform(), - [this](PointCloudProcessor::OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) - lidar_pubs[i]->publish(*msgs[i]); - })); + auto point_type = get_parameter("point_type").as_string(); + processors.push_back( + PointCloudProcessorFactory::create_point_cloud_processor(point_type, info, + tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), + [this](PointCloudProcessor_OutputType msgs) { + for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i]->publish(*msgs[i]); + } + ) + ); + + // 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"); + } } - if (check_token(tokens, "SCAN")) { + if (impl::check_token(tokens, "SCAN")) { scan_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { scan_pubs[i] = create_publisher( @@ -115,7 +126,7 @@ class OusterDriver : public OusterSensor { })); } - if (check_token(tokens, "IMG")) { + if (impl::check_token(tokens, "IMG")) { const std::map channel_field_topic_map_1{ {sensor::ChanField::RANGE, "range_image"}, @@ -150,8 +161,8 @@ class OusterDriver : public OusterSensor { })); } - if (check_token(tokens, "PCL") || check_token(tokens, "SCAN") || - check_token(tokens, "IMG")) + if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN") || + impl::check_token(tokens, "IMG")) lidar_packet_handler = LidarPacketHandler::create_handler( info, processors, timestamp_mode, static_cast(ptp_utc_tai_offset * 1e+9)); diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index fdecc4dd..f820a489 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -11,36 +11,24 @@ * vision applications, use higher bit depth values in /os_cloud_node/points */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" // clang-format on -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "ouster/image_processing.h" #include "ouster_ros/visibility_control.h" #include "ouster_ros/os_processing_node_base.h" -namespace sensor = ouster::sensor; -namespace viz = ouster::viz; - -using pixel_type = uint16_t; -const size_t pixel_value_max = std::numeric_limits::max(); +#include "lidar_packet_handler.h" +#include "image_processor.h" namespace ouster_ros { +namespace sensor = ouster::sensor; +using ouster_sensor_msgs::msg::PacketMsg; + + class OusterImage : public OusterProcessingNodeBase { public: OUSTER_ROS_PUBLIC @@ -51,6 +39,8 @@ class OusterImage : public OusterProcessingNodeBase { private: void on_init() { + declare_parameter("timestamp_mode", ""); + declare_parameter("ptp_utc_tai_offset", -37.0); declare_parameter("use_system_default_qos", false); create_metadata_subscriber( [this](const auto& msg) { metadata_handler(msg); }); @@ -61,19 +51,17 @@ class OusterImage : public OusterProcessingNodeBase { RCLCPP_INFO(get_logger(), "OusterImage: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); - create_cloud_object(); - const int n_returns = get_n_returns(info); - create_publishers(n_returns); - create_subscriptions(n_returns); + create_publishers_subscribers(get_n_returns(info)); } - void create_cloud_object() { - uint32_t H = info.format.pixels_per_column; - uint32_t W = info.format.columns_per_frame; - cloud = ouster_ros::Cloud{W, H}; - } + void create_publishers_subscribers(int n_returns) { - void create_publishers(int n_returns) { + // TODO: avoid having to replicate the parameters: + // timestamp_mode, ptp_utc_tai_offset, use_system_default_qos in yet + // another node. + auto timestamp_mode = get_parameter("timestamp_mode").as_string(); + auto ptp_utc_tai_offset = + get_parameter("ptp_utc_tai_offset").as_double(); bool use_system_default_qos = get_parameter("use_system_default_qos").as_bool(); rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); @@ -81,137 +69,58 @@ class OusterImage : public OusterProcessingNodeBase { auto selected_qos = use_system_default_qos ? system_default_qos : sensor_data_qos; - nearir_image_pub = create_publisher( - "nearir_image", selected_qos); - - rclcpp::Publisher::SharedPtr a_pub; - for (int i = 0; i < n_returns; i++) { - a_pub = create_publisher( - topic_for_return("range_image", i), selected_qos); - range_image_pubs.push_back(a_pub); - - a_pub = create_publisher( - topic_for_return("signal_image", i), selected_qos); - signal_image_pubs.push_back(a_pub); - - a_pub = create_publisher( - topic_for_return("reflec_image", i), selected_qos); - reflec_image_pubs.push_back(a_pub); + const std::map + channel_field_topic_map_1 { + {sensor::ChanField::RANGE, "range_image"}, + {sensor::ChanField::SIGNAL, "signal_image"}, + {sensor::ChanField::REFLECTIVITY, "reflec_image"}, + {sensor::ChanField::NEAR_IR, "nearir_image"}}; + + const std::map + channel_field_topic_map_2 { + {sensor::ChanField::RANGE, "range_image"}, + {sensor::ChanField::SIGNAL, "signal_image"}, + {sensor::ChanField::REFLECTIVITY, "reflec_image"}, + {sensor::ChanField::NEAR_IR, "nearir_image"}, + {sensor::ChanField::RANGE2, "range_image2"}, + {sensor::ChanField::SIGNAL2, "signal_image2"}, + {sensor::ChanField::REFLECTIVITY2, "reflec_image2"}}; + + auto which_map = n_returns == 1 ? &channel_field_topic_map_1 + : &channel_field_topic_map_2; + for (auto it = which_map->begin(); it != which_map->end(); ++it) { + image_pubs[it->first] = + create_publisher(it->second, + selected_qos); } - } - void create_subscriptions(int n_returns) { - pc_subs.resize(n_returns); - rclcpp::SensorDataQoS qos; - for (int i = 0; i < n_returns; ++i) { - pc_subs[i] = create_subscription( - topic_for_return("points", i), qos, - [this, i](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - point_cloud_handler(msg, i); + std::vector processors { + ImageProcessor::create( + info, "os_lidar", /*TODO: tf_bcast.point_cloud_frame_id()*/ + [this](ImageProcessor::OutputType msgs) { + for (auto it = msgs.begin(); it != msgs.end(); ++it) { + image_pubs[it->first]->publish(*it->second); + } + }) + }; + + lidar_packet_handler = LidarPacketHandler::create_handler( + info, processors, timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); + lidar_packet_sub = create_subscription( + "lidar_packets", selected_qos, + [this](const PacketMsg::ConstSharedPtr msg) { + lidar_packet_handler(msg->buf.data()); }); - } - } - - void point_cloud_handler(const sensor_msgs::msg::PointCloud2::SharedPtr m, - int return_index) { - pcl::fromROSMsg(*m, cloud); - - const bool first = (return_index == 0); - uint32_t H = info.format.pixels_per_column; - uint32_t W = info.format.columns_per_frame; - - auto range_image = - make_image_msg(H, W, m->header.stamp, m->header.frame_id); - auto signal_image = - make_image_msg(H, W, m->header.stamp, m->header.frame_id); - auto reflec_image = - make_image_msg(H, W, m->header.stamp, m->header.frame_id); - auto nearir_image = - make_image_msg(H, W, m->header.stamp, m->header.frame_id); - - ouster::img_t nearir_image_eigen(H, W); - ouster::img_t signal_image_eigen(H, W); - ouster::img_t reflec_image_eigen(H, W); - - // views into message data - auto range_image_map = Eigen::Map>( - (pixel_type*)range_image->data.data(), H, W); - auto signal_image_map = Eigen::Map>( - (pixel_type*)signal_image->data.data(), H, W); - auto reflec_image_map = Eigen::Map>( - (pixel_type*)reflec_image->data.data(), H, W); - - auto nearir_image_map = Eigen::Map>( - (pixel_type*)nearir_image->data.data(), H, W); - - // copy data out of Cloud message, with destaggering - for (size_t u = 0; u < H; u++) { - for (size_t v = 0; v < W; v++) { - const auto& pt = cloud[u * W + v]; - - // 16 bit img: use 4mm resolution and throw out returns > - // 260m - auto r = (pt.range + 0b10) >> 2; - range_image_map(u, v) = r > pixel_value_max ? 0 : r; - - signal_image_eigen(u, v) = pt.intensity; - reflec_image_eigen(u, v) = pt.reflectivity; - nearir_image_eigen(u, v) = pt.ambient; - } - } - - signal_ae(signal_image_eigen, first); - reflec_ae(reflec_image_eigen, first); - nearir_buc(nearir_image_eigen); - nearir_ae(nearir_image_eigen, first); - nearir_image_eigen = nearir_image_eigen.sqrt(); - signal_image_eigen = signal_image_eigen.sqrt(); - - // copy data into image messages - signal_image_map = - (signal_image_eigen * pixel_value_max).cast(); - reflec_image_map = - (reflec_image_eigen * pixel_value_max).cast(); - if (first) { - nearir_image_map = - (nearir_image_eigen * pixel_value_max).cast(); - nearir_image_pub->publish(std::move(nearir_image)); - } - - // publish at return index - range_image_pubs[return_index]->publish(std::move(range_image)); - signal_image_pubs[return_index]->publish(std::move(signal_image)); - reflec_image_pubs[return_index]->publish(std::move(reflec_image)); - } - - static sensor_msgs::msg::Image::UniquePtr make_image_msg( - size_t H, size_t W, const rclcpp::Time& stamp, - const std::string& frame) { - auto msg = std::make_unique(); - msg->width = W; - msg->height = H; - msg->step = W * sizeof(pixel_type); - msg->encoding = sensor_msgs::image_encodings::MONO16; - msg->data.resize(W * H * sizeof(pixel_type)); - msg->header.stamp = stamp; - msg->header.frame_id = frame; - return msg; } private: - rclcpp::Publisher::SharedPtr nearir_image_pub; - std::vector::SharedPtr> - range_image_pubs; - std::vector::SharedPtr> - signal_image_pubs; - std::vector::SharedPtr> - reflec_image_pubs; - std::vector::SharedPtr> - pc_subs; - - ouster_ros::Cloud cloud; - viz::AutoExposure nearir_ae, signal_ae, reflec_ae; - viz::BeamUniformityCorrector nearir_buc; + rclcpp::Subscription::SharedPtr lidar_packet_sub; + std::map::SharedPtr> + image_pubs; + + LidarPacketHandler::HandlerType lidar_packet_handler; }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index 0ddbf8e9..e6b63c4c 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -6,13 +6,12 @@ * @brief A utilty file that contains helper methods */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" // clang-format on -#include #include #include @@ -112,228 +111,26 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { throw std::runtime_error("Unreachable"); } } -} // namespace impl - -template -void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, - std::chrono::nanoseconds scan_ts, const PointT& points, - const ouster::img_t& range, - const ouster::img_t& reflectivity, - const ouster::img_t& near_ir, - const ouster::img_t& signal) { - auto timestamp = ls.timestamp(); - const auto rg = range.data(); - const auto rf = reflectivity.data(); - const auto nr = near_ir.data(); - const auto sg = signal.data(); - const auto t_zero = std::chrono::duration::zero(); - -#ifdef __OUSTER_UTILIZE_OPENMP__ -#pragma omp parallel for collapse(2) -#endif - for (auto u = 0; u < ls.h; u++) { - for (auto v = 0; v < ls.w; v++) { - const auto col_ts = std::chrono::nanoseconds(timestamp[v]); - const auto ts = col_ts > scan_ts ? col_ts - scan_ts : t_zero; - const auto idx = u * ls.w + v; - const auto xyz = points.row(idx); - cloud.points[idx] = ouster_ros::Point{ - {static_cast(xyz(0)), static_cast(xyz(1)), - static_cast(xyz(2)), 1.0f}, - static_cast(sg[idx]), - static_cast(ts.count()), - static_cast(rf[idx]), - static_cast(u), - static_cast(nr[idx]), - static_cast(rg[idx]), - }; - } - } -} -template -void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, - uint64_t scan_ts, const PointT& points, - const ouster::img_t& range, - const ouster::img_t& reflectivity, - const ouster::img_t& near_ir, - const ouster::img_t& signal) { - auto timestamp = ls.timestamp(); +// TODO: move to a separate file +std::set parse_tokens(const std::string& input, char delim) { - const auto rg = range.data(); - const auto rf = reflectivity.data(); - const auto nr = near_ir.data(); - const auto sg = signal.data(); + std::set tokens; + std::stringstream ss(input); + std::string token; -#ifdef __OUSTER_UTILIZE_OPENMP__ -#pragma omp parallel for collapse(2) -#endif - for (auto u = 0; u < ls.h; u++) { - for (auto v = 0; v < ls.w; v++) { - const auto col_ts = timestamp[v]; - const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL; - const auto idx = u * ls.w + v; - const auto xyz = points.row(idx); - cloud.points[idx] = ouster_ros::Point{ - {static_cast(xyz(0)), static_cast(xyz(1)), - static_cast(xyz(2)), 1.0f}, - static_cast(sg[idx]), - static_cast(ts), - static_cast(rf[idx]), - static_cast(u), - static_cast(nr[idx]), - static_cast(rg[idx]), - }; - } + while (getline(ss, token, delim)) { + // Remove leading and trailing whitespaces from the token + size_t start = token.find_first_not_of(" "); + size_t end = token.find_last_not_of(" "); + token = token.substr(start, end - start + 1); + if (!token.empty()) tokens.insert(token); } -} - -template -void copy_scan_to_cloud_destaggered( - ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, uint64_t scan_ts, - const PointT& points, const ouster::img_t& range, - const ouster::img_t& reflectivity, - const ouster::img_t& near_ir, const ouster::img_t& signal, - const std::vector& pixel_shift_by_row) { - auto timestamp = ls.timestamp(); - const auto rg = range.data(); - const auto rf = reflectivity.data(); - const auto nr = near_ir.data(); - const auto sg = signal.data(); -#ifdef __OUSTER_UTILIZE_OPENMP__ -#pragma omp parallel for collapse(2) -#endif - for (auto u = 0; u < ls.h; u++) { - for (auto v = 0; v < ls.w; v++) { - const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w; - auto ts = timestamp[v_shift]; ts = ts > scan_ts ? ts - scan_ts : 0UL; - const auto src_idx = u * ls.w + v_shift; - const auto tgt_idx = u * ls.w + v; - const auto xyz = points.row(src_idx); - cloud.points[tgt_idx] = ouster_ros::Point{ - {static_cast(xyz(0)), static_cast(xyz(1)), - static_cast(xyz(2)), 1.0f}, - static_cast(sg[src_idx]), - static_cast(ts), - static_cast(rf[src_idx]), - static_cast(u), - static_cast(nr[src_idx]), - static_cast(rg[src_idx]), - }; - } - } + return tokens; } -void scan_to_cloud_f(ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, - std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index) { - bool second = (return_index == 1); - - assert(cloud.width == static_cast(ls.w) && - cloud.height == static_cast(ls.h) && - "point cloud and lidar scan size mismatch"); - - // across supported lidar profiles range is always 32-bit - auto range_channel_field = - second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; - ouster::img_t range = ls.field(range_channel_field); - - ouster::img_t reflectivity = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - ouster::img_t signal = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t near_ir = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::cartesianT(points, range, lut_direction, lut_offset); - - copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir, - signal); -} - -void scan_to_cloud_f(ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index) { - bool second = (return_index == 1); - - assert(cloud.width == static_cast(ls.w) && - cloud.height == static_cast(ls.h) && - "point cloud and lidar scan size mismatch"); - - // across supported lidar profiles range is always 32-bit - auto range_channel_field = - second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; - ouster::img_t range = ls.field(range_channel_field); - - ouster::img_t reflectivity = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - ouster::img_t signal = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t near_ir = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::cartesianT(points, range, lut_direction, lut_offset); - - copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir, - signal); -} - -void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, - ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, - uint64_t scan_ts, const ouster::LidarScan& ls, - const std::vector& pixel_shift_by_row, - int return_index) { - bool second = (return_index == 1); - - assert(cloud.width == static_cast(ls.w) && - cloud.height == static_cast(ls.h) && - "point cloud and lidar scan size mismatch"); - - // across supported lidar profiles range is always 32-bit - auto range_channel_field = - second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; - ouster::img_t range = ls.field(range_channel_field); - - ouster::img_t reflectivity = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - ouster::img_t signal = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t near_ir = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::cartesianT(points, range, lut_direction, lut_offset); - - copy_scan_to_cloud_destaggered(cloud, ls, scan_ts, points, range, - reflectivity, near_ir, signal, - pixel_shift_by_row); -} - -sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, - const rclcpp::Time& timestamp, - const std::string& frame) { - sensor_msgs::msg::PointCloud2 msg{}; - pcl::toROSMsg(cloud, msg); - msg.header.frame_id = frame; - msg.header.stamp = timestamp; - return msg; -} +} // namespace impl geometry_msgs::msg::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 110cb7e6..810a4d82 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -37,19 +37,18 @@ OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) : OusterSensor("os_sensor", options) {} void OusterSensor::declare_parameters() { - declare_parameter("sensor_hostname", ""); - declare_parameter("lidar_ip", ""); // community driver param - declare_parameter("metadata", ""); - declare_parameter("udp_dest", ""); - declare_parameter("computer_ip", - ""); // community driver param - declare_parameter("mtp_dest", ""); - declare_parameter("mtp_main", false); - declare_parameter("lidar_port", 0); - declare_parameter("imu_port", 0); - declare_parameter("lidar_mode", ""); - declare_parameter("timestamp_mode", ""); - declare_parameter("udp_profile_lidar", ""); + declare_parameter("sensor_hostname", ""); + declare_parameter("lidar_ip", ""); // community driver param + declare_parameter("metadata", ""); + declare_parameter("udp_dest", ""); + declare_parameter("computer_ip", ""); // community driver param + declare_parameter("mtp_dest", ""); + declare_parameter("mtp_main", false); + declare_parameter("lidar_port", 0); + declare_parameter("imu_port", 0); + declare_parameter("lidar_mode", ""); + declare_parameter("timestamp_mode", ""); + declare_parameter("udp_profile_lidar", ""); declare_parameter("use_system_default_qos", false); } diff --git a/ouster-ros/src/point_cloud_compose.h b/ouster-ros/src/point_cloud_compose.h new file mode 100644 index 00000000..9183cf11 --- /dev/null +++ b/ouster-ros/src/point_cloud_compose.h @@ -0,0 +1,161 @@ +#pragma once + +#include +#include + +#include "ouster_ros/os_point.h" +#include "ouster_ros/sensor_point_types.h" +#include "ouster_ros/common_point_types.h" + +#include "point_meta_helpers.h" +#include "point_transform.h" + +namespace ouster_ros { + +using ouster::sensor::ChanFieldType; + +template +struct TypeSelector { /*undefined*/ +}; + +template <> +struct TypeSelector { + typedef uint8_t type; +}; + +template <> +struct TypeSelector { + typedef uint16_t type; +}; + +template <> +struct TypeSelector { + typedef uint32_t type; +}; + +template <> +struct TypeSelector { + typedef uint64_t type; +}; + +/** + * @brief constructs a suitable tuple at compile time that can store a reference + * to all the fields of a specific LidarScan object (without conversion) + * according to the information specificed by the ChanFieldTable. + */ +template & Table> +constexpr auto make_lidar_scan_tuple() { + if constexpr (Index < N) { + using ElementType = typename TypeSelector::type; + return std::tuple_cat( + std::make_tuple(static_cast(0)), + std::move(make_lidar_scan_tuple())); + } else { + return std::make_tuple(); + } +} + +/** + * @brief maps the fields of a LidarScan object to the elements of the supplied + * tuple in the same order. + */ +template & Table, + typename Tuple> +void map_lidar_scan_fields_to_tuple(Tuple& tp, const ouster::LidarScan& ls) { + static_assert( + std::tuple_size_v == N, + "target tuple size has a different size from the channel field table"); + if constexpr (Index < N) { + using FieldType = typename TypeSelector::type; + using ElementType = std::remove_const_t< + std::remove_pointer_t>>; + static_assert(std::is_same_v, + "tuple element, field element types mismatch!"); + std::get(tp) = ls.field(Table[Index].first).data(); + map_lidar_scan_fields_to_tuple(tp, ls); + } +} + +/** + * @brief constructs a suitable tuple at compile time that can store a reference + * to all the fields of a specific LidarScan object (without conversion) + * according to the information specificed by the ChanFieldTable and directly + * maps the fields of the supplied LidarScan to the constructed tuple before + * returning. + * @param[in] ls LidarScan + */ +template & Table> +constexpr auto make_lidar_scan_tuple(const ouster::LidarScan& ls) { + auto tp = make_lidar_scan_tuple<0, N, Table>(); + map_lidar_scan_fields_to_tuple<0, N, Table>(tp, ls); + return tp; +} + +/** + * @brief copies field values from LidarScan fields combined as a tuple into the + * the corresponding elements of the input point pt. + * @param[out] pt point to copy values into. + * @param[in] tp tuple containing arrays to copy LidarScan field values from. + * @param[in] idx index of the point to be copied. + * @remark this method is to be used mainly with sensor native point types. + */ +template +void copy_lidar_scan_fields_to_point(PointT& pt, const Tuple& tp, int idx) { + if constexpr (Index < std::tuple_size_v) { + point::get<5 + Index>(pt) = std::get(tp)[idx]; + copy_lidar_scan_fields_to_point(pt, tp, idx); + } else { + unused_variable(pt); + unused_variable(tp); + unused_variable(idx); + } +} + +template +using Cloud = pcl::PointCloud; + +template & PROFILE, typename PointT, + typename PointS> +void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, + PointS& staging_point, + const ouster::PointsF& points, + uint64_t scan_ts, const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row) { + auto ls_tuple = make_lidar_scan_tuple<0, N, PROFILE>(ls); + auto timestamp = ls.timestamp(); + + for (auto u = 0; u < ls.h; u++) { + for (auto v = 0; v < ls.w; v++) { + const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w; + auto ts = timestamp[v_shift]; + ts = ts > scan_ts ? ts - scan_ts : 0UL; + const auto src_idx = u * ls.w + v_shift; + const auto tgt_idx = u * ls.w + v; + const auto xyz = points.row(src_idx); + // if target point and staging point has matching type bind the + // target directly and avoid performing transform_point at the end + auto& pt = CondBinaryBind>::run( + cloud.points[tgt_idx], staging_point); + // all native point types have x, y, z, t and ring values + pt.x = static_cast(xyz(0)); + pt.y = static_cast(xyz(1)); + pt.z = static_cast(xyz(2)); + // TODO: in the future we could probably skip copying t and ring + // values if knowning before hand that the target point cloud does + // not have a field to hold the timestamp or a ring for example the + // case of pcl::PointXYZ or pcl::PointXYZI. + pt.t = static_cast(ts); + pt.ring = static_cast(u); + copy_lidar_scan_fields_to_point<0>(pt, ls_tuple, src_idx); + // only perform point transform operation when PointT, and PointS + // don't match + CondBinaryOp>::run( + cloud.points[tgt_idx], staging_point, + [](auto& tgt_pt, const auto& src_pt) { + point::transform(tgt_pt, src_pt); + }); + } + } +} + +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/point_cloud_processor.h b/ouster-ros/src/point_cloud_processor.h index 6faba7be..083aae29 100644 --- a/ouster-ros/src/point_cloud_processor.h +++ b/ouster-ros/src/point_cloud_processor.h @@ -14,26 +14,38 @@ #include "ouster_ros/os_ros.h" // clang-format on +#include "point_cloud_compose.h" #include "lidar_packet_handler.h" namespace ouster_ros { +// Moved out of PointCloudProcessor to avoid type templatization +using PointCloudProcessor_OutputType = + std::vector>; +using PointCloudProcessor_PostProcessingFn = std::function; + + +template class PointCloudProcessor { public: - using OutputType = - std::vector>; - using PostProcessingFn = std::function; + using ScanToCloudFn = std::function& cloud, + const ouster::PointsF& points, + uint64_t scan_ts, const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index)>; public: PointCloudProcessor(const ouster::sensor::sensor_info& info, const std::string& frame_id, bool apply_lidar_to_sensor_transform, - PostProcessingFn func) + ScanToCloudFn scan_to_cloud_fn_, + PointCloudProcessor_PostProcessingFn post_processing_fn_) : frame(frame_id), pixel_shift_by_row(info.format.pixel_shift_by_row), cloud{info.format.columns_per_frame, info.format.pixels_per_column}, pc_msgs(get_n_returns(info)), - post_processing_fn(func) { + scan_to_cloud_fn(scan_to_cloud_fn_), + post_processing_fn(post_processing_fn_) { for (size_t i = 0; i < pc_msgs.size(); ++i) pc_msgs[i] = std::make_shared(); ouster::mat4d additional_transform = @@ -53,7 +65,8 @@ class PointCloudProcessor { } private: - void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, + template + void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, sensor_msgs::msg::PointCloud2& cloud) { // TODO: remove the staging step in the future pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); @@ -63,8 +76,11 @@ class PointCloudProcessor { void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) { for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { - scan_to_cloud_f_destaggered(cloud, points, lut_direction, - lut_offset, scan_ts, lidar_scan, + auto range_channel = static_cast(sensor::ChanField::RANGE + i); + auto range = lidar_scan.field(range_channel); + ouster::cartesianT(points, range, lut_direction, lut_offset); + + scan_to_cloud_fn(cloud, points, scan_ts, lidar_scan, pixel_shift_by_row, i); pcl_toROSMsg(cloud, *pc_msgs[i]); @@ -79,9 +95,10 @@ class PointCloudProcessor { static LidarScanProcessor create(const ouster::sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, - PostProcessingFn func) { + ScanToCloudFn scan_to_cloud_fn_, + PointCloudProcessor_PostProcessingFn post_processing_fn) { auto handler = std::make_shared( - info, frame, apply_lidar_to_sensor_transform, func); + info, frame, apply_lidar_to_sensor_transform, scan_to_cloud_fn_, post_processing_fn); return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) { @@ -100,11 +117,10 @@ class PointCloudProcessor { ouster::PointsF lut_offset; ouster::PointsF points; std::vector pixel_shift_by_row; - ouster_ros::Cloud cloud; - - OutputType pc_msgs; - - PostProcessingFn post_processing_fn; + ouster_ros::Cloud cloud; + PointCloudProcessor_OutputType pc_msgs; + ScanToCloudFn scan_to_cloud_fn; + PointCloudProcessor_PostProcessingFn post_processing_fn; }; } // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/point_cloud_processor_factory.h b/ouster-ros/src/point_cloud_processor_factory.h new file mode 100644 index 00000000..859e92da --- /dev/null +++ b/ouster-ros/src/point_cloud_processor_factory.h @@ -0,0 +1,154 @@ +#pragma once + +#include "point_cloud_processor.h" + +namespace ouster_ros { + +namespace sensor = ouster::sensor; +using sensor::UDPProfileLidar; + +class PointCloudProcessorFactory { + template + static typename PointCloudProcessor::ScanToCloudFn + make_scan_to_cloud_fn(const sensor::sensor_info& info) { + switch (info.format.udp_profile_lidar) { + case UDPProfileLidar::PROFILE_LIDAR_LEGACY: + 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) { + unused_variable(return_index); + Point_LEGACY staging_pt; + scan_to_cloud_f_destaggered( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + }; + + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_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_RNG19_RFL8_SIG16_NIR16_DUAL staging_pt; + if (return_index == 0) { + scan_to_cloud_f_destaggered< + Profile_RNG19_RFL8_SIG16_NIR16_DUAL.size(), + Profile_RNG19_RFL8_SIG16_NIR16_DUAL>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + } else { + scan_to_cloud_f_destaggered< + Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN + .size(), + Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + } + }; + + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16: + 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) { + unused_variable(return_index); + Point_RNG19_RFL8_SIG16_NIR16 staging_pt; + scan_to_cloud_f_destaggered< + Profile_RNG19_RFL8_SIG16_NIR16.size(), + Profile_RNG19_RFL8_SIG16_NIR16>(cloud, staging_pt, + points, scan_ts, ls, + pixel_shift_by_row); + }; + + case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8: + 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) { + unused_variable(return_index); + Point_RNG15_RFL8_NIR8 staging_pt; + scan_to_cloud_f_destaggered( + 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( + const sensor::sensor_info& info, const std::string& frame, + bool apply_lidar_to_sensor_transform, + PointCloudProcessor_PostProcessingFn post_processing_fn) { + auto scan_to_cloud_fn = make_scan_to_cloud_fn(info); + return PointCloudProcessor::create( + info, frame, apply_lidar_to_sensor_transform, scan_to_cloud_fn, + post_processing_fn); + } + + public: + static bool point_type_requires_intensity(const std::string& point_type) { + return point_type == "xyzi" || point_type == "xyzir" || + point_type == "original"; + } + + 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, + PointCloudProcessor_PostProcessingFn post_processing_fn) { + if (point_type == "native") { + switch (info.format.udp_profile_lidar) { + case UDPProfileLidar::PROFILE_LIDAR_LEGACY: + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL: + return make_point_cloud_procssor< + 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< + 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( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + default: + // TODO: implement fallback? + throw std::runtime_error("unsupported udp_profile_lidar"); + } + } else if (point_type == "xyz") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "xyzi") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "xyzir") { + 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, + post_processing_fn); + } + + throw std::runtime_error( + "Un-supported point type used: " + point_type + "!"); + } +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/point_meta_helpers.h b/ouster-ros/src/point_meta_helpers.h new file mode 100644 index 00000000..75fc6f8f --- /dev/null +++ b/ouster-ros/src/point_meta_helpers.h @@ -0,0 +1,151 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_meta_helpers.h + * @brief Provides a set of templated routines to enumerate or manipulate + * various pcl point representations at compile time. + */ + +#pragma once + +#include +#include +#include + +namespace ouster_ros { + +/** + * @brief A macro that facilitate providing a compile time check if a struct has + * a member with a specific name + */ +#define DEFINE_MEMBER_CHECKER(member) \ + template \ + struct has_##member : std::false_type { }; \ + template \ + struct has_##member::value, \ + void \ + >::type \ + > : std::true_type { }; \ + template \ + inline constexpr bool has_##member##_v = has_##member::value; + +/** + * @brief A template that can be used to silence unused varaiables warning. + */ +template inline void unused_variable(const T&) {} + +/** + * @brief A struct that allows enabling or disabling the execution of an operation + * using a compile time condition. + */ +template +struct CondBinaryOp { + template + inline static void run(A& a, B& b, BinaryOp binary_op) { + if constexpr (Enable) { + binary_op(a, b); + } else { + unused_variable(a); unused_variable(b); unused_variable(binary_op); + } + } +}; + +/** + * @brief A struct that a reference to the first variable if the compile time + * condition was true and returns a reference to the second variable if condition + * was false. + */ +template +struct CondBinaryBind { + template + inline static auto& run(A& a, B& b) { + if constexpr (Enable) { + unused_variable(b); + return a; + } else { + unused_variable(a); + return b; + } + } +}; + +namespace point { + +/** + * @brief A compile-time function to retrieve the number of elements that a + * certain pcl point type has + * @param[in] point a pcl point type + * @return the number of elements that a point has + */ +template +inline constexpr std::size_t size(const T& point) { + return std::tuple_size::value; +} + +template <> +inline constexpr std::size_t size(const pcl::PointXYZ&) { return 3U; } + +template <> +inline constexpr std::size_t size(const pcl::PointXYZI&) { return 4U; } + +// generic accessor that avoid having to type template before get +template +inline constexpr auto& get(T& point) { return point.template get(); } + +// pcl::PointXYZ compile time element accessors +template <> +inline constexpr auto& get<0, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.x; } +template <> +inline constexpr auto& get<1, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.y; } +template <> +inline constexpr auto& get<2, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.z; } + +// pcl::PointXYZI compile time element accessors +template <> +inline constexpr auto& get<0, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.x; } +template <> +inline constexpr auto& get<1, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.y; } +template <> +inline constexpr auto& get<2, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.z; } +template <> +inline constexpr auto& get<3, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.intensity; } + +// TODO: create a generalized vardiac templates of apply and enumerate functions + +/** + * @brief Iterates the elements of a point (compile time) applying a lambda + * function to each element in sequence within the range [Index, N) where: + * `Index < N and N <= size(pt)` + */ +template +constexpr void apply(PointT& pt, UnaryOp unary_op) { + if constexpr (Index < N) { + unary_op(get(pt)); + apply(pt, unary_op); + } else { + unused_variable(unary_op); + } +} + +/** + * @brief Enumerates the elements of a point (compile time) applying a lambda + * function to each element in sequence within the range [Index, N) where: + * `Index < N and N <= size(pt)` + * The lambda function recieves the index of each element as the first parameter + * and a reference to the element as the second parameter + */ +template +constexpr void enumerate(PointT& pt, EnumOp enum_op) { + if constexpr (Index < N) { + enum_op(Index, get(pt)); + enumerate(pt, enum_op); + } else { + unused_variable(enum_op); + } +} + +} // point +} // ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/point_transform.h b/ouster-ros/src/point_transform.h new file mode 100644 index 00000000..128fc024 --- /dev/null +++ b/ouster-ros/src/point_transform.h @@ -0,0 +1,127 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_transform.h + * @brief Implements the main transform_point method used to convert point from + * a source pcl point format usually sensor native point representation to other + * pcl point formats such as Velodyne XYZIR or pcl::XYZ, pcl::XYZI, ... + */ + +#pragma once + +#include "point_meta_helpers.h" + +namespace ouster_ros { +namespace point { + +DEFINE_MEMBER_CHECKER(x); +DEFINE_MEMBER_CHECKER(y); +DEFINE_MEMBER_CHECKER(z); +DEFINE_MEMBER_CHECKER(t); +DEFINE_MEMBER_CHECKER(ring); +DEFINE_MEMBER_CHECKER(intensity); +DEFINE_MEMBER_CHECKER(ambient); +DEFINE_MEMBER_CHECKER(range); +DEFINE_MEMBER_CHECKER(signal); +DEFINE_MEMBER_CHECKER(reflectivity); +DEFINE_MEMBER_CHECKER(near_ir); + +template +void transform(PointTGT& tgt_pt, const PointSRC& src_pt) { + // NOTE: for now we assume all points have xyz component + tgt_pt.x = src_pt.x; tgt_pt.y = src_pt.y; tgt_pt.z = src_pt.z; + + // t: timestamp + CondBinaryOp && has_t_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.t = src_pt.t; } + ); + + CondBinaryOp && !has_t_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { tgt_pt.t = 0U; } + ); + + // ring + CondBinaryOp && has_ring_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.ring = src_pt.ring; } + ); + + CondBinaryOp && !has_ring_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { + tgt_pt.ring = static_cast(0); + } + ); + + // range + CondBinaryOp && has_range_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.range = src_pt.range; } + ); + + CondBinaryOp && !has_range_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { tgt_pt.range = 0U; } + ); + + // signal + CondBinaryOp && has_signal_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.signal = src_pt.signal; } + ); + + CondBinaryOp && !has_signal_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { + tgt_pt.signal = static_cast(0); + } + ); + + // intensity <- signal + // PointTGT should not have signal and intensity at the same time [normally] + CondBinaryOp && has_signal_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { + tgt_pt.intensity = static_cast(src_pt.signal); + } + ); + + CondBinaryOp && !has_signal_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { + tgt_pt.intensity = static_cast(0); + } + ); + + // reflectivity + CondBinaryOp && has_reflectivity_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { + tgt_pt.reflectivity = src_pt.reflectivity; + } + ); + + CondBinaryOp && !has_reflectivity_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { + tgt_pt.reflectivity = static_cast(0); + } + ); + + // near_ir + CondBinaryOp && has_near_ir_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto& src_pt) { tgt_pt.near_ir = src_pt.near_ir; } + ); + + CondBinaryOp && !has_near_ir_v>::run( + tgt_pt, src_pt, [](auto& tgt_pt, const auto&) { + tgt_pt.near_ir = static_cast(0); } + ); + + // ambient <- near_ir + CondBinaryOp && has_near_ir_v>::run(tgt_pt, src_pt, + [](auto& tgt_pt, const auto& src_pt) { + tgt_pt.ambient = static_cast(src_pt.near_ir); + } + ); + + CondBinaryOp && !has_near_ir_v>::run(tgt_pt, src_pt, + [](auto& tgt_pt, const auto&) { + tgt_pt.ambient = static_cast(0); + } + ); +} + +} // point +} // ouster_ros \ No newline at end of file diff --git a/ouster-ros/test/point_accessor_test.cpp b/ouster-ros/test/point_accessor_test.cpp new file mode 100644 index 00000000..e69a3b91 --- /dev/null +++ b/ouster-ros/test/point_accessor_test.cpp @@ -0,0 +1,218 @@ +#include + +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the +// header file needs to be the first include due to PCL_NO_PRECOMPILE flag +// clang-format off +#include "ouster_ros/os_ros.h" +// clang-format on +#include "ouster_ros/sensor_point_types.h" +#include "ouster_ros/common_point_types.h" +#include "ouster_ros/os_point.h" +#include "../src/point_meta_helpers.h" + +using namespace std; +using namespace ouster_ros; + +class PointAccessorTest : public ::testing::Test { + protected: + void SetUp() override { + pt_xyz = pcl::_PointXYZ{0.0f, 1.0f, 2.0f, 1.0f}; + pt_xyzi = pcl::_PointXYZI{{0.0f, 1.0f, 2.0f, 1.0}, 3.0f}; + pt_xyzir = ouster_ros::_PointXYZIR{{0.0f, 1.0f, 2.0f, 1.0f}, 3.0f, 4}; + + pt_legacy = ouster_ros::_Point_LEGACY{ + {0.0f, 1.0f, 2.0f, 1.0f}, // x, y, z, w + 3, 4, // t, ring + 5, 6, 7, 8 // range, signal, reflectivity, near_ir + }; + + pt_rg19_rf8_sg16_nr16_dual = ouster_ros::_Point_RNG19_RFL8_SIG16_NIR16_DUAL{ + {0.0f, 1.0f, 2.0f, 1.0f}, // x, y, z, w + 3, 4, // t, ring + 5, 6, 7, 8 // range, signal, reflectivity, near_ir + }; + + pt_rg19_rf8_sg16_nr16 = ouster_ros::_Point_RNG19_RFL8_SIG16_NIR16{ + {0.0f, 1.0f, 2.0f, 1.0f}, // x, y, z, w + 3, 4, // t, ring + 5, 6, 7, 8 // range, signal, reflectivity, near_ir + }; + + pt_rg15_rfl8_nr8 = ouster_ros::_Point_RNG15_RFL8_NIR8{ + {0.0f, 1.0f, 2.0f, 1.0f}, // x, y, z, w + 3, 4, // t, ring + 5, 6, 7, // range, reflectivity, near_ir + }; + + pt_os_point = ouster_ros::_Point{ + {0.0f, 1.0f, 2.0f, 1.0f}, // x, y, z, w + 3.0f, 4, // intensity, t, + 5, 6, 7, 8 // reflectivity, ring, ambient, range + }; + } + + void TearDown() override {} + + // pcl & velodyne point types + static pcl::PointXYZ pt_xyz; + static pcl::PointXYZI pt_xyzi; + static PointXYZIR pt_xyzir; + // native point types + static Point_LEGACY pt_legacy; + static Point_RNG19_RFL8_SIG16_NIR16_DUAL pt_rg19_rf8_sg16_nr16_dual; + static Point_RNG19_RFL8_SIG16_NIR16 pt_rg19_rf8_sg16_nr16; + static Point_RNG15_RFL8_NIR8 pt_rg15_rfl8_nr8; + // ouster_ros original/legacy point (not to be confused with Point_LEGACY) + static ouster_ros::Point pt_os_point; +}; + +// pcl & velodyne point types +pcl::PointXYZ PointAccessorTest::pt_xyz; +pcl::PointXYZI PointAccessorTest::pt_xyzi; +PointXYZIR PointAccessorTest::pt_xyzir; +// native point types +Point_LEGACY PointAccessorTest::pt_legacy; +Point_RNG19_RFL8_SIG16_NIR16_DUAL PointAccessorTest::pt_rg19_rf8_sg16_nr16_dual; +Point_RNG19_RFL8_SIG16_NIR16 PointAccessorTest::pt_rg19_rf8_sg16_nr16; +Point_RNG15_RFL8_NIR8 PointAccessorTest::pt_rg15_rfl8_nr8; +// ouster_ros original/legacy point (not to be confused with Point_LEGACY) +ouster_ros::Point PointAccessorTest::pt_os_point; + +TEST_F(PointAccessorTest, ElementCount) { + // pcl & velodyne point types + EXPECT_EQ(point::size(pt_xyz), 3U); + EXPECT_EQ(point::size(pt_xyzi), 4U); + EXPECT_EQ(point::size(pt_xyzir), 5U); + // all native sensor point types has {x, y, z, t and ring} fields + EXPECT_EQ(point::size(pt_legacy), 5 + Profile_LEGACY.size()); + EXPECT_EQ(point::size(pt_rg19_rf8_sg16_nr16_dual), + 5 + Profile_RNG19_RFL8_SIG16_NIR16_DUAL.size()); + EXPECT_EQ(point::size(pt_rg19_rf8_sg16_nr16), + 5 + Profile_RNG19_RFL8_SIG16_NIR16.size()); + EXPECT_EQ(point::size(pt_rg15_rfl8_nr8), + 5 + Profile_RNG15_RFL8_NIR8.size()); + // ouster_ros original/legacy point type + EXPECT_EQ(point::size(pt_os_point), 9U); +} + +// This test is only concerned with pcl & velodyne point types to verify that +// point::get performs as expected. Subsequent tests does the same test on +// the remaining point types but uses point::apply and point::enumerate rather +// than accessing each element individually/directly as this test case does. +TEST_F(PointAccessorTest, ElementGetSet) { + // pcl::PointXYZ + EXPECT_EQ(point::get<0>(pt_xyz), 0.0f); + EXPECT_EQ(point::get<1>(pt_xyz), 1.0f); + EXPECT_EQ(point::get<2>(pt_xyz), 2.0f); + // pcl::PointXYZI + EXPECT_EQ(point::get<0>(pt_xyzi), 0.0f); + EXPECT_EQ(point::get<1>(pt_xyzi), 1.0f); + EXPECT_EQ(point::get<2>(pt_xyzi), 2.0f); + EXPECT_EQ(point::get<3>(pt_xyzi), 3.0f); + // ouster_ros::PointXYZIR + EXPECT_EQ(point::get<0>(pt_xyzir), 0.0f); + EXPECT_EQ(point::get<1>(pt_xyzir), 1.0f); + EXPECT_EQ(point::get<2>(pt_xyzir), 2.0f); + EXPECT_EQ(point::get<3>(pt_xyzir), 3.0f); + EXPECT_EQ(point::get<4>(pt_xyzir), 4); + + // pcl::PointXYZ + point::get<0>(pt_xyz) = 10.0f; + point::get<1>(pt_xyz) = 11.0f; + point::get<2>(pt_xyz) = 12.0f; + // pcl::PointXYZI + point::get<0>(pt_xyzi) = 10.0f; + point::get<1>(pt_xyzi) = 11.0f; + point::get<2>(pt_xyzi) = 12.0f; + point::get<3>(pt_xyzi) = 13.0f; + // ouster_ros::PointXYZIR + point::get<0>(pt_xyzir) = 10.0f; + point::get<1>(pt_xyzir) = 11.0f; + point::get<2>(pt_xyzir) = 12.0f; + point::get<3>(pt_xyzir) = 13.0f; + point::get<4>(pt_xyzir) = 14; + + // pcl::PointXYZ + EXPECT_EQ(point::get<0>(pt_xyz), 10.0f); + EXPECT_EQ(point::get<1>(pt_xyz), 11.0f); + EXPECT_EQ(point::get<2>(pt_xyz), 12.0f); + // pcl::PointXYZI + EXPECT_EQ(point::get<0>(pt_xyzi), 10.0f); + EXPECT_EQ(point::get<1>(pt_xyzi), 11.0f); + EXPECT_EQ(point::get<2>(pt_xyzi), 12.0f); + EXPECT_EQ(point::get<3>(pt_xyzi), 13.0f); + // ouster_ros::PointXYZIR + EXPECT_EQ(point::get<0>(pt_xyzir), 10.0f); + EXPECT_EQ(point::get<1>(pt_xyzir), 11.0f); + EXPECT_EQ(point::get<2>(pt_xyzir), 12.0f); + EXPECT_EQ(point::get<3>(pt_xyzir), 13.0f); + EXPECT_EQ(point::get<4>(pt_xyzir), 14); +} + +template +void expect_element_equals_index(PointT& pt) { + point::enumerate<0, N>(pt, [](auto index, auto value) { + EXPECT_EQ(value, static_cast(index)); + }); +} + +TEST_F(PointAccessorTest, ExpectElementValueSameAsIndex) { + // pcl + velodyne point types + expect_element_equals_index(pt_xyz); + expect_element_equals_index(pt_xyzi); + expect_element_equals_index(pt_xyzir); + // native sensor point types + expect_element_equals_index(pt_legacy); + expect_element_equals_index( + pt_rg19_rf8_sg16_nr16_dual); + expect_element_equals_index( + pt_rg19_rf8_sg16_nr16); + expect_element_equals_index( + pt_rg15_rfl8_nr8); + // ouster_ros original/legacy point type + expect_element_equals_index(pt_os_point); +} + +template +void increment_by_value(PointT& pt, int increment) { + point::apply<0, N>(pt, [increment](auto& value) { value += increment; }); +} + +template +void expect_value_increased_by_value(PointT& pt, int increment) { + point::enumerate<0, N>(pt, [increment](auto index, auto value) { + EXPECT_EQ(value, static_cast(index + increment)); + }); +}; + +TEST_F(PointAccessorTest, ExpectPointElementValueIncrementedByValue) { + auto increment = 1; + + // pcl + velodyne point types + increment_by_value(pt_xyz, increment); + expect_value_increased_by_value(pt_xyz, increment); + increment_by_value(pt_xyzi, increment); + expect_value_increased_by_value(pt_xyzi, increment); + increment_by_value(pt_xyzir, increment); + expect_value_increased_by_value(pt_xyzir, increment); + // // native sensor point types + increment_by_value(pt_legacy, increment); + expect_value_increased_by_value(pt_legacy, + increment); + increment_by_value( + pt_rg19_rf8_sg16_nr16_dual, increment); + expect_value_increased_by_value( + pt_rg19_rf8_sg16_nr16_dual, increment); + increment_by_value( + pt_rg19_rf8_sg16_nr16, increment); + expect_value_increased_by_value( + pt_rg19_rf8_sg16_nr16, increment); + increment_by_value(pt_rg15_rfl8_nr8, + increment); + expect_value_increased_by_value( + pt_rg15_rfl8_nr8, increment); + // // ouster_ros original/legacy point type + increment_by_value(pt_os_point, increment); + expect_value_increased_by_value(pt_os_point, + increment); +} diff --git a/ouster-ros/test/point_cloud_compose_test.cpp b/ouster-ros/test/point_cloud_compose_test.cpp new file mode 100644 index 00000000..c119f1bc --- /dev/null +++ b/ouster-ros/test/point_cloud_compose_test.cpp @@ -0,0 +1,74 @@ +#include +#include + +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the +// header file needs to be the first include due to PCL_NO_PRECOMPILE flag +// clang-format off +#include "ouster_ros/os_ros.h" +// clang-format on +#include "ouster_ros/sensor_point_types.h" +#include "ouster_ros/common_point_types.h" +#include "ouster_ros/os_point.h" +#include "../src/point_meta_helpers.h" +#include "../src/point_cloud_compose.h" + +class PointCloudComposeTest : public ::testing::Test { + protected: + void SetUp() override {} + + void TearDown() override {} +}; + +using namespace std; +using namespace ouster::sensor; +using namespace ouster_ros; + +// TODO: generalize the test case! + +TEST_F(PointCloudComposeTest, MapLidarScanFields) { + const auto WIDTH = 5U; + const auto HEIGHT = 3U; + const auto SAMPLES = WIDTH * HEIGHT; + UDPProfileLidar lidar_udp_profile = + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL; + + ouster::LidarScan ls(WIDTH, HEIGHT, lidar_udp_profile); + + auto fill_data = [](auto& img, auto base, auto count) { + auto* p = img.data(); + for (auto i = 0U; i < count; ++i) + p[i] = + static_cast>(base + i); + }; + + auto range = ls.field(RANGE); + auto signal = ls.field(SIGNAL); + auto reflect = ls.field(REFLECTIVITY); + auto near_ir = ls.field(NEAR_IR); + + // choose a base value that could ultimately wrap around + fill_data(range, static_cast(1 + (1ULL << 32) - SAMPLES / 2), + SAMPLES); + fill_data(signal, static_cast(3 + (1 << 16) - SAMPLES / 2), + SAMPLES); + fill_data(reflect, static_cast(5 + (1 << 8) - SAMPLES / 2), + SAMPLES); + fill_data(near_ir, static_cast(7 + (1 << 16) - SAMPLES / 2), + SAMPLES); + + ouster_ros::Cloud cloud{WIDTH, HEIGHT}; + + auto ls_tuple = + make_lidar_scan_tuple<0, Profile_RNG19_RFL8_SIG16_NIR16_DUAL.size(), + Profile_RNG19_RFL8_SIG16_NIR16_DUAL>(ls); + + ouster_ros::Point_RNG19_RFL8_SIG16_NIR16_DUAL pt; + + for (auto src_idx = 0U; src_idx < SAMPLES; ++src_idx) { + copy_lidar_scan_fields_to_point<0>(pt, ls_tuple, src_idx); + EXPECT_EQ(point::get<5>(pt), range(0, src_idx)); + EXPECT_EQ(point::get<6>(pt), signal(0, src_idx)); + EXPECT_EQ(point::get<7>(pt), reflect(0, src_idx)); + EXPECT_EQ(point::get<8>(pt), near_ir(0, src_idx)); + } +} diff --git a/ouster-ros/test/point_transform_test.cpp b/ouster-ros/test/point_transform_test.cpp new file mode 100644 index 00000000..18607cb9 --- /dev/null +++ b/ouster-ros/test/point_transform_test.cpp @@ -0,0 +1,292 @@ +#include + +#include + +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the +// header file needs to be the first include due to PCL_NO_PRECOMPILE flag +// clang-format off +#include "ouster_ros/os_ros.h" +// clang-format on +#include "ouster_ros/sensor_point_types.h" +#include "ouster_ros/common_point_types.h" +#include "ouster_ros/os_point.h" +#include "../src/point_cloud_compose.h" +#include "../src/point_meta_helpers.h" + +using namespace ouster_ros; +using namespace ouster_ros::point; + +class PointTransformTest : public ::testing::Test { + template + void initialize_point_elements_with_randoms(PointT& pt) { + std::default_random_engine g; + std::uniform_real_distribution d(0.0, 128.0); + point::apply<0, N>(pt, [&g, &d](auto& value) { + using value_type = std::remove_reference_t; + value = static_cast(d(g)); + }); + } + + protected: + void SetUp() override { + // pcl + velodyne point types + initialize_point_elements_with_randoms(pt_xyz); + initialize_point_elements_with_randoms(pt_xyzi); + initialize_point_elements_with_randoms(pt_xyzir); + // native sensor point types + initialize_point_elements_with_randoms( + pt_legacy); + initialize_point_elements_with_randoms(pt_rg19_rf8_sg16_nr16_dual); + initialize_point_elements_with_randoms(pt_rg19_rf8_sg16_nr16); + initialize_point_elements_with_randoms( + pt_rg15_rfl8_nr8); + // ouster_ros original/legacy point type + initialize_point_elements_with_randoms( + pt_os_point); + } + + void TearDown() override {} + + // pcl & velodyne point types + static pcl::PointXYZ pt_xyz; + static pcl::PointXYZI pt_xyzi; + static PointXYZIR pt_xyzir; + // native point types + static Point_LEGACY pt_legacy; + static Point_RNG19_RFL8_SIG16_NIR16_DUAL pt_rg19_rf8_sg16_nr16_dual; + static Point_RNG19_RFL8_SIG16_NIR16 pt_rg19_rf8_sg16_nr16; + static Point_RNG15_RFL8_NIR8 pt_rg15_rfl8_nr8; + // ouster_ros original/legacy point (not to be confused with Point_LEGACY) + static ouster_ros::Point pt_os_point; +}; + +// pcl & velodyne point types +pcl::PointXYZ PointTransformTest::pt_xyz; +pcl::PointXYZI PointTransformTest::pt_xyzi; +PointXYZIR PointTransformTest::pt_xyzir; +// native point types +Point_LEGACY PointTransformTest::pt_legacy; +Point_RNG19_RFL8_SIG16_NIR16_DUAL + PointTransformTest::pt_rg19_rf8_sg16_nr16_dual; +Point_RNG19_RFL8_SIG16_NIR16 PointTransformTest::pt_rg19_rf8_sg16_nr16; +Point_RNG15_RFL8_NIR8 PointTransformTest::pt_rg15_rfl8_nr8; +// ouster_ros original/legacy point (not to be confused with Point_LEGACY) +ouster_ros::Point PointTransformTest::pt_os_point; + +template +void expect_points_xyz_equal(PointT& point1, PointU& point2) { + EXPECT_EQ(point1.x, point2.x); + EXPECT_EQ(point1.y, point2.y); + EXPECT_EQ(point1.z, point2.z); +} + +template +void verify_point_transform(PointTGT& tgt_pt, const PointSRC& src_pt) { + // t: timestamp + CondBinaryOp && has_t_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.t, src_pt.t); + }); + + CondBinaryOp && !has_t_v>::run( + tgt_pt, src_pt, + [](const auto& tgt_pt, const auto&) { EXPECT_EQ(tgt_pt.t, 0U); }); + + // ring + CondBinaryOp && has_ring_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.ring, src_pt.ring); + }); + + CondBinaryOp && !has_ring_v>::run( + tgt_pt, src_pt, + [](const auto& tgt_pt, const auto&) { EXPECT_EQ(tgt_pt.ring, 0U); }); + + // range + CondBinaryOp && has_range_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.range, src_pt.range); + }); + + CondBinaryOp && !has_range_v>::run( + tgt_pt, src_pt, + [](const auto& tgt_pt, const auto&) { EXPECT_EQ(tgt_pt.range, 0U); }); + + // signal + CondBinaryOp && has_signal_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.signal, src_pt.signal); + }); + + CondBinaryOp && !has_signal_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto&) { + EXPECT_EQ(tgt_pt.signal, static_cast(0)); + }); + + // intensity <- signal + CondBinaryOp && has_signal_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.intensity, + static_cast(src_pt.signal)); + }); + + CondBinaryOp && !has_signal_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto&) { + EXPECT_EQ(tgt_pt.intensity, + static_cast(0)); + }); + + // reflectivity + CondBinaryOp && has_reflectivity_v>:: + run(tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.reflectivity, src_pt.reflectivity); + }); + + CondBinaryOp && + !has_reflectivity_v>:: + run(tgt_pt, src_pt, [](const auto& tgt_pt, const auto&) { + EXPECT_EQ(tgt_pt.reflectivity, + static_cast(0)); + }); + + // near_ir + CondBinaryOp && has_near_ir_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.near_ir, src_pt.near_ir); + }); + + CondBinaryOp && has_near_ir_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto&) { + EXPECT_EQ(tgt_pt.near_ir, static_cast(0)); + }); + + // ambient <- near_ir + CondBinaryOp && has_near_ir_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto& src_pt) { + EXPECT_EQ(tgt_pt.ambient, + static_cast(src_pt.near_ir)); + }); + + CondBinaryOp && !has_near_ir_v>::run( + tgt_pt, src_pt, [](const auto& tgt_pt, const auto&) { + EXPECT_EQ(tgt_pt.ambient, static_cast(0)); + }); +} + +// lambda function to check that point elements other than xyz have been zeroed +template +void expect_point_fields_zeros(PointT& pt) { + // starting at 3 to skip xyz + point::apply<3, N>(pt, [](auto value) { + EXPECT_EQ(value, static_cast(0)); + }); +}; + +TEST_F(PointTransformTest, ExpectPointFieldZeroed) { + point::transform(pt_xyzi, pt_xyz); + expect_points_xyz_equal(pt_xyzi, pt_xyz); + expect_point_fields_zeros(pt_xyzi); + + point::transform(pt_xyzir, pt_xyz); + expect_points_xyz_equal(pt_xyzir, pt_xyz); + expect_point_fields_zeros(pt_xyzir); + + point::transform(pt_legacy, pt_xyz); + expect_points_xyz_equal(pt_legacy, pt_xyz); + expect_point_fields_zeros(pt_legacy); + + point::transform(pt_rg19_rf8_sg16_nr16_dual, pt_xyz); + expect_points_xyz_equal(pt_rg19_rf8_sg16_nr16_dual, pt_xyz); + expect_point_fields_zeros( + pt_rg19_rf8_sg16_nr16_dual); + + point::transform(pt_rg19_rf8_sg16_nr16, pt_xyz); + expect_points_xyz_equal(pt_rg19_rf8_sg16_nr16, pt_xyz); + expect_point_fields_zeros( + pt_rg19_rf8_sg16_nr16); + + point::transform(pt_rg15_rfl8_nr8, pt_xyz); + expect_points_xyz_equal(pt_rg15_rfl8_nr8, pt_xyz); + expect_point_fields_zeros(pt_rg15_rfl8_nr8); + + point::transform(pt_os_point, pt_xyz); + expect_points_xyz_equal(pt_os_point, pt_xyz); + expect_point_fields_zeros(pt_os_point); +} + +TEST_F(PointTransformTest, TestTransformReduce_LEGACY) { + point::transform(pt_xyz, pt_legacy); + expect_points_xyz_equal(pt_xyz, pt_legacy); + verify_point_transform(pt_xyz, pt_legacy); + + point::transform(pt_xyzi, pt_legacy); + expect_points_xyz_equal(pt_xyzi, pt_legacy); + verify_point_transform(pt_xyzi, pt_legacy); + + point::transform(pt_xyzir, pt_legacy); + expect_points_xyz_equal(pt_xyzir, pt_legacy); + verify_point_transform(pt_xyzir, pt_legacy); +} + +TEST_F(PointTransformTest, TestTransformReduce_RNG19_RFL8_SIG16_NIR16_DUAL) { + point::transform(pt_xyz, pt_rg19_rf8_sg16_nr16_dual); + expect_points_xyz_equal(pt_xyz, pt_rg19_rf8_sg16_nr16_dual); + verify_point_transform(pt_xyz, pt_rg19_rf8_sg16_nr16_dual); + + point::transform(pt_xyzi, pt_rg19_rf8_sg16_nr16_dual); + expect_points_xyz_equal(pt_xyzi, pt_rg19_rf8_sg16_nr16_dual); + verify_point_transform(pt_xyzi, pt_rg19_rf8_sg16_nr16_dual); + + point::transform(pt_xyzir, pt_rg19_rf8_sg16_nr16_dual); + expect_points_xyz_equal(pt_xyzir, pt_rg19_rf8_sg16_nr16_dual); + verify_point_transform(pt_xyzir, pt_rg19_rf8_sg16_nr16_dual); +} + +TEST_F(PointTransformTest, TestTransformReduce_RNG19_RFL8_SIG16_NIR16) { + point::transform(pt_xyz, pt_rg19_rf8_sg16_nr16); + expect_points_xyz_equal(pt_xyz, pt_rg19_rf8_sg16_nr16); + verify_point_transform(pt_xyz, pt_rg19_rf8_sg16_nr16); + + point::transform(pt_xyzi, pt_rg19_rf8_sg16_nr16); + expect_points_xyz_equal(pt_xyzi, pt_rg19_rf8_sg16_nr16); + verify_point_transform(pt_xyzi, pt_rg19_rf8_sg16_nr16); + + point::transform(pt_xyzir, pt_rg19_rf8_sg16_nr16); + expect_points_xyz_equal(pt_xyzir, pt_rg19_rf8_sg16_nr16); + verify_point_transform(pt_xyzir, pt_rg19_rf8_sg16_nr16); +} + +TEST_F(PointTransformTest, TestTransformReduce_RNG15_RFL8_NIR8) { + point::transform(pt_xyz, pt_rg15_rfl8_nr8); + expect_points_xyz_equal(pt_xyz, pt_rg15_rfl8_nr8); + verify_point_transform(pt_xyz, pt_rg15_rfl8_nr8); + + point::transform(pt_xyzi, pt_rg15_rfl8_nr8); + expect_points_xyz_equal(pt_xyzi, pt_rg15_rfl8_nr8); + verify_point_transform(pt_xyzi, pt_rg15_rfl8_nr8); + + point::transform(pt_xyzir, pt_rg15_rfl8_nr8); + expect_points_xyz_equal(pt_xyzir, pt_rg15_rfl8_nr8); + verify_point_transform(pt_xyzir, pt_rg15_rfl8_nr8); +} + +TEST_F(PointTransformTest, + TestTransform_SensorNativePoints_2_ouster_ros_Point) { + point::transform(pt_os_point, pt_legacy); + expect_points_xyz_equal(pt_os_point, pt_legacy); + verify_point_transform(pt_os_point, pt_legacy); + + point::transform(pt_os_point, pt_rg19_rf8_sg16_nr16_dual); + expect_points_xyz_equal(pt_os_point, pt_rg19_rf8_sg16_nr16_dual); + verify_point_transform(pt_os_point, pt_rg19_rf8_sg16_nr16_dual); + + point::transform(pt_os_point, pt_rg19_rf8_sg16_nr16); + expect_points_xyz_equal(pt_os_point, pt_rg19_rf8_sg16_nr16); + verify_point_transform(pt_os_point, pt_rg19_rf8_sg16_nr16); + + point::transform(pt_os_point, pt_rg15_rfl8_nr8); + expect_points_xyz_equal(pt_os_point, pt_rg15_rfl8_nr8); + verify_point_transform(pt_os_point, pt_rg15_rfl8_nr8); +} \ No newline at end of file diff --git a/ouster-sensor-msgs/package.xml b/ouster-sensor-msgs/package.xml index 6e4df774..5c46bfd2 100644 --- a/ouster-sensor-msgs/package.xml +++ b/ouster-sensor-msgs/package.xml @@ -2,7 +2,7 @@ ouster_sensor_msgs - 0.11.0 + 0.11.1 ouster_ros message and service definitions ouster developers BSD