diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4b36cef0..c387ee8d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog ========= +[unreleased] +============ +* added the ability to customize the published point clouds(s) to velodyne point cloud format and + other common pcl point types. +* ouster_image_nodelet can operate separately from ouster_cloud_nodelet. + + ouster_ros v0.10.0 ================== diff --git a/CMakeLists.txt b/CMakeLists.txt index f64c0770..3ace79ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,13 @@ add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp) # ==== Test ==== if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}_test tests/ring_buffer_test.cpp) + catkin_add_gtest(${PROJECT_NAME}_test + tests/ring_buffer_test.cpp + src/os_ros.cpp + tests/point_accessor_test.cpp + tests/point_transform_test.cpp + tests/point_cloud_compose_test.cpp + ) target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES}) endif() diff --git a/include/ouster_ros/common_point_types.h b/include/ouster_ros/common_point_types.h new file mode 100644 index 00000000..2bfea7a8 --- /dev/null +++ b/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/include/ouster_ros/os_point.h b/include/ouster_ros/os_point.h index 8b745ac6..e319220d 100644 --- a/include/ouster_ros/os_point.h +++ b/include/ouster_ros/os_point.h @@ -10,36 +10,79 @@ #include -#include -#include - -#include - namespace ouster_ros { -struct EIGEN_ALIGN16 Point { +// The default/original represntation of the point cloud since the driver +// inception. This shouldn't be confused with Point_LEGACY which provides exact +// mapping of the fields of Ouster LidarScan of the Legacy Profile, copying the +// the same order and using the same bit representation. For example, this Point +// struct uses float data type to represent intensity (aka signal); however, the +// sensor sends the signal channel either as UINT16 or UINT32 depending on the +// active udp lidar profile. +struct EIGEN_ALIGN16 _Point { PCL_ADD_POINT4D; - float intensity; + float intensity; // equivalent to signal uint32_t t; uint16_t reflectivity; - uint16_t ring; - uint16_t ambient; + uint16_t ring; // equivalent to channel + uint16_t ambient; // equivalent to near_ir uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace ouster_ros + +struct Point : public _Point { + + inline Point(const _Point& pt) + { + x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f; + intensity = pt.intensity; + t = pt.t; + reflectivity = pt.reflectivity; + ring = pt.ring; + ambient = pt.ambient; + range = pt.range; + } + + inline Point() + { + x = y = z = 0.0f; data[3] = 1.0f; + intensity = 0.0f; + t = 0; + reflectivity = 0; + ring = 0; + ambient = 0; + range = 0; + } + + inline const auto as_tuple() const { + return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range); + } + + inline auto as_tuple() { + return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range); + } + + template + inline auto& get() { + return std::get(as_tuple()); + } +}; + +} // namespace ouster_ros // clang-format off + +// DEFAULT/ORIGINAL POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) - // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) (std::uint16_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) + // clang-format on diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 1eb70146..d053732b 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -29,8 +29,6 @@ namespace ouster_ros { namespace sensor = ouster::sensor; -using Cloud = pcl::PointCloud; -using ns = std::chrono::nanoseconds; /** * Checks sensor_info if it currently represents a legacy udp lidar profile @@ -40,13 +38,13 @@ using ns = std::chrono::nanoseconds; bool is_legacy_lidar_profile(const sensor::sensor_info& info); /** - * Gets the number of point cloud returns that this sensor_info object - * represents + * Gets the number of point cloud returns that this sensor_info object represents * @param[in] info sensor_info * @return number of returns */ int get_n_returns(const sensor::sensor_info& info); + /** * Gets the number beams based on supplied sensor_info * @param[in] info sensor_info @@ -88,74 +86,6 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, 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::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, - const ros::Time& timestamp, - const std::string& frame); - /** * Convert transformation matrix return by sensor to ROS transform * @param[in] mat transformation matrix return by sensor @@ -179,9 +109,12 @@ geometry_msgs::TransformStamped transform_to_tf_msg( * @return ROS message suitable for publishing as a LaserScan */ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( - const ouster::LidarScan& ls, const ros::Time& timestamp, - const std::string& frame, const ouster::sensor::lidar_mode lidar_mode, - const uint16_t ring, const int return_index); + const ouster::LidarScan& ls, + const ros::Time& timestamp, + const std::string &frame, + const ouster::sensor::lidar_mode lidar_mode, + const uint16_t ring, + const int return_index); namespace impl { sensor::ChanField suitable_return(sensor::ChanField input_field, bool second); @@ -197,10 +130,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 +151,13 @@ inline ros::Time ts_to_ros_time(uint64_t ts) { return t; } -} // namespace impl +std::set parse_tokens(const std::string& input, char delim); + +inline bool check_token(const std::set& tokens, + const std::string& token) { + return tokens.find(token) != tokens.end(); +} + +} // namespace impl } // namespace ouster_ros diff --git a/include/ouster_ros/sensor_point_types.h b/include/ouster_ros/sensor_point_types.h new file mode 100644 index 00000000..7d73e6a1 --- /dev/null +++ b/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/CMakeLists.txt b/ouster-ros/CMakeLists.txt new file mode 100644 index 00000000..b56cf112 --- /dev/null +++ b/ouster-ros/CMakeLists.txt @@ -0,0 +1,231 @@ +cmake_minimum_required(VERSION 3.10.0) + +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/ouster-sdk/cmake) +include(DefaultBuildType) + +# ==== Project Name ==== +project(ouster_ros) + +# ==== Requirements ==== +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(ouster_sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(pcl_conversions REQUIRED) +find_package(tf2_eigen REQUIRED) + +# ==== Options ==== +add_compile_options(-Wall -Wextra) +if(NOT DEFINED CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) + +set(_ouster_ros_INCLUDE_DIRS + include + ouster-sdk/ouster_client/include + ouster-sdk/ouster_client/include/optional-lite +) + +# ==== Libraries ==== +# Build static libraries and bundle them into ouster_ros using the `--whole-archive` flag. This is +# necessary because catkin doesn't interoperate easily with target-based cmake builds. Object +# libraries are the recommended way to do this, but require >=3.13 to propagate usage requirements. +set(_SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS}) +set(BUILD_SHARED_LIBS OFF) + +option(BUILD_VIZ "Turn off building VIZ" OFF) +option(BUILD_PCAP "Turn off building PCAP" OFF) +find_package(OusterSDK REQUIRED) + +set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) + +# catkin adds all include dirs to a single variable, don't try to use targets +include_directories(${_ouster_ros_INCLUDE_DIRS}) + +# use only MPL-licensed parts of eigen +add_definitions(-DEIGEN_MPL2_ONLY) + +add_library(ouster_ros_library SHARED + src/os_ros.cpp +) + +set(ouster_ros_library_deps + rclcpp + sensor_msgs + geometry_msgs + ouster_sensor_msgs + pcl_conversions + tf2 + tf2_eigen +) + +ament_target_dependencies(ouster_ros_library + ${ouster_ros_library_deps} +) + +target_link_libraries(ouster_ros_library + # PUBLIC (unsupported) + ouster_build + pcl_common + # PRIVATE (unsupported) + -Wl,--whole-archive ouster_client -Wl,--no-whole-archive +) + +# helper method to construct ouster-ros components +function(create_ros2_component + component_lib_name + src_list + additonal_dependencies +) + + add_library(${component_lib_name} SHARED + ${src_list}) + target_compile_definitions(${component_lib_name} + PRIVATE + "OUSTER_ROS_BUILDING_DLL" + ) + + ament_target_dependencies(${component_lib_name} + rclcpp + class_loader + rclcpp_components + rclcpp_lifecycle + std_msgs + sensor_msgs + geometry_msgs + ${additonal_dependencies} + ) + + target_link_libraries(${component_lib_name} + ouster_ros_library + ${cpp_typesupport_target} + ) + +endfunction() + + +# ==== os_sensor_component ==== +create_ros2_component(os_sensor_component + "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp" + "std_srvs" +) +rclcpp_components_register_node(os_sensor_component + PLUGIN "ouster_ros::OusterSensor" + EXECUTABLE os_sensor +) + +# ==== os_replay_component ==== +create_ros2_component(os_replay_component + "src/os_sensor_node_base.cpp;src/os_replay_node.cpp" + "" +) +rclcpp_components_register_node(os_replay_component + PLUGIN "ouster_ros::OusterReplay" + EXECUTABLE os_replay +) + +# ==== os_cloud_component ==== +create_ros2_component(os_cloud_component + "src/os_processing_node_base.cpp;src/os_cloud_node.cpp" + "tf2_ros" +) +rclcpp_components_register_node(os_cloud_component + PLUGIN "ouster_ros::OusterCloud" + EXECUTABLE os_cloud +) + +# ==== os_image_component ==== +create_ros2_component(os_image_component + "src/os_processing_node_base.cpp;src/os_image_node.cpp" + "" +) +rclcpp_components_register_node(os_image_component + PLUGIN "ouster_ros::OusterImage" + EXECUTABLE os_image +) + +# ==== 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" +) +rclcpp_components_register_node(os_driver_component + PLUGIN "ouster_ros::OusterDriver" + EXECUTABLE os_driver +) + + +# ==== Test ==== +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + 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() + + +# ==== Install ==== +install( + TARGETS + ouster_ros_library + os_sensor_component + os_replay_component + os_cloud_component + os_image_component + os_driver_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY + ${_ouster_ros_INCLUDE_DIRS} + DESTINATION + include/${PROJECT_NAME} +) + +install( + FILES + ../LICENSE + DESTINATION + share/${PROJECT_NAME} +) + +install( + DIRECTORY + launch + config + DESTINATION + share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_dependencies(rosidl_default_runtime) +ament_export_libraries(ouster_ros_library) +ament_export_dependencies(${ouster_ros_library_deps}) +ament_package() diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp new file mode 100644 index 00000000..506ac711 --- /dev/null +++ b/ouster-ros/src/os_driver_node.cpp @@ -0,0 +1,208 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_driver.cpp + * @brief This node combines the capabilities of os_sensor, os_cloud and + * os_image into a single ROS node/component + */ + +// 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 "os_sensor_node.h" + +#include "os_static_transforms_broadcaster.h" +#include "imu_packet_handler.h" +#include "lidar_packet_handler.h" +#include "point_cloud_processor.h" +#include "laser_scan_processor.h" +#include "image_processor.h" +#include "point_cloud_processor_factory.h" + +namespace ouster_ros { + +namespace sensor = ouster::sensor; + +class OusterDriver : public OusterSensor { + public: + OUSTER_ROS_PUBLIC + explicit OusterDriver(const rclcpp::NodeOptions& options) + : 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("point_type", "original"); + } + + virtual void on_metadata_updated(const sensor::sensor_info& info) override { + OusterSensor::on_metadata_updated(info); + tf_bcast.broadcast_transforms(info); + } + + virtual void create_publishers() override { + auto proc_mask = get_parameter("proc_mask").as_string(); + auto tokens = impl::parse_tokens(proc_mask, '|'); + + bool use_system_default_qos = + get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); + auto selected_qos = + use_system_default_qos ? system_default_qos : sensor_data_qos; + + auto timestamp_mode = get_parameter("timestamp_mode").as_string(); + auto ptp_utc_tai_offset = + get_parameter("ptp_utc_tai_offset").as_double(); + + if (impl::check_token(tokens, "IMU")) { + imu_pub = + create_publisher("imu", selected_qos); + imu_packet_handler = ImuPacketHandler::create_handler( + info, tf_bcast.imu_frame_id(), timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); + } + + int num_returns = get_n_returns(info); + + std::vector processors; + 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); + } + + 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) { + NODELET_WARN_STREAM( + "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8"); + } + } + + if (impl::check_token(tokens, "SCAN")) { + scan_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + scan_pubs[i] = create_publisher( + topic_for_return("scan", i), selected_qos); + } + + // TODO: avoid duplication in os_cloud_node + int beams_count = static_cast(get_beams_count(info)); + int scan_ring = get_parameter("scan_ring").as_int(); + scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); + if (scan_ring != get_parameter("scan_ring").as_int()) { + NODELET_WARN_STREAM( + "scan ring is set to a value that exceeds available range" + "please choose a value between [0, " + << beams_count + << "], " + "ring value clamped to: " + << scan_ring); + } + + 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]); + })); + } + + if (impl::check_token(tokens, "IMG")) { + 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 = num_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); + } + + processors.push_back(ImageProcessor::create( + info, 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); + } + })); + } + + 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)); + } + + virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { + if (lidar_packet_handler) lidar_packet_handler(raw_lidar_packet); + } + + virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override { + if (imu_packet_handler) + imu_pub->publish(imu_packet_handler(raw_imu_packet)); + } + + virtual void cleanup() override { + imu_packet_handler = nullptr; + lidar_packet_handler = nullptr; + imu_pub.reset(); + for (auto p : lidar_pubs) p.reset(); + for (auto p : scan_pubs) p.reset(); + for (auto p : image_pubs) p.second.reset(); + OusterSensor::cleanup(); + } + + private: + OusterStaticTransformsBroadcaster tf_bcast; + + rclcpp::Publisher::SharedPtr imu_pub; + std::vector::SharedPtr> + lidar_pubs; + std::vector::SharedPtr> + scan_pubs; + std::map::SharedPtr> + image_pubs; + ImuPacketHandler::HandlerType imu_packet_handler; + LidarPacketHandler::HandlerType lidar_packet_handler; +}; + +} // namespace ouster_ros + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(ouster_ros::OusterDriver) diff --git a/src/image_processor.h b/src/image_processor.h index e3eefd8f..68a2d263 100644 --- a/src/image_processor.h +++ b/src/image_processor.h @@ -18,10 +18,11 @@ #include "ouster/image_processing.h" +namespace ouster_ros { + namespace sensor = ouster::sensor; namespace viz = ouster::viz; -namespace ouster_ros { class ImageProcessor { public: using OutputType = diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index 7effc6ba..9c683fe9 100644 --- a/src/lidar_packet_handler.h +++ b/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/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 75deb21f..c1f7f3f8 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -20,22 +20,19 @@ #include #include -#include -#include -#include -#include - #include "ouster_ros/PacketMsg.h" #include "os_transforms_broadcaster.h" #include "imu_packet_handler.h" #include "lidar_packet_handler.h" #include "point_cloud_processor.h" #include "laser_scan_processor.h" - -namespace sensor = ouster::sensor; +#include "point_cloud_processor_factory.h" namespace ouster_ros { +namespace sensor = ouster::sensor; +using ouster_ros::PacketMsg; + class OusterCloud : public nodelet::Nodelet { public: OusterCloud() : tf_bcast(getName()) {} @@ -91,14 +88,14 @@ class OusterCloud : public nodelet::Nodelet { void create_publishers_subscribers(const sensor::sensor_info& info) { auto& pnh = getPrivateNodeHandle(); auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"}); - auto tokens = parse_tokens(proc_mask, '|'); + auto tokens = impl::parse_tokens(proc_mask, '|'); auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); auto& nh = getNodeHandle(); - if (check_token(tokens, "IMU")) { + if (impl::check_token(tokens, "IMU")) { imu_pub = nh.advertise("imu", 100); imu_packet_handler = ImuPacketHandler::create_handler( info, tf_bcast.imu_frame_id(), timestamp_mode, @@ -115,41 +112,52 @@ class OusterCloud : public nodelet::Nodelet { 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] = nh.advertise( topic_for_return("points", i), 10); } - 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) { - if (msgs[i]->header.stamp > last_msg_ts) - last_msg_ts = msgs[i]->header.stamp; - lidar_pubs[i].publish(*msgs[i]); + auto point_type = pnh.param("point_type", std::string{"original"}); + 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) { + if (msgs[i]->header.stamp > last_msg_ts) + last_msg_ts = msgs[i]->header.stamp; + 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) { + NODELET_WARN_STREAM( + "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] = nh.advertise( topic_for_return("scan", i), 10); } - // TODO: avoid duplication in os_cloud_node + // TODO: avoid this duplication in os_cloud_node int beams_count = static_cast(get_beams_count(info)); int scan_ring = pnh.param("scan_ring", 0); scan_ring = std::min(std::max(scan_ring, 0), beams_count - 1); if (scan_ring != pnh.param("scan_ring", 0)) { NODELET_WARN_STREAM( - "scan ring is set to a value that exceeds available range" - "please choose a value between [0, " << beams_count << - "], ring value clamped to: " << scan_ring); + "scan ring is set to a value that exceeds available range " + "please choose a value between [0, " << beams_count << "], " + "ring value clamped to: " << scan_ring); } processors.push_back(LaserScanProcessor::create( @@ -163,7 +171,7 @@ class OusterCloud : public nodelet::Nodelet { })); } - 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/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 0d56db5a..bfe84927 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -24,6 +24,7 @@ #include "point_cloud_processor.h" #include "laser_scan_processor.h" #include "image_processor.h" +#include "point_cloud_processor_factory.h" namespace sensor = ouster::sensor; @@ -52,14 +53,14 @@ class OusterDriver : public OusterSensor { auto& pnh = getPrivateNodeHandle(); auto proc_mask = pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"}); - auto tokens = parse_tokens(proc_mask, '|'); + auto tokens = impl::parse_tokens(proc_mask, '|'); auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); auto& nh = getNodeHandle(); - if (check_token(tokens, "IMU")) { + if (impl::check_token(tokens, "IMU")) { imu_pub = nh.advertise("imu", 100); imu_packet_handler = ImuPacketHandler::create_handler( info, tf_bcast.imu_frame_id(), timestamp_mode, @@ -69,24 +70,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] = nh.advertise( topic_for_return("points", i), 10); } - 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 = pnh.param("point_type", std::string{"original"}); + 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) { + NODELET_WARN_STREAM( + "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] = nh.advertise( @@ -113,7 +122,7 @@ class OusterDriver : public OusterSensor { })); } - if (check_token(tokens, "IMG")) { + if (impl::check_token(tokens, "IMG")) { const std::map channel_field_topic_map_1{ {sensor::ChanField::RANGE, "range_image"}, @@ -147,8 +156,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/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index 76fe5e70..736fae69 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -18,29 +18,17 @@ // clang-format on #include -#include -#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include "lidar_packet_handler.h" +#include "image_processor.h" -#include "ouster/image_processing.h" +namespace ouster_ros { namespace sensor = ouster::sensor; -namespace viz = ouster::viz; - -using pixel_type = uint16_t; -const size_t pixel_value_max = std::numeric_limits::max(); +using ouster_ros::PacketMsg; -namespace ouster_ros { class OusterImage : public nodelet::Nodelet { private: @@ -54,155 +42,77 @@ class OusterImage : public nodelet::Nodelet { "metadata", 1, &OusterImage::metadata_handler, this); } + private: void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) { NODELET_INFO("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); - } - - 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}; + create_publishers_subscribers(get_n_returns(info)); } - void create_publishers(int n_returns) { - auto& nh = getNodeHandle(); - nearir_image_pub = - nh.advertise("nearir_image", 100); - - ros::Publisher a_pub; - for (int i = 0; i < n_returns; ++i) { - a_pub = nh.advertise( - topic_for_return("range_image", i), 100); - range_image_pubs.push_back(a_pub); - - a_pub = nh.advertise( - topic_for_return("signal_image", i), 100); - signal_image_pubs.push_back(a_pub); - - a_pub = nh.advertise( - topic_for_return("reflec_image", i), 100); - reflec_image_pubs.push_back(a_pub); - } - } + void create_publishers_subscribers(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& pnh = getPrivateNodeHandle(); + auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); + double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); + + 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; - void create_subscriptions(int n_returns) { auto& nh = getNodeHandle(); - pc_subs.resize(n_returns); - for (int i = 0; i < n_returns; ++i) { - pc_subs[i] = nh.subscribe( - topic_for_return("points", i), 100, - [this, i](const sensor_msgs::PointCloud2::ConstPtr msg) { - point_cloud_handler(msg, i); - }); - } - } - void point_cloud_handler(const sensor_msgs::PointCloud2::ConstPtr& 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; - } + for (auto it = which_map->begin(); it != which_map->end(); ++it) { + image_pubs[it->first] = + nh.advertise(it->second, 100); } - 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(nearir_image); - } - - // publish at return index - range_image_pubs[return_index].publish(range_image); - signal_image_pubs[return_index].publish(signal_image); - reflec_image_pubs[return_index].publish(reflec_image); - } - - static sensor_msgs::ImagePtr make_image_msg(size_t H, size_t W, - const ros::Time& stamp, - const std::string& frame) { - auto msg = boost::make_shared(); - 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; + 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 = nh.subscribe( + "lidar_packets", 100, + [this](const PacketMsg::ConstPtr msg) { + lidar_packet_handler(msg->buf.data()); + }); } private: ros::Subscriber metadata_sub; - ros::Publisher nearir_image_pub; - std::vector range_image_pubs; - std::vector signal_image_pubs; - std::vector reflec_image_pubs; - std::vector pc_subs; - sensor::sensor_info info; - ouster_ros::Cloud cloud; - viz::AutoExposure nearir_ae, signal_ae, reflec_ae; - viz::BeamUniformityCorrector nearir_buc; -}; + ros::Subscriber lidar_packet_sub; + std::map image_pubs; + LidarPacketHandler::HandlerType lidar_packet_handler; +}; } // namespace ouster_ros PLUGINLIB_EXPORT_CLASS(ouster_ros::OusterImage, nodelet::Nodelet) diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 63e88593..0dd13f67 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -6,13 +6,12 @@ * @brief A utilty file that contains helper methods */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" // clang-format on -#include #include #include @@ -112,228 +111,25 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { } } -} // 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]), - }; - } - } -} +// TODO: move to a separate file +std::set parse_tokens(const std::string& input, char delim) { -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(); + std::set tokens; + std::stringstream ss(input); + std::string token; - 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 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]), - }; - } - } -} - -void scan_to_cloud_f(ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, - std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index) { - bool second = (return_index == 1); - - assert(cloud.width == static_cast(ls.w) && - cloud.height == static_cast(ls.h) && - "point cloud and lidar scan size mismatch"); - - // across supported lidar profiles range is always 32-bit - auto range_channel_field = - second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; - ouster::img_t range = ls.field(range_channel_field); - - ouster::img_t reflectivity = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - ouster::img_t signal = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t near_ir = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::cartesianT(points, range, lut_direction, lut_offset); - copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir, - signal); + return tokens; } -void scan_to_cloud_f(ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, - int return_index) { - bool second = (return_index == 1); - - assert(cloud.width == static_cast(ls.w) && - cloud.height == static_cast(ls.h) && - "point cloud and lidar scan size mismatch"); - - // across supported lidar profiles range is always 32-bit - auto range_channel_field = - second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; - ouster::img_t range = ls.field(range_channel_field); - - ouster::img_t reflectivity = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - ouster::img_t signal = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t near_ir = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::cartesianT(points, range, lut_direction, lut_offset); - - copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir, - signal); -} - -void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, - ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, - uint64_t scan_ts, const ouster::LidarScan& ls, - const std::vector& pixel_shift_by_row, - int return_index) { - bool second = (return_index == 1); - - assert(cloud.width == static_cast(ls.w) && - cloud.height == static_cast(ls.h) && - "point cloud and lidar scan size mismatch"); - - // across supported lidar profiles range is always 32-bit - auto range_channel_field = - second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; - ouster::img_t range = ls.field(range_channel_field); - - ouster::img_t reflectivity = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); - - ouster::img_t signal = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::SIGNAL, second), ls); - - ouster::img_t near_ir = impl::get_or_fill_zero( - impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls); - - ouster::cartesianT(points, range, lut_direction, lut_offset); - - copy_scan_to_cloud_destaggered(cloud, ls, scan_ts, points, range, - reflectivity, near_ir, signal, - pixel_shift_by_row); -} - -sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, - const ros::Time& timestamp, - const std::string& frame) { - sensor_msgs::PointCloud2 msg{}; - pcl::toROSMsg(cloud, msg); - msg.header.frame_id = frame; - msg.header.stamp = timestamp; - return msg; -} +} // namespace impl geometry_msgs::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index e95d8b35..18a24bab 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include "ouster_ros/PacketMsg.h" diff --git a/src/os_transforms_broadcaster.h b/src/os_transforms_broadcaster.h index ca6f12d2..c4b6b215 100644 --- a/src/os_transforms_broadcaster.h +++ b/src/os_transforms_broadcaster.h @@ -10,6 +10,7 @@ #include #include +#include namespace ouster_ros { diff --git a/src/point_cloud_compose.h b/src/point_cloud_compose.h new file mode 100644 index 00000000..c6fed507 --- /dev/null +++ b/src/point_cloud_compose.h @@ -0,0 +1,161 @@ +#pragma once + +#include +#include + +#include "ouster_ros/os_point.h" +#include "ouster_ros/sensor_point_types.h" +#include "ouster_ros/common_point_types.h" + +#include "point_meta_helpers.h" +#include "point_transform.h" + +namespace ouster_ros { + +using ouster::sensor::ChanFieldType; + +template +struct TypeSelector { /*undefined*/ +}; + +template <> +struct TypeSelector { + typedef uint8_t type; +}; + +template <> +struct TypeSelector { + typedef uint16_t type; +}; + +template <> +struct TypeSelector { + typedef uint32_t type; +}; + +template <> +struct TypeSelector { + typedef uint64_t type; +}; + +/** + * @brief constructs a suitable tuple at compile time that can store a reference + * to all the fields of a specific LidarScan object (without conversion) + * according to the information specificed by the ChanFieldTable. + */ +template & Table> +constexpr auto make_lidar_scan_tuple() { + if constexpr (Index < N) { + using ElementType = typename TypeSelector::type; + return std::tuple_cat( + std::make_tuple(static_cast(0)), + std::move(make_lidar_scan_tuple())); + } else { + return std::make_tuple(); + } +} + +/** + * @brief maps the fields of a LidarScan object to the elements of the supplied + * tuple in the same order. + */ +template & Table, + typename Tuple> +void map_lidar_scan_fields_to_tuple(Tuple& tp, const ouster::LidarScan& ls) { + static_assert( + std::tuple_size_v == N, + "target tuple size has a different size from the channel field table"); + if constexpr (Index < N) { + using FieldType = typename TypeSelector::type; + using ElementType = std::remove_const_t< + std::remove_pointer_t>>; + static_assert(std::is_same_v, + "tuple element, field element types mismatch!"); + std::get(tp) = ls.field(Table[Index].first).data(); + map_lidar_scan_fields_to_tuple(tp, ls); + } +} + +/** + * @brief constructs a suitable tuple at compile time that can store a reference + * to all the fields of a specific LidarScan object (without conversion) + * according to the information specificed by the ChanFieldTable and directly + * maps the fields of the supplied LidarScan to the constructed tuple before + * returning. + * @param[in] ls LidarScan + */ +template & Table> +constexpr auto make_lidar_scan_tuple(const ouster::LidarScan& ls) { + auto tp = make_lidar_scan_tuple<0, N, Table>(); + map_lidar_scan_fields_to_tuple<0, N, Table>(tp, ls); + return tp; +} + +/** + * @brief copies field values from LidarScan fields combined as a tuple into the + * the corresponding elements of the input point pt. + * @param[out] pt point to copy values into. + * @param[in] tp tuple containing arrays to copy LidarScan field values from. + * @param[in] idx index of the point to be copied. + * @remark this method is to be used mainly with sensor native point types. + */ +template +void copy_lidar_scan_fields_to_point(PointT& pt, const Tuple& tp, int idx) { + if constexpr (Index < std::tuple_size_v) { + point::get<5 + Index>(pt) = std::get(tp)[idx]; + copy_lidar_scan_fields_to_point(pt, tp, idx); + } else { + unused_variable(pt); + unused_variable(tp); + unused_variable(idx); + } +} + +template +using Cloud = pcl::PointCloud; + +template & PROFILE, typename PointT, + typename PointS> +void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, + PointS& staging_point, + const ouster::PointsF& points, + uint64_t scan_ts, const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row) { + auto ls_tuple = make_lidar_scan_tuple<0, N, PROFILE>(ls); + auto timestamp = ls.timestamp(); + + for (auto u = 0; u < ls.h; u++) { + for (auto v = 0; v < ls.w; v++) { + const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w; + auto ts = timestamp[v_shift]; + ts = ts > scan_ts ? ts - scan_ts : 0UL; + const auto src_idx = u * ls.w + v_shift; + const auto tgt_idx = u * ls.w + v; + const auto xyz = points.row(src_idx); + // if target point and staging point has matching type bind the + // target directly and avoid performing transform_point at the end + auto& pt = CondBinaryBind>::run( + cloud.points[tgt_idx], staging_point); + // all native point types have x, y, z, t and ring values + pt.x = static_cast(xyz(0)); + pt.y = static_cast(xyz(1)); + pt.z = static_cast(xyz(2)); + // TODO: in the future we could probably skip copying t and ring + // values if knowning before hand that the target point cloud does + // not have a field to hold the timestamp or a ring for example the + // case of pcl::PointXYZ or pcl::PointXYZI. + pt.t = static_cast(ts); + pt.ring = static_cast(u); + copy_lidar_scan_fields_to_point<0>(pt, ls_tuple, src_idx); + // only perform point transform operation when PointT, and PointS + // don't match + CondBinaryOp>::run( + cloud.points[tgt_idx], staging_point, + [](auto& tgt_pt, const auto& src_pt) { + point::transform(tgt_pt, src_pt); + }); + } + } +} + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h index f8e0fa0f..593e8d7d 100644 --- a/src/point_cloud_processor.h +++ b/src/point_cloud_processor.h @@ -14,25 +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 = @@ -52,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::PointCloud2& cloud) { // TODO: remove the staging step in the future pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); @@ -62,9 +76,13 @@ class PointCloudProcessor { void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const ros::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]); pc_msgs[i]->header.stamp = msg_ts; pc_msgs[i]->header.frame_id = frame; @@ -77,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 ros::Time& msg_ts) { @@ -98,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/src/point_cloud_processor_factory.h b/src/point_cloud_processor_factory.h new file mode 100644 index 00000000..859e92da --- /dev/null +++ b/src/point_cloud_processor_factory.h @@ -0,0 +1,154 @@ +#pragma once + +#include "point_cloud_processor.h" + +namespace ouster_ros { + +namespace sensor = ouster::sensor; +using sensor::UDPProfileLidar; + +class PointCloudProcessorFactory { + template + static typename PointCloudProcessor::ScanToCloudFn + make_scan_to_cloud_fn(const sensor::sensor_info& info) { + switch (info.format.udp_profile_lidar) { + case UDPProfileLidar::PROFILE_LIDAR_LEGACY: + return [](ouster_ros::Cloud& cloud, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { + unused_variable(return_index); + Point_LEGACY staging_pt; + scan_to_cloud_f_destaggered( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + }; + + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL: + return [](ouster_ros::Cloud& cloud, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { + Point_RNG19_RFL8_SIG16_NIR16_DUAL staging_pt; + if (return_index == 0) { + scan_to_cloud_f_destaggered< + Profile_RNG19_RFL8_SIG16_NIR16_DUAL.size(), + Profile_RNG19_RFL8_SIG16_NIR16_DUAL>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + } else { + scan_to_cloud_f_destaggered< + Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN + .size(), + Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + } + }; + + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16: + return [](ouster_ros::Cloud& cloud, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { + unused_variable(return_index); + Point_RNG19_RFL8_SIG16_NIR16 staging_pt; + scan_to_cloud_f_destaggered< + Profile_RNG19_RFL8_SIG16_NIR16.size(), + Profile_RNG19_RFL8_SIG16_NIR16>(cloud, staging_pt, + points, scan_ts, ls, + pixel_shift_by_row); + }; + + case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8: + return [](ouster_ros::Cloud& cloud, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { + unused_variable(return_index); + Point_RNG15_RFL8_NIR8 staging_pt; + scan_to_cloud_f_destaggered( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row); + }; + + default: + throw std::runtime_error("unsupported udp_profile_lidar"); + } + } + + template + static LidarScanProcessor make_point_cloud_procssor( + const sensor::sensor_info& info, const std::string& frame, + bool apply_lidar_to_sensor_transform, + PointCloudProcessor_PostProcessingFn post_processing_fn) { + auto scan_to_cloud_fn = make_scan_to_cloud_fn(info); + return PointCloudProcessor::create( + info, frame, apply_lidar_to_sensor_transform, scan_to_cloud_fn, + post_processing_fn); + } + + public: + static bool point_type_requires_intensity(const std::string& point_type) { + return point_type == "xyzi" || point_type == "xyzir" || + point_type == "original"; + } + + static LidarScanProcessor create_point_cloud_processor( + const std::string& point_type, const sensor::sensor_info& info, + const std::string& frame, bool apply_lidar_to_sensor_transform, + PointCloudProcessor_PostProcessingFn post_processing_fn) { + if (point_type == "native") { + switch (info.format.udp_profile_lidar) { + case UDPProfileLidar::PROFILE_LIDAR_LEGACY: + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL: + return make_point_cloud_procssor< + Point_RNG19_RFL8_SIG16_NIR16_DUAL>( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16: + return make_point_cloud_procssor< + Point_RNG19_RFL8_SIG16_NIR16>( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8: + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + default: + // TODO: implement fallback? + throw std::runtime_error("unsupported udp_profile_lidar"); + } + } else if (point_type == "xyz") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "xyzi") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "xyzir") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } else if (point_type == "original") { + return make_point_cloud_procssor( + info, frame, apply_lidar_to_sensor_transform, + post_processing_fn); + } + + throw std::runtime_error( + "Un-supported point type used: " + point_type + "!"); + } +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/src/point_meta_helpers.h b/src/point_meta_helpers.h new file mode 100644 index 00000000..75fc6f8f --- /dev/null +++ b/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/src/point_transform.h b/src/point_transform.h new file mode 100644 index 00000000..128fc024 --- /dev/null +++ b/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/tests/point_accessor_test.cpp b/tests/point_accessor_test.cpp new file mode 100644 index 00000000..e69a3b91 --- /dev/null +++ b/tests/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/tests/point_cloud_compose_test.cpp b/tests/point_cloud_compose_test.cpp new file mode 100644 index 00000000..c119f1bc --- /dev/null +++ b/tests/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/tests/point_transform_test.cpp b/tests/point_transform_test.cpp new file mode 100644 index 00000000..18607cb9 --- /dev/null +++ b/tests/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