diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 8505457e..82b0df3f 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,14 +7,20 @@ Changelog ouster_ros ---------- +* breaking change: renamed ``ouster_ros/ros.h`` to ``ouster_ros/os_ros.h`` and + ``ouster_ros/point.h`` to ``ouster_ros/os_point.h``. +* breaking change: change the type of the ring field within ``ouster::Point`` from ``uint8_t`` to + ``uint16_t`` * correct LICENSE file installation path. * update code files copyrights period. * bug fix: ros driver doesn't use correct udp_dest given by user during launch * update published TF transforms time with senosr or ros time based on the active timestamp mode. -* breaking change: renamed ``ouster_ros/ros.h`` to ``ouster_ros/os_ros.h`` and - ``ouster_ros/point.h`` to ``ouster_ros/os_point.h``. * validate lidar and imu port values. warn users when assigning random port numbers. +* switch to using the cartesianT method when populating pcl point cloud for performance and reduced + cpu utilization +* reduce dynamic memory allocation within the driver for performance and driver stability +* add ``pcl_ros`` as a dependency to ``package.xml`` ouster_client -------------- @@ -31,6 +37,9 @@ ouster_client * added a new method ``init_logger()`` to provide control over the logs emitted by ``ouster_client``. * add parsing for new FW 3.0 thermal features shot_limiting and thermal_shutdown statuses and countdowns * add frame_status to LidarScan +* introduce a new method ``cartesianT()`` which speeds up the computation of point projecion from range + image, the method also can process the cartesian product with single float precision. A new unit test + ``cartesian_test`` which shows achieved speed up gains by the number of valid returns in lidar scan. [20221004] ========== diff --git a/include/ouster_ros/os_point.h b/include/ouster_ros/os_point.h index 56286098..cc899f4a 100644 --- a/include/ouster_ros/os_point.h +++ b/include/ouster_ros/os_point.h @@ -22,7 +22,7 @@ struct EIGEN_ALIGN16 Point { float intensity; uint32_t t; uint16_t reflectivity; - uint8_t ring; + uint16_t ring; uint16_t ambient; uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -38,7 +38,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) - (std::uint8_t, ring, ring) + (std::uint16_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 63283abf..297a59f1 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -93,6 +93,27 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, ouster::LidarScan::ts_t scan_ts, const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, int return_index = 0); +/** + * 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] ls 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, + ouster::LidarScan::ts_t scan_ts, + const ouster::LidarScan& ls, + ouster_ros::Cloud& cloud, + int return_index); + + /** * Serialize a PCL point cloud to a ROS message * @param[in] cloud the PCL point cloud to convert diff --git a/ouster-sdk b/ouster-sdk index 279c9fb3..615e7877 160000 --- a/ouster-sdk +++ b/ouster-sdk @@ -1 +1 @@ -Subproject commit 279c9fb36c7e217d76bdde56bc82f833fe14783b +Subproject commit 615e7877a4e4c0aa6ec65aa7ad79872fdf5e7d77 diff --git a/package.xml b/package.xml index a58e85fd..b0389bd6 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.7.1 + 0.7.2 Ouster ROS driver ouster developers BSD @@ -12,6 +12,8 @@ sensor_msgs geometry_msgs tf2_ros + pcl_ros + pcl_conversions boost nodelet @@ -20,7 +22,6 @@ message_generation tf2_eigen libpcl-all-dev - pcl_conversions curl spdlog diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 06d75cdf..ba7312ac 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -87,7 +88,14 @@ class OusterCloud : public nodelet::Nodelet { lidar_pubs[i] = pub; } - xyz_lut = ouster::make_xyz_lut(info); + // The ouster_ros drive currently only uses single precision when it + // produces the point cloud. So it isn't of a benefit to compute point + // cloud xyz coordinates using double precision (for the time being). + auto xyz_lut = ouster::make_xyz_lut(info); + lut_direction = xyz_lut.direction.cast(); + lut_offset = xyz_lut.offset.cast(); + points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); + pc_ptr = boost::make_shared(); ls = ouster::LidarScan{W, H, info.format.udp_profile_lidar}; cloud = ouster_ros::Cloud{W, H}; @@ -104,14 +112,20 @@ class OusterCloud : public nodelet::Nodelet { "imu_packets", 100, &OusterCloud::imu_handler, this); } + void pcl_toROSMsg(const ouster_ros::Cloud &pcl_cloud, sensor_msgs::PointCloud2 &cloud) { + // TODO: remove the staging step in the future + static pcl::PCLPointCloud2 pcl_pc2; + pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); + pcl_conversions::moveFromPCL(pcl_pc2, cloud); + } + void convert_scan_to_pointcloud_publish(std::chrono::nanoseconds scan_ts, const ros::Time& msg_ts) { for (int i = 0; i < n_returns; ++i) { - scan_to_cloud(xyz_lut, scan_ts, ls, cloud, i); - sensor_msgs::PointCloud2 pc = - ouster_ros::cloud_to_cloud_msg(cloud, msg_ts, sensor_frame); - sensor_msgs::PointCloud2Ptr pc_ptr = - boost::make_shared(pc); + scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, ls, cloud, i); + pcl_toROSMsg(cloud, *pc_ptr); + pc_ptr->header.stamp = msg_ts; + pc_ptr->header.frame_id = sensor_frame; lidar_pubs[i].publish(pc_ptr); } @@ -172,11 +186,15 @@ class OusterCloud : public nodelet::Nodelet { std::vector lidar_pubs; ros::Subscriber imu_packet_sub; ros::Publisher imu_pub; + sensor_msgs::PointCloud2::Ptr pc_ptr; + sensor::sensor_info info; int n_returns = 0; - ouster::XYZLut xyz_lut; + ouster::PointsF lut_direction; + ouster::PointsF lut_offset; + ouster::PointsF points; ouster::LidarScan ls; ouster_ros::Cloud cloud; std::unique_ptr scan_batcher; @@ -189,6 +207,7 @@ class OusterCloud : public nodelet::Nodelet { bool use_ros_time; }; + } // namespace nodelets_os PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterCloud, nodelet::Nodelet) diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 063c2b45..122cdebf 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -1,7 +1,7 @@ /** * Copyright (c) 2018-2022, Ouster, Inc. * All rights reserved. - * + * * @file ros.cpp * @brief A nodelet that connects to a live ouster sensor */ @@ -21,10 +21,11 @@ #include #include -namespace ouster_ros { namespace sensor = ouster::sensor; +namespace ouster_ros { + bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, const sensor::packet_format& pf) { pm.buf.resize(pf.imu_packet_size + 1); @@ -114,13 +115,12 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { template inline ouster::img_t get_or_fill_zero(sensor::ChanField f, const ouster::LidarScan& ls) { - ouster::img_t result{ls.h, ls.w}; - if (ls.field_type(f)) { - ouster::impl::visit_field(ls, f, read_and_cast(), result); - } else { - result = Eigen::Matrix::Zero(ls.h, ls.w); + if (!ls.field_type(f)) { + return ouster::img_t::Zero(ls.h, ls.w); } + + ouster::img_t result{ls.h, ls.w}; + ouster::impl::visit_field(ls, f, read_and_cast(), result); return result; } @@ -163,6 +163,76 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, } } +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(); + +#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 ts = std::min( + std::chrono::nanoseconds(timestamp[v]) - scan_ts, scan_ts); + 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]), + }; + } + } +} + +void scan_to_cloud_f(ouster::PointsF& points, + const ouster::PointsF& lut_direction, + const ouster::PointsF& lut_offset, + ouster::LidarScan::ts_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 = get_or_fill_zero( + suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); + + ouster::img_t signal = get_or_fill_zero( + suitable_return(sensor::ChanField::SIGNAL, second), ls); + + ouster::img_t near_ir = get_or_fill_zero( + 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); +} + sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, const ros::Time& timestamp, const std::string& frame) {