From 61fa6ce687a112ba064258fcf52c7823a8e0992f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= Date: Sat, 23 Mar 2024 09:34:33 +0100 Subject: [PATCH] merge changes to fork (#3) * SW-5607: rename package ouster_msgs to avoid package name conflict in ros index (#244) * rename package ouster_msgs to avoid conflict name conflict in ros index * set ouster_sensor_msgs version number to match with ouster_ros package * 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. * SW-5466: Support Velodyne point type in the ROS driver amendments (#254) * Add support to control point_type through launch.xml files + * Add a note to CHANGELOG about the breaking change for ptp/utc time offset * SW-5623: Bump up ouster_client to 20231031 release (#262) * Bump ouster-client to 2023103 release * fix: gracefully stop the driver when shutdown is requested. * ROS-196: laser scan from ros driver is not properly aligned with point cloud [humble] (#203) * Apply destagger to laser scan + Add laser to RVIZ * Align LaserScan with the PointCloud * Apply proper pixel shift * Resolve the issue of zeroed laserscan on dual mode * Address an issue where LaserScan appeared different on FW prior to 2.4 * Fix the issue for odd numbers * List selected sensors on the main page + Update RVIZ config to highlight the 2D LaserScan. * Use timeout when waiting for packets to be proceed in case they don't come (#293) * docs: fix spelling mistakes (#296) --------- Co-authored-by: Ussama Naal <606033+Samahu@users.noreply.github.com> Co-authored-by: Andre Nguyen --- CHANGELOG.rst | 49 ++- README.md | 78 ++-- ouster-ros/CMakeLists.txt | 20 +- ouster-ros/config/driver_params.yaml | 19 + .../config/os_sensor_cloud_image_params.yaml | 1 + ouster-ros/config/viz.rviz | 54 ++- .../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 | 92 +---- .../include/ouster_ros/os_sensor_node_base.h | 4 +- .../include/ouster_ros/sensor_point_types.h | 336 ++++++++++++++++++ ouster-ros/launch/record.composite.launch.xml | 10 + ouster-ros/launch/replay.composite.launch.xml | 10 + ouster-ros/launch/sensor.composite.launch.xml | 10 + .../launch/sensor.independent.launch.xml | 10 + ouster-ros/launch/sensor_mtp.launch.xml | 10 + ouster-ros/ouster-sdk | 2 +- ouster-ros/package.xml | 4 +- ouster-ros/src/laser_scan_processor.h | 27 +- ouster-ros/src/lidar_packet_handler.h | 22 -- ouster-ros/src/os_cloud_node.cpp | 64 ++-- ouster-ros/src/os_driver_node.cpp | 53 ++- ouster-ros/src/os_image_node.cpp | 217 ++++------- ouster-ros/src/os_ros.cpp | 262 +++----------- ouster-ros/src/os_sensor_node.cpp | 65 ++-- ouster-ros/src/os_sensor_node.h | 23 +- ouster-ros/src/os_sensor_node_base.cpp | 2 +- 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 +++++++++++++++ .../CMakeLists.txt | 2 +- .../msg/PacketMsg.msg | 0 .../package.xml | 4 +- .../srv/GetConfig.srv | 0 .../srv/GetMetadata.srv | 0 .../srv/SetConfig.srv | 0 41 files changed, 2171 insertions(+), 639 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 rename {ouster-msgs => ouster-sensor-msgs}/CMakeLists.txt (93%) rename {ouster-msgs => ouster-sensor-msgs}/msg/PacketMsg.msg (100%) rename {ouster-msgs => ouster-sensor-msgs}/package.xml (92%) rename {ouster-msgs => ouster-sensor-msgs}/srv/GetConfig.srv (100%) rename {ouster-msgs => ouster-sensor-msgs}/srv/GetMetadata.srv (100%) rename {ouster-msgs => ouster-sensor-msgs}/srv/SetConfig.srv (100%) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index dcec3f6e..3a3a2d74 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,13 +4,60 @@ Changelog [unreleased] ============ -* breaking: publish PCL point clouds destaggered. +* [BUGFIX]: LaserScan is not properly aligned with generated point cloud + * address an issue where LaserScan appeared different on FW prior to 2.4 +* [BUGFIX]: LaserScan does not work when using dual mode +* [BUGFIX]: ROS2 crashes when standby mode is set and then set to normal + + +ouster_ros v0.12.0 +================== +* [BREAKING]: updated ouster_client to the release of ``20231031`` [v0.10.0]; changes listed below. +* [BREAKING]: publish PCL point clouds destaggered. * introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode is used. + * [BREAKING]: the default value of ``ptp_utc_tai_offset`` is set to ``-37.0``. To retain the same + time offset for an existing system, users need to set ``ptp_utc_tai_offset`` to ``0.0``. * 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. +* fix: gracefully stop the driver when shutdown is requested. +ouster_client +------------- +* [BREAKING] Updates to ``sensor_info`` include: + * new fields added: ``build_date``, ``image_rev``, ``prod_pn``, ``status``, ``cal`` (representing + the value stored in the ``calibration_status`` metadata JSON key), ``config`` (representing the + value of the ``sensor_config`` metadata JSON key) + * the original JSON string is accessible via the ``original_string()`` method + * The ``updated_metadata_string()`` now returns a JSON string reflecting any modifications to + ``sensor_info`` + * ``to_string`` is now marked as deprecated +* [BREAKING] The RANGE field defined in `parsing.cpp`, for the low data rate profile, is now 32 bits + wide (originally 16 bits). + * Please note this fixes a SDK bug. The underlying UDP format is unchanged. +* [BREAKING] The NEAR_IR field defined in `parsing.cpp`, for the low data rate profile, is now 16 + bits wide (originally 8 bits). + * Plase note this fixes a SDK bug. The underlying UDP format is unchanged. +* [BREAKING] changed frame_id return size to 32 bits from 16 bits +* An array of per-packet timestamps (called ``packet_timestamp``) is added to ``LidarScan`` +* The client now retries failed requests to an Ouster sensor's HTTP API +* Increased the default timeout for HTTP requests to 40s +* Added FuSA UDP profile to support Ouster FW 3.1+ +* Improved ``ScanBatcher`` performance by roughly 3x (depending on hardware) +* Receive buffer size increased from 256KB to 1MB +* [bugfix] Fixed an issue that caused incorrect Cartesian point computation in the ``viz.Cloud`` + Python class +* [bugfix] Fixed an issue that resulted in some ``packet_format`` methods returning an uninitialized + value +* [bugfix] Fixed a libpcap-related linking issue +* [bugfix] Fixed an eigen 3.3-related linking issue +* [bugfix] Fixed a zero beam angle calculation issue +* [bugfix] Fixed dropped columns issue with 4096x5 and 2048x10 ouster_ros v0.10.0 ================== diff --git a/README.md b/README.md index ca202634..b9408478 100644 --- a/README.md +++ b/README.md @@ -12,24 +12,27 @@ | 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) + - [Supported Devices](#supported-devices) + - [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 @@ -41,6 +44,17 @@ messages on the topics of `/ouster/imu` and `/ouster/points`. In the case the us dual return and it was configured to use this capability, then another topic will published under the name `/ouster/points2` which corresponds to the second point cloud. + +## Supported Devices +The driver supports the following list of Ouster sensors: +- [OS0](https://ouster.com/products/hardware/os0-lidar-sensor) +- [OS1](https://ouster.com/products/hardware/os1-lidar-sensor) +- [OS2](https://ouster.com/products/hardware/os2-lidar-sensor) +- [OSDome](https://ouster.com/products/hardware/osdome-lidar-sensor) + +You can obtain detailed specs sheet about the sensors and obtain updated FW through the website +[downloads](https://ouster.com/downloads) section. + ## Requirements This branch is only intended for use with **Rolling**, **Humble** and **Iron** ROS 2 distros. Please refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with the @@ -238,8 +252,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 e673740c..b56cf112 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(ouster_msgs REQUIRED) +find_package(ouster_sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(Eigen3 REQUIRED) @@ -63,7 +63,7 @@ set(ouster_ros_library_deps rclcpp sensor_msgs geometry_msgs - ouster_msgs + ouster_sensor_msgs pcl_conversions tf2 tf2_eigen @@ -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/config/viz.rviz b/ouster-ros/config/viz.rviz index 10756549..c7f1642b 100644 --- a/ouster-ros/config/viz.rviz +++ b/ouster-ros/config/viz.rviz @@ -13,7 +13,9 @@ Panels: - /nearir1/Topic1 - /reflec1 - /signal1 - Splitter Ratio: 0.5 + - /LaserScan1 + - /LaserScan1/Topic1 + Splitter Ratio: 0.626074492931366 Tree Height: 1185 - Class: rviz_common/Selection Name: Selection @@ -92,23 +94,23 @@ Visualization Manager: Enabled: true Frame Timeout: 15 Frames: - " os_imu": + All Enabled: true + os_imu: Value: true - " os_lidar": + os_lidar: Value: true - " os_sensor": + os_sensor: Value: true - All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - " os_sensor": - " os_imu": + os_sensor: + os_imu: {} - " os_lidar": + os_lidar: {} Update Interval: 0 Value: true @@ -168,10 +170,44 @@ Visualization Manager: Reliability Policy: Best Effort Value: /ouster/signal_image Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1000 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.02 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /ouster/scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: "os_sensor" + Fixed Frame: os_sensor Frame Rate: 30 Name: root Tools: 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..0a174f3f 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 representation 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 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 214d5b37..10fbf66b 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -19,20 +19,17 @@ #include #include -#include #include #include #include -#include "ouster_msgs/msg/packet_msg.hpp" +#include "ouster_sensor_msgs/msg/packet_msg.hpp" #include "ouster_ros/os_point.h" 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 @@ -85,79 +82,11 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, * @param[in] pf the packet format * @return ROS sensor message with fields populated from the packet */ -sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, +sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_sensor_msgs::msg::PacketMsg& pm, const rclcpp::Time& timestamp, 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 @@ -179,6 +108,8 @@ geometry_msgs::msg::TransformStamped transform_to_tf_msg( * @param[in] frame the parent frame of the generated laser scan message * @param[in] lidar_mode lidar mode (width x frequency) * @param[in] ring selected ring to be published + * @param[in] pixel_shift_by_row pixel shifts by row + * @param[in] return_index index of return desired starting at 0 * @return ROS message suitable for publishing as a LaserScan */ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( @@ -186,7 +117,7 @@ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( const rclcpp::Time& timestamp, const std::string &frame, const ouster::sensor::lidar_mode lidar_mode, - const uint16_t ring, + const uint16_t ring, const std::vector& pixel_shift_by_row, const int return_index); namespace impl { @@ -203,10 +134,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 +149,14 @@ 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(); +} + +ouster::util::version parse_version(const std::string& fw_rev); } // namespace impl diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h index 1834bf64..4de4cbb5 100644 --- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h @@ -13,7 +13,7 @@ #include #include -#include "ouster_msgs/srv/get_metadata.hpp" +#include "ouster_sensor_msgs/srv/get_metadata.hpp" namespace ouster_ros { @@ -43,7 +43,7 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { protected: ouster::sensor::sensor_info info; - rclcpp::Service::SharedPtr get_metadata_srv; + rclcpp::Service::SharedPtr get_metadata_srv; std::string cached_metadata; rclcpp::Publisher::SharedPtr metadata_pub; }; 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/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index 51196283..39ebac82 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -67,6 +67,15 @@ use this parameter in conjunction with the SCAN flag and choose a value the range [0, sensor_beams_count)"/> + + @@ -93,6 +102,7 @@ + diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 649ba6b6..e0e4e20b 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -47,6 +47,15 @@ use this parameter in conjunction with the SCAN flag and choose a value the range [0, sensor_beams_count)"/> + + @@ -64,6 +73,7 @@ + diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index a96642f7..758e6479 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -63,6 +63,15 @@ use this parameter in conjunction with the SCAN flag and choose a value the range [0, sensor_beams_count)"/> + + @@ -84,6 +93,7 @@ + diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 6c1bf364..5222ce8c 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -65,6 +65,15 @@ use this parameter in conjunction with the SCAN flag and choose a value the range [0, sensor_beams_count)"/> + + @@ -90,6 +99,7 @@ + diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index a7da2b7a..a89fc875 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -71,6 +71,15 @@ use this parameter in conjunction with the SCAN flag and choose a value the range [0, sensor_beams_count)"/> + + @@ -92,6 +101,7 @@ + diff --git a/ouster-ros/ouster-sdk b/ouster-ros/ouster-sdk index d7307986..2898060f 160000 --- a/ouster-ros/ouster-sdk +++ b/ouster-ros/ouster-sdk @@ -1 +1 @@ -Subproject commit d73079865ebadd961d7af81b235bb34747f7cb6a +Subproject commit 2898060fb47a69b4fbf9807d3c17f6fdc8249379 diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index a8759d09..6e3aad22 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.10.3 + 0.12.2 Ouster ROS2 driver ouster developers BSD @@ -14,7 +14,7 @@ rclcpp_components rclcpp_lifecycle std_msgs - ouster_msgs + ouster_sensor_msgs sensor_msgs geometry_msgs tf2_ros diff --git a/ouster-ros/src/laser_scan_processor.h b/ouster-ros/src/laser_scan_processor.h index 63dae838..72096517 100644 --- a/ouster-ros/src/laser_scan_processor.h +++ b/ouster-ros/src/laser_scan_processor.h @@ -8,7 +8,7 @@ #pragma once -// 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" @@ -29,16 +29,28 @@ class LaserScanProcessor { : frame(frame_id), ld_mode(info.mode), ring_(ring), - scan_msgs(get_n_returns(info), - std::make_shared()), - post_processing_fn(func) {} + pixel_shift_by_row(info.format.pixel_shift_by_row), + scan_msgs(get_n_returns(info)), + post_processing_fn(func) { + for (size_t i = 0; i < scan_msgs.size(); ++i) + scan_msgs[i] = std::make_shared(); + + const auto fw = impl::parse_version(info.fw_rev); + if (fw.major == 2 && fw.minor < 4) { + std::transform(pixel_shift_by_row.begin(), + pixel_shift_by_row.end(), + pixel_shift_by_row.begin(), + [](auto c) { return c - 31; }); + } + } private: void process(const ouster::LidarScan& lidar_scan, uint64_t, const rclcpp::Time& msg_ts) { - for (int i = 0; i < static_cast(scan_msgs.size()); ++i) { - *scan_msgs[i] = lidar_scan_to_laser_scan_msg( - lidar_scan, msg_ts, frame, ld_mode, ring_, i); + for (size_t i = 0; i < scan_msgs.size(); ++i) { + *scan_msgs[i] = + lidar_scan_to_laser_scan_msg(lidar_scan, msg_ts, frame, ld_mode, + ring_, pixel_shift_by_row, i); } if (post_processing_fn) post_processing_fn(scan_msgs); @@ -61,6 +73,7 @@ class LaserScanProcessor { std::string frame; sensor::lidar_mode ld_mode; uint16_t ring_; + std::vector pixel_shift_by_row; OutputType scan_msgs; PostProcessingFn post_processing_fn; }; 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 a111242b..56d0b384 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,12 +15,7 @@ #include #include -#include -#include -#include -#include - -#include "ouster_msgs/msg/packet_msg.hpp" +#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" - -namespace sensor = ouster::sensor; -using ouster_msgs::msg::PacketMsg; +#include "point_cloud_processor_factory.h" namespace ouster_ros { +namespace sensor = ouster::sensor; +using ouster_sensor_msgs::msg::PacketMsg; + 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( @@ -141,12 +148,11 @@ class OusterCloud : public OusterProcessingNodeBase { processors.push_back(LaserScanProcessor::create( info, tf_bcast.lidar_frame_id(), scan_ring, [this](LaserScanProcessor::OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) - scan_pubs[i]->publish(*msgs[i]); + for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]); })); } - 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..6af4a235 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,15 @@ 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"); + } + + ~OusterDriver() override { + RCLCPP_DEBUG(get_logger(), "OusterDriver::~OusterDriver() called"); + halt(); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { @@ -45,7 +52,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 +65,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 +76,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( @@ -110,12 +126,11 @@ class OusterDriver : public OusterSensor { processors.push_back(LaserScanProcessor::create( info, tf_bcast.lidar_frame_id(), scan_ring, [this](LaserScanProcessor::OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) - scan_pubs[i]->publish(*msgs[i]); + for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]); })); } - 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 +165,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 9ea69425..0d80c7c1 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 @@ -20,12 +19,16 @@ #include #include #include +#include -namespace sensor = ouster::sensor; -using ouster_msgs::msg::PacketMsg; namespace ouster_ros { +namespace sensor = ouster::sensor; +using namespace ouster::util; +using ouster_sensor_msgs::msg::PacketMsg; + + bool is_legacy_lidar_profile(const sensor::sensor_info& info) { using sensor::UDPProfileLidar; return info.format.udp_profile_lidar == @@ -112,228 +115,42 @@ 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"); +version parse_version(const std::string& fw_rev) { + auto rgx = std::regex(R"(v(\d+).(\d+)\.(\d+))"); + std::smatch matches; + std::regex_search(fw_rev, matches, rgx); - // 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); -} + if (matches.size() < 4) return invalid_version; -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); + try { + return version{static_cast(stoul(matches[1])), + static_cast(stoul(matches[2])), + static_cast(stoul(matches[3]))}; + } catch (const std::exception&) { + return invalid_version; + } } -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, @@ -354,7 +171,8 @@ geometry_msgs::msg::TransformStamped transform_to_tf_msg( sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( const ouster::LidarScan& ls, const rclcpp::Time& timestamp, const std::string& frame, const ouster::sensor::lidar_mode ld_mode, - const uint16_t ring, const int return_index) { + const uint16_t ring, const std::vector& pixel_shift_by_row, + const int return_index) { sensor_msgs::msg::LaserScan msg; msg.header.stamp = timestamp; msg.header.frame_id = frame; @@ -380,10 +198,14 @@ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( const auto sg = signal.data(); msg.ranges.resize(ls.w); msg.intensities.resize(ls.w); - int idx = ls.w * ring + ls.w; - for (int i = 0; idx-- > ls.w * ring; ++i) { - msg.ranges[i] = rg[idx] * ouster::sensor::range_unit; - msg.intensities[i] = static_cast(sg[idx]); + + uint16_t u = ring; + for (auto v = 0; v < ls.w; ++v) { + auto v_shift = (v + ls.w - pixel_shift_by_row[u] + ls.w / 2) % ls.w; + auto src_idx = u * ls.w + v_shift; + auto tgt_idx = ls.w - 1 - v; + msg.ranges[tgt_idx] = rg[src_idx] * ouster::sensor::range_unit; + msg.intensities[tgt_idx] = static_cast(sg[src_idx]); } return msg; diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index c40e1a3a..d8fedb37 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -16,9 +16,9 @@ #include "os_sensor_node.h" -using ouster_msgs::msg::PacketMsg; -using ouster_msgs::srv::GetConfig; -using ouster_msgs::srv::SetConfig; +using ouster_sensor_msgs::msg::PacketMsg; +using ouster_sensor_msgs::srv::GetConfig; +using ouster_sensor_msgs::srv::SetConfig; using namespace std::chrono_literals; using namespace std::string_literals; @@ -36,20 +36,29 @@ OusterSensor::OusterSensor(const std::string& name, OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) : OusterSensor("os_sensor", options) {} +OusterSensor::~OusterSensor() { + RCLCPP_DEBUG(get_logger(), "OusterSensor::~OusterSensor() called"); + halt(); +} + +void OusterSensor::halt() { + stop_packet_processing_threads(); + stop_sensor_connection_thread(); +} + 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); } @@ -119,7 +128,7 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_cleanup( } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM( get_logger(), - "exception thrown durng cleanup, details: " << ex.what()); + "exception thrown during cleanup, details: " << ex.what()); return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -146,7 +155,7 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_shutdown( } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM( get_logger(), - "exception thrown durng cleanup, details: " << ex.what()); + "exception thrown during cleanup, details: " << ex.what()); return LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -324,7 +333,6 @@ void OusterSensor::reactivate_sensor(bool init_id_reset) { reset_last_init_id = init_id_reset; active_config.clear(); cached_metadata.clear(); - imu_packets_skip = lidar_packets_skip = true; auto request_transitions = std::vector{ lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; @@ -399,7 +407,7 @@ std::shared_ptr OusterSensor::create_sensor_client( std::shared_ptr cli; if (sensor::in_multicast(udp_dest)) { - // use the mtp_init_client to recieve data via multicast + // use the mtp_init_client to receive data via multicast // if mtp_main is true when sensor will be configured cli = sensor::mtp_init_client(hostname, config, mtp_dest, mtp_main); } else if (lidar_port != 0 && imu_port != 0) { @@ -701,7 +709,7 @@ void OusterSensor::handle_lidar_packet(sensor::client& cli, if (success) { read_lidar_packet_errors = 0; if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) { - // TODO: short circut reset if no breaking changes occured? + // TODO: short circuit reset if no breaking changes occured? RCLCPP_WARN(get_logger(), "sensor init_id has changed! reactivating.."); reactivate_sensor(); @@ -788,25 +796,22 @@ void OusterSensor::stop_sensor_connection_thread() { } void OusterSensor::start_packet_processing_threads() { - imu_packets_skip = false; imu_packets_processing_thread_active = true; imu_packets_processing_thread = std::make_unique([this]() { - auto read_packet = [this](const uint8_t* buffer) { - if (!imu_packets_skip) on_imu_packet_msg(buffer); - }; while (imu_packets_processing_thread_active) { - imu_packets->read(read_packet); + imu_packets->read_timeout([this](const uint8_t* buffer) { + if (buffer != nullptr) on_imu_packet_msg(buffer); + }, 1s); } RCLCPP_DEBUG(get_logger(), "imu_packets_processing_thread done."); }); - lidar_packets_skip = false; lidar_packets_processing_thread_active = true; lidar_packets_processing_thread = std::make_unique([this]() { while (lidar_packets_processing_thread_active) { - lidar_packets->read([this](const uint8_t* buffer) { - if (!lidar_packets_skip) on_lidar_packet_msg(buffer); - }); + lidar_packets->read_timeout([this](const uint8_t* buffer) { + if (buffer != nullptr) on_lidar_packet_msg(buffer); + }, 1s); } RCLCPP_DEBUG(get_logger(), "lidar_packets_processing_thread done."); }); diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index 25bdc33d..7008ee4d 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -20,9 +20,9 @@ #include #include #include -#include "ouster_msgs/msg/packet_msg.hpp" -#include "ouster_msgs/srv/get_config.hpp" -#include "ouster_msgs/srv/set_config.hpp" +#include "ouster_sensor_msgs/msg/packet_msg.hpp" +#include "ouster_sensor_msgs/srv/get_config.hpp" +#include "ouster_sensor_msgs/srv/set_config.hpp" #include "ouster_ros/visibility_control.h" #include "ouster_ros/os_sensor_node_base.h" @@ -39,6 +39,7 @@ class OusterSensor : public OusterSensorNodeBase { OUSTER_ROS_PUBLIC OusterSensor(const std::string& name, const rclcpp::NodeOptions& options); explicit OusterSensor(const rclcpp::NodeOptions& options); + ~OusterSensor() override; LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State& state); @@ -66,6 +67,8 @@ class OusterSensor : public OusterSensorNodeBase { virtual void cleanup(); + void halt(); + private: void declare_parameters(); @@ -145,13 +148,13 @@ class OusterSensor : public OusterSensorNodeBase { std::string mtp_dest; bool mtp_main; std::shared_ptr sensor_client; - ouster_msgs::msg::PacketMsg lidar_packet; - ouster_msgs::msg::PacketMsg imu_packet; - rclcpp::Publisher::SharedPtr lidar_packet_pub; - rclcpp::Publisher::SharedPtr imu_packet_pub; + ouster_sensor_msgs::msg::PacketMsg lidar_packet; + ouster_sensor_msgs::msg::PacketMsg imu_packet; + rclcpp::Publisher::SharedPtr lidar_packet_pub; + rclcpp::Publisher::SharedPtr imu_packet_pub; rclcpp::Service::SharedPtr reset_srv; - rclcpp::Service::SharedPtr get_config_srv; - rclcpp::Service::SharedPtr set_config_srv; + rclcpp::Service::SharedPtr get_config_srv; + rclcpp::Service::SharedPtr set_config_srv; std::shared_ptr> change_state_client; // TODO: implement & utilize a lock-free ring buffer in future @@ -162,11 +165,9 @@ class OusterSensor : public OusterSensorNodeBase { std::unique_ptr sensor_connection_thread; std::atomic imu_packets_processing_thread_active = {false}; - std::atomic imu_packets_skip; std::unique_ptr imu_packets_processing_thread; std::atomic lidar_packets_processing_thread_active = {false}; - std::atomic lidar_packets_skip; std::unique_ptr lidar_packets_processing_thread; bool force_sensor_reinit = false; diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp index 364de005..a238d974 100644 --- a/ouster-ros/src/os_sensor_node_base.cpp +++ b/ouster-ros/src/os_sensor_node_base.cpp @@ -13,7 +13,7 @@ #include namespace sensor = ouster::sensor; -using ouster_msgs::srv::GetMetadata; +using ouster_sensor_msgs::srv::GetMetadata; using sensor::UDPProfileLidar; namespace ouster_ros { diff --git a/ouster-ros/src/point_cloud_compose.h b/ouster-ros/src/point_cloud_compose.h new file mode 100644 index 00000000..8aa2a79f --- /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 specified 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 specified 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 knowing 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..fcbb075d --- /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_processor( + 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_processor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL: + return make_point_cloud_processor< + Point_RNG19_RFL8_SIG16_NIR16_DUAL>( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16: + return make_point_cloud_processor< + Point_RNG19_RFL8_SIG16_NIR16>( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8: + return make_point_cloud_processor( + 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_processor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "xyzi") { + return make_point_cloud_processor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "xyzir") { + return make_point_cloud_processor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "original") { + return make_point_cloud_processor( + 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-msgs/CMakeLists.txt b/ouster-sensor-msgs/CMakeLists.txt similarity index 93% rename from ouster-msgs/CMakeLists.txt rename to ouster-sensor-msgs/CMakeLists.txt index 50ef44c4..b43b1864 100644 --- a/ouster-msgs/CMakeLists.txt +++ b/ouster-sensor-msgs/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.5) -project(ouster_msgs) +project(ouster_sensor_msgs) find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) diff --git a/ouster-msgs/msg/PacketMsg.msg b/ouster-sensor-msgs/msg/PacketMsg.msg similarity index 100% rename from ouster-msgs/msg/PacketMsg.msg rename to ouster-sensor-msgs/msg/PacketMsg.msg diff --git a/ouster-msgs/package.xml b/ouster-sensor-msgs/package.xml similarity index 92% rename from ouster-msgs/package.xml rename to ouster-sensor-msgs/package.xml index 8a8c7fac..d2087191 100644 --- a/ouster-msgs/package.xml +++ b/ouster-sensor-msgs/package.xml @@ -1,8 +1,8 @@ - ouster_msgs - 0.2.0 + ouster_sensor_msgs + 0.12.2 ouster_ros message and service definitions ouster developers BSD diff --git a/ouster-msgs/srv/GetConfig.srv b/ouster-sensor-msgs/srv/GetConfig.srv similarity index 100% rename from ouster-msgs/srv/GetConfig.srv rename to ouster-sensor-msgs/srv/GetConfig.srv diff --git a/ouster-msgs/srv/GetMetadata.srv b/ouster-sensor-msgs/srv/GetMetadata.srv similarity index 100% rename from ouster-msgs/srv/GetMetadata.srv rename to ouster-sensor-msgs/srv/GetMetadata.srv diff --git a/ouster-msgs/srv/SetConfig.srv b/ouster-sensor-msgs/srv/SetConfig.srv similarity index 100% rename from ouster-msgs/srv/SetConfig.srv rename to ouster-sensor-msgs/srv/SetConfig.srv