diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml index b723fafd..81d5a432 100644 --- a/.github/workflows/docker-image.yml +++ b/.github/workflows/docker-image.yml @@ -17,7 +17,7 @@ jobs: ros_distro: - noetic steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: submodules: true - name: Build the Docker image diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 22662a94..bb635ccd 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -23,7 +23,26 @@ Changelog * Added a new launch ``persist_config`` option to request the sensor persist the current config * Added a new ``loop`` option to the ``replay.launch`` file. * Added support for automatic sensor reconnection. Consult ``attempt_reconnect`` launch file arg - documentation and the associated params to enable. + documentation and the associated params to enable. Known Issues: + - RVIZ can't handle image resize + - Can't handle points cloud resize properly (erroneous or corrupt PointCloud) + - Doesn't detect and handle invalid configurations +* Added a new parameter ``organized`` to request publishing unorganized point cloud +* Added a new parameter ``destagger`` to request publishing staggered point cloud +* Added two parameters ``min_range``, ``max_range`` to limit the lidar effective range +* Updated ouster_client to the release of ``20240425`` [v0.11.1]; changes listed below. + +ouster_client +------------- +* Added a new buffered UDP source implementation BufferedUDPSource. +* The method version_of_string is marked as deprecated, use version_from_string +instead. +* Added a new method firmware_version_from_metadata which works across firmwares. +* Added support for return order configuration parameter. +* Added support for gyro and accelerometer FSR configuration parameters. +* [BUGFIX] mtp_init_client throws a bad optional access. +* [BUGFIX] properly handle 32-bit frame IDs from the +* FUSA_RNG15_RFL8_NIR8_DUAL sensor UDP profile. ouster_ros v0.10.0 diff --git a/CMakeLists.txt b/CMakeLists.txt index 994b2bc3..df044da2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,7 @@ set(BUILD_SHARED_LIBS OFF) option(BUILD_VIZ "Enabled for Python build" OFF) option(BUILD_PCAP "Enabled for Python build" OFF) +option(BUILD_OSF "Build OSF library." OFF) find_package(OusterSDK REQUIRED) set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) @@ -107,7 +108,6 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}_test src/os_ros.cpp tests/test_main.cpp - tests/ring_buffer_test.cpp tests/lock_free_ring_buffer_test.cpp tests/point_accessor_test.cpp tests/point_transform_test.cpp diff --git a/Dockerfile b/Dockerfile index f7fed546..682cf93b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -50,7 +50,7 @@ FROM build-env SHELL ["/bin/bash", "-c"] -ENV CXXFLAGS="-Werror -Wno-deprecated-declarations" +ENV CXXFLAGS="-Wno-deprecated-declarations" RUN /opt/ros/$ROS_DISTRO/env.sh catkin_make \ -DCMAKE_BUILD_TYPE=Release \ && /opt/ros/$ROS_DISTRO/env.sh catkin_make install diff --git a/README.md b/README.md index 07018417..5500de29 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Official ROS1/ROS2 drivers for Ouster sensors [ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) | -[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) | +[ROS2 (rolling/humble/iron/jazzy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) | [ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)

@@ -9,7 +9,7 @@ | ROS Version | Build Status (Linux) | |:-----------:|:------:| | ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) -| ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) +| ROS2 (rolling/humble/iron/jazzy) | [![rolling/humble/iron/jazzy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) | ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) - [Official ROS1/ROS2 drivers for Ouster sensors](#official-ros1ros2-drivers-for-ouster-sensors) @@ -28,7 +28,7 @@ - [Invoking Services](#invoking-services) - [GetMetadata](#getmetadata) - [GetConfig](#getconfig) - - [SetConfig (experimental)](#setconfig-experimental) + - [SetConfig](#setconfig) - [License](#license) @@ -235,7 +235,7 @@ To get the current config of a live sensor, invoke the command: rosservice call /ouster/get_config ``` -#### SetConfig (experimental) +#### SetConfig To change config via a file while connected to a live sensor, invoke the command: ```bash rosservice call /ouster/set_config "config_file: ''" diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 631aa6ad..ec6127a8 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -160,6 +160,14 @@ inline bool check_token(const std::set& tokens, ouster::util::version parse_version(const std::string& fw_rev); +template +uint64_t ulround(T value) { + T rounded_value = std::round(value); + if (rounded_value < 0) return 0ULL; + if (rounded_value > ULLONG_MAX) return ULLONG_MAX; + return static_cast(rounded_value); +} + } // namespace impl } // namespace ouster_ros diff --git a/launch/common.launch b/launch/common.launch index 1ed5c7d7..8ab62420 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -41,6 +41,11 @@ xyzir }"/> + + + + + + + + + diff --git a/launch/driver.launch b/launch/driver.launch index 820d7563..ae62eaef 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -5,14 +5,16 @@ - - - - - + + + + + + @@ -117,6 +130,10 @@ value="$(arg dormant_period_between_reconnects)"/> + + + + diff --git a/launch/replay.launch b/launch/replay.launch index ed99213d..6bd3e5bc 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -12,8 +12,8 @@ - - + + + + + + @@ -93,6 +103,10 @@ + + + + diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch index 9f02869f..f6e957e4 100644 --- a/launch/replay_pcap.launch +++ b/launch/replay_pcap.launch @@ -6,8 +6,8 @@ - - + + + + + + @@ -86,6 +96,10 @@ + + + + diff --git a/launch/sensor.launch b/launch/sensor.launch index 46ffeb1e..d1a9f402 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -5,14 +5,16 @@ - - - - + + + + + + @@ -137,6 +150,10 @@ + + + + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 963781cc..142165ac 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -9,14 +9,16 @@ start client with active configuration of sensor"/> - - - - - - + + + + + + + + + + + output="screen" required="true" args="manager"/> @@ -108,6 +129,11 @@ + + + @@ -130,6 +156,10 @@ + + + + diff --git a/ouster-sdk b/ouster-sdk index 2898060f..d9967685 160000 --- a/ouster-sdk +++ b/ouster-sdk @@ -1 +1 @@ -Subproject commit 2898060fb47a69b4fbf9807d3c17f6fdc8249379 +Subproject commit d99676858b3b6f90e1618354ff7930cabafa9769 diff --git a/src/impl/cartesian.h b/src/impl/cartesian.h index ac524faf..a86ac350 100644 --- a/src/impl/cartesian.h +++ b/src/impl/cartesian.h @@ -16,7 +16,10 @@ namespace ouster { * LidarScan. * @param[in] direction the direction of an xyz lut. * @param[in] offset the offset of an xyz lut. - * @param[in] invalid the value to assign of an xyz lut. + * @param[in] min_range minimum lidar range to consider (millimeters). + * @param[in] max_range maximum lidar range to consider (millimeters). + * @param[in] invalid the value to assign of an xyz lut when range values are + * equal to or exceed the min_range and max_range values. * * @return Cartesian points where ith row is a 3D point which corresponds * to ith pixel in LidarScan where i = row * w + col. @@ -25,7 +28,7 @@ template void cartesianT(PointsT& points, const Eigen::Ref>& range, const PointsT& direction, const PointsT& offset, - T invalid) { + uint32_t min_r, uint32_t max_r, T invalid) { assert(points.rows() == direction.rows() && "points & direction row count mismatch"); assert(points.rows() == offset.rows() && @@ -51,7 +54,7 @@ void cartesianT(PointsT& points, const auto idx_x = col_x + i; const auto idx_y = col_y + i; const auto idx_z = col_z + i; - if (r == 0) { + if (r <= min_r || r >= max_r) { pts[idx_x] = pts[idx_y] = pts[idx_z] = invalid; } else { pts[idx_x] = r * dir[idx_x] + ofs[idx_x]; diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h index b8349ba9..81b9ce4c 100644 --- a/src/imu_packet_handler.h +++ b/src/imu_packet_handler.h @@ -19,34 +19,39 @@ namespace ouster_ros { class ImuPacketHandler { public: using HandlerOutput = sensor_msgs::Imu; - using HandlerType = std::function; + using HandlerType = std::function; public: - static HandlerType create_handler(const ouster::sensor::sensor_info& info, + static HandlerType create_handler(const sensor::sensor_info& info, const std::string& frame, const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) { - const auto& pf = ouster::sensor::get_format(info); - using Timestamper = std::function; + const auto& pf = sensor::get_format(info); + using Timestamper = std::function; Timestamper timestamper; if (timestamp_mode == "TIME_FROM_ROS_TIME") { timestamper = Timestamper{ - [](const uint8_t* /*imu_buf*/) { return ros::Time::now(); }}; + [](const sensor::ImuPacket& imu_packet) { + return impl::ts_to_ros_time(imu_packet.host_timestamp); + }}; } else if (timestamp_mode == "TIME_FROM_PTP_1588") { - timestamper = - Timestamper{[pf, ptp_utc_tai_offset](const uint8_t* imu_buf) { - uint64_t ts = pf.imu_gyro_ts(imu_buf); + timestamper = Timestamper{ + [pf, ptp_utc_tai_offset](const sensor::ImuPacket& imu_packet) { + auto ts = pf.imu_gyro_ts(imu_packet.buf.data()); ts = impl::ts_safe_offset_add(ts, ptp_utc_tai_offset); return impl::ts_to_ros_time(ts); }}; } else { - timestamper = Timestamper{[pf](const uint8_t* imu_buf) { - return impl::ts_to_ros_time(pf.imu_gyro_ts(imu_buf)); - }}; + timestamper = Timestamper{ + [pf](const sensor::ImuPacket& imu_packet) { + auto ts = pf.imu_gyro_ts(imu_packet.buf.data()); + return impl::ts_to_ros_time(ts); + }}; } - return [&pf, &frame, timestamper](const uint8_t* imu_buf) { - return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf); + return [&pf, &frame, timestamper](const sensor::ImuPacket& imu_packet) { + return packet_to_imu_msg(pf, timestamper(imu_packet), frame, + imu_packet.buf.data()); }; } }; diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h index ed858978..2baa180f 100644 --- a/src/lidar_packet_handler.h +++ b/src/lidar_packet_handler.h @@ -47,14 +47,6 @@ uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); } -template -uint64_t ulround(T value) { - T rounded_value = std::round(value); - if (rounded_value < 0) return 0ULL; - if (rounded_value > ULLONG_MAX) return ULLONG_MAX; - return static_cast(rounded_value); -} - } // namespace namespace ouster_ros { @@ -65,15 +57,15 @@ using LidarScanProcessor = std::function; class LidarPacketHandler { - using LidarPacketAccumlator = std::function; + using LidarPacketAccumlator = std::function; public: using HandlerOutput = ouster::LidarScan; - using HandlerType = std::function; + using HandlerType = std::function; public: - LidarPacketHandler(const ouster::sensor::sensor_info& info, + LidarPacketHandler(const sensor::sensor_info& info, const std::vector& handlers, const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) @@ -103,23 +95,24 @@ class LidarPacketHandler { compute_scan_ts = [this](const auto& ts_v) { return compute_scan_ts_0(ts_v); }; + const sensor::packet_format& pf = sensor::get_format(info); if (timestamp_mode == "TIME_FROM_ROS_TIME") { lidar_packet_accumlator = - LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { - return lidar_handler_ros_time(pf, lidar_buf); + LidarPacketAccumlator{[this, pf](const sensor::LidarPacket& lidar_packet) { + return lidar_handler_ros_time(pf, lidar_packet); }}; } else if (timestamp_mode == "TIME_FROM_PTP_1588") { lidar_packet_accumlator = LidarPacketAccumlator{ - [this, pf, ptp_utc_tai_offset](const uint8_t* lidar_buf) { - return lidar_handler_sensor_time_ptp(pf, lidar_buf, + [this, pf, ptp_utc_tai_offset](const sensor::LidarPacket& lidar_packet) { + return lidar_handler_sensor_time_ptp(pf, lidar_packet, ptp_utc_tai_offset); }}; } else { lidar_packet_accumlator = - LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { - return lidar_handler_sensor_time(pf, lidar_buf); + LidarPacketAccumlator{[this, pf](const sensor::LidarPacket& lidar_packet) { + return lidar_handler_sensor_time(pf, lidar_packet); }}; } } @@ -142,13 +135,13 @@ class LidarPacketHandler { public: static HandlerType create_handler( - const ouster::sensor::sensor_info& info, + const sensor::sensor_info& info, const std::vector& handlers, const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) { auto handler = std::make_shared( info, handlers, timestamp_mode, ptp_utc_tai_offset); - return [handler](const uint8_t* lidar_buf) { - if (handler->lidar_packet_accumlator(lidar_buf)) { + return [handler](const sensor::LidarPacket& lidar_packet) { + if (handler->lidar_packet_accumlator(lidar_packet)) { handler->ring_buffer_has_elements.notify_one(); } }; @@ -158,17 +151,14 @@ class LidarPacketHandler { void process_scans() { - using namespace std::chrono; - { + using namespace std::chrono; std::unique_lock index_lock(ring_buffer_mutex); ring_buffer_has_elements.wait_for(index_lock, 1s, [this] { return !ring_buffer.empty(); }); - if (ring_buffer.empty()) { - return; - } + if (ring_buffer.empty()) return; } std::unique_lock lock(*mutexes[ring_buffer.read_head()]); @@ -200,7 +190,7 @@ class LidarPacketHandler { last_scan_last_nonzero_idx, last_scan_last_nonzero_value, scan_width + curr_scan_first_nonzero_idx, curr_scan_first_nonzero_value, scan_width); - return ulround(interpolated_value); + return impl::ulround(interpolated_value); } uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, @@ -208,7 +198,7 @@ class LidarPacketHandler { double extrapolated_value = curr_scan_first_nonzero_value - scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; - return ulround(extrapolated_value); + return impl::ulround(extrapolated_value); } // compute_scan_ts_0 for first scan @@ -275,7 +265,7 @@ class LidarPacketHandler { } bool lidar_handler_sensor_time(const sensor::packet_format&, - const uint8_t* lidar_buf) { + const sensor::LidarPacket& lidar_packet) { if (ring_buffer.full()) { NODELET_WARN("lidar_scans full, DROPPING PACKET"); @@ -284,7 +274,7 @@ class LidarPacketHandler { std::unique_lock lock(*(mutexes[ring_buffer.write_head()])); - if (!(*scan_batcher)(lidar_buf, *lidar_scans[ring_buffer.write_head()])) return false; + if (!(*scan_batcher)(lidar_packet, *lidar_scans[ring_buffer.write_head()])) return false; lidar_scan_estimated_ts = compute_scan_ts(lidar_scans[ring_buffer.write_head()]->timestamp()); lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts); @@ -294,7 +284,7 @@ class LidarPacketHandler { } bool lidar_handler_sensor_time_ptp(const sensor::packet_format&, - const uint8_t* lidar_buf, + const sensor::LidarPacket& lidar_packet, int64_t ptp_utc_tai_offset) { if (ring_buffer.full()) { @@ -305,7 +295,7 @@ class LidarPacketHandler { std::unique_lock lock( *(mutexes[ring_buffer.write_head()])); - if (!(*scan_batcher)(lidar_buf, *lidar_scans[ring_buffer.write_head()])) return false; + if (!(*scan_batcher)(lidar_packet, *lidar_scans[ring_buffer.write_head()])) return false; auto ts_v = lidar_scans[ring_buffer.write_head()]->timestamp(); for (int i = 0; i < ts_v.rows(); ++i) ts_v[i] = impl::ts_safe_offset_add(ts_v[i], ptp_utc_tai_offset); @@ -319,11 +309,11 @@ class LidarPacketHandler { } bool lidar_handler_ros_time(const sensor::packet_format& pf, - const uint8_t* lidar_buf) { - auto packet_receive_time = ros::Time::now(); + const sensor::LidarPacket& lidar_packet) { + auto packet_receive_time = impl::ts_to_ros_time(lidar_packet.host_timestamp); if (!lidar_handler_ros_time_frame_ts_initialized) { lidar_handler_ros_time_frame_ts = extrapolate_frame_ts( - pf, lidar_buf, packet_receive_time); // first point cloud time + pf, lidar_packet.buf.data(), packet_receive_time); // first point cloud time lidar_handler_ros_time_frame_ts_initialized = true; } @@ -335,12 +325,12 @@ class LidarPacketHandler { std::unique_lock lock( *(mutexes[ring_buffer.write_head()])); - if (!(*scan_batcher)(lidar_buf, *lidar_scans[ring_buffer.write_head()])) return false; + if (!(*scan_batcher)(lidar_packet, *lidar_scans[ring_buffer.write_head()])) return false; lidar_scan_estimated_ts = compute_scan_ts(lidar_scans[ring_buffer.write_head()]->timestamp()); lidar_scan_estimated_msg_ts = lidar_handler_ros_time_frame_ts; // set time for next point cloud msg lidar_handler_ros_time_frame_ts = - extrapolate_frame_ts(pf, lidar_buf, packet_receive_time); + extrapolate_frame_ts(pf, lidar_packet.buf.data(), packet_receive_time); ring_buffer.write(); diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 92c7e02f..189a5bb1 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -100,7 +100,12 @@ class OusterCloud : public nodelet::Nodelet { imu_packet_sub = getNodeHandle().subscribe( "imu_packets", 100, [this](const PacketMsg::ConstPtr msg) { if (imu_packet_handler) { - auto imu_msg = imu_packet_handler(msg->buf.data()); + // TODO[UN]: this is not ideal since we can't reuse the msg buffer + // Need to redefine the Packet object and allow use of array_views + sensor::ImuPacket imu_packet(msg->buf.size()); + memcpy(imu_packet.buf.data(), msg->buf.data(), msg->buf.size()); + imu_packet.host_timestamp = static_cast(ros::Time::now().toNSec()); + auto imu_msg = imu_packet_handler(imu_packet); if (imu_msg.header.stamp > last_msg_ts) last_msg_ts = imu_msg.header.stamp; imu_pub.publish(imu_msg); @@ -129,7 +134,14 @@ class OusterCloud : public nodelet::Nodelet { void create_lidar_packets_sub() { lidar_packet_sub = getNodeHandle().subscribe( "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) { - if (lidar_packet_handler) lidar_packet_handler(msg->buf.data()); + if (lidar_packet_handler) { + // TODO[UN]: this is not ideal since we can't reuse the msg buffer + // Need to redefine the Packet object and allow use of array_views + sensor::LidarPacket lidar_packet(msg->buf.size()); + memcpy(lidar_packet.buf.data(), msg->buf.data(), msg->buf.size()); + lidar_packet.host_timestamp = static_cast(ros::Time::now().toNSec()); + lidar_packet_handler(lidar_packet); + } }); } @@ -150,10 +162,27 @@ class OusterCloud : public nodelet::Nodelet { std::vector processors; if (impl::check_token(tokens, "PCL")) { auto point_type = pnh.param("point_type", std::string{"original"}); + auto organized = pnh.param("organized", true); + auto destagger = pnh.param("destagger", true); + auto min_range_m = pnh.param("min_range", 0.0); + auto max_range_m = pnh.param("max_range", 10000.0); + if (min_range_m < 0.0 || max_range_m < 0.0) { + NODELET_FATAL("min_range and max_range need to be positive"); + throw std::runtime_error("negative range limits!"); + } + if (min_range_m >= max_range_m) { + NODELET_FATAL("min_range can't be equal or exceed max_range"); + throw std::runtime_error("min_range equal to or exceeds max_range!"); + } + // convert to millimeters + uint32_t min_range = impl::ulround(min_range_m * 1000); + uint32_t max_range = impl::ulround(max_range_m * 1000); + auto rows_step = pnh.param("rows_step", 1); processors.push_back( - PointCloudProcessorFactory::create_point_cloud_processor(point_type, - info, tf_bcast.point_cloud_frame_id(), + PointCloudProcessorFactory::create_point_cloud_processor( + point_type, info, tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), + organized, destagger, min_range, max_range, rows_step, [this](PointCloudProcessor_OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) { if (msgs[i]->header.stamp > last_msg_ts) diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 3eea44a1..13129a9e 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -27,6 +27,8 @@ #include "point_cloud_processor_factory.h" namespace sensor = ouster::sensor; +using ouster::sensor::LidarPacket; +using ouster::sensor::ImuPacket; namespace ouster_ros { @@ -35,13 +37,12 @@ class OusterDriver : public OusterSensor { OusterDriver() : tf_bcast(getName()) {} ~OusterDriver() override { NODELET_DEBUG("OusterDriver::~OusterDriver() called"); - halt(); } protected: virtual void onInit() override { auto& pnh = getPrivateNodeHandle(); - auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"}); + auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN|IMG|RAW"}); auto tokens = impl::parse_tokens(proc_mask, '|'); if (impl::check_token(tokens, "IMU")) create_imu_pub(); @@ -51,6 +52,7 @@ class OusterDriver : public OusterSensor { create_laser_scan_pubs(); if (impl::check_token(tokens, "IMG")) create_image_pubs(); + publish_raw = impl::check_token(tokens, "RAW"); OusterSensor::onInit(); } @@ -121,11 +123,29 @@ class OusterDriver : public OusterSensor { std::vector processors; if (impl::check_token(tokens, "PCL")) { auto point_type = pnh.param("point_type", std::string{"original"}); + auto organized = pnh.param("organized", true); + auto destagger = pnh.param("destagger", true); + auto min_range_m = pnh.param("min_range", 0.0); + auto max_range_m = pnh.param("max_range", 10000.0); + if (min_range_m < 0.0 || max_range_m < 0.0) { + NODELET_FATAL("min_range and max_range need to be positive"); + throw std::runtime_error("negative range limits!"); + } + if (min_range_m >= max_range_m) { + NODELET_FATAL("min_range can't be equal or exceed max_range"); + throw std::runtime_error("min_range equal to or exceeds max_range!"); + } + // convert to millimeters + uint32_t min_range = impl::ulround(min_range_m * 1000); + uint32_t max_range = impl::ulround(max_range_m * 1000); + auto rows_step = pnh.param("rows_step", 1); processors.push_back( PointCloudProcessorFactory::create_point_cloud_processor(point_type, info, tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), + organized, destagger, min_range, max_range, rows_step, [this](PointCloudProcessor_OutputType msgs) { - for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i].publish(*msgs[i]); + for (size_t i = 0; i < msgs.size(); ++i) + lidar_pubs[i].publish(*msgs[i]); } ) ); @@ -178,16 +198,23 @@ class OusterDriver : public OusterSensor { 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_lidar_packet_msg(const LidarPacket& lidar_packet) override { + if (lidar_packet_handler) { + lidar_packet_handler(lidar_packet); + } + + if (publish_raw) + OusterSensor::on_lidar_packet_msg(lidar_packet); } - virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override { - if (imu_packet_handler) + virtual void on_imu_packet_msg(const ImuPacket& imu_packet) override { if (imu_packet_handler) { - auto imu_msg = imu_packet_handler(raw_imu_packet); + auto imu_msg = imu_packet_handler(imu_packet); imu_pub.publish(imu_msg); } + + if (publish_raw) + OusterSensor::on_imu_packet_msg(imu_packet); } private: @@ -200,6 +227,9 @@ class OusterDriver : public OusterSensor { ImuPacketHandler::HandlerType imu_packet_handler; LidarPacketHandler::HandlerType lidar_packet_handler; + + bool publish_raw = false; + }; } // namespace ouster_ros diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index ff8e8685..7358589f 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -52,7 +52,14 @@ class OusterImage : public nodelet::Nodelet { void create_lidar_packets_subscriber() { lidar_packet_sub = getNodeHandle().subscribe( "lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) { - if (lidar_packet_handler) lidar_packet_handler(msg->buf.data()); + if (lidar_packet_handler) { + // TODO[UN]: this is not ideal since we can't reuse the msg buffer + // Need to redefine the Packet object and allow use of array_views + sensor::LidarPacket lidar_packet(msg->buf.size()); + memcpy(lidar_packet.buf.data(), msg->buf.data(), msg->buf.size()); + lidar_packet.host_timestamp = static_cast(ros::Time::now().toNSec()); + lidar_packet_handler(lidar_packet); + } }); } diff --git a/src/os_pcap_nodelet.cpp b/src/os_pcap_nodelet.cpp index a5f38077..b2cdab49 100644 --- a/src/os_pcap_nodelet.cpp +++ b/src/os_pcap_nodelet.cpp @@ -22,7 +22,6 @@ #include "ouster_ros/os_sensor_nodelet_base.h" #include "ouster_ros/PacketMsg.h" -#include "thread_safe_ring_buffer.h" #include namespace sensor = ouster::sensor; @@ -43,11 +42,16 @@ class OusterPcap : public OusterSensorNodeletBase { open_pcap(pcap_file); publish_metadata(); create_get_metadata_service(); - start_packet_processing_threads(); start_packet_read_thread(); NODELET_INFO("Running in replay mode"); } + ~OusterPcap() override { + NODELET_DEBUG("OusterPcap::~OusterPcap() called"); + stop_packet_read_thread(); + } + + std::string get_meta_file() const { auto meta_file = getPrivateNodeHandle().param("metadata", std::string{}); @@ -82,16 +86,8 @@ class OusterPcap : public OusterSensorNodeletBase { void allocate_buffers() { auto& pf = sensor::get_format(info); - lidar_packet.buf.resize(pf.lidar_packet_size); - // TODO: gauge necessary queue size for lidar packets - lidar_packets = - std::make_unique(pf.lidar_packet_size, 1024); - imu_packet.buf.resize(pf.imu_packet_size); - // TODO: gauge necessary queue size for lidar packets - imu_packets = - std::make_unique(pf.imu_packet_size, 1024); } void create_publishers() { @@ -123,64 +119,6 @@ class OusterPcap : public OusterSensorNodeletBase { } } - void start_packet_processing_threads() { - imu_packets_processing_thread_active = true; - imu_packets_processing_thread = std::make_unique([this]() { - while (imu_packets_processing_thread_active) { - imu_packets->read([this](const uint8_t* buffer) { - on_imu_packet_msg(buffer); - }); - } - NODELET_DEBUG("imu_packets_processing_thread done."); - }); - - lidar_packets_processing_thread_active = true; - lidar_packets_processing_thread = - std::make_unique([this]() { - while (lidar_packets_processing_thread_active) { - lidar_packets->read([this](const uint8_t* buffer) { - on_lidar_packet_msg(buffer); - }); - } - - NODELET_DEBUG("lidar_packets_processing_thread done."); - }); - } - - void stop_packet_processing_threads() { - NODELET_DEBUG("stopping packet processing threads."); - - if (imu_packets_processing_thread->joinable()) { - imu_packets_processing_thread_active = false; - imu_packets_processing_thread->join(); - } - - if (lidar_packets_processing_thread->joinable()) { - lidar_packets_processing_thread_active = false; - lidar_packets_processing_thread->join(); - } - } - - void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) { - // copying the data from queue buffer into the message buffer - // this can be avoided by constructing an abstraction where - // OusterSensor has its own RingBuffer of PacketMsg but for - // now we are focusing on optimizing the code for OusterDriver - std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, - lidar_packet.buf.size()); - lidar_packet_pub.publish(lidar_packet); - } - - void on_imu_packet_msg(const uint8_t* raw_imu_packet) { - // copying the data from queue buffer into the message buffer - // this can be avoided by constructing an abstraction where - // OusterSensor has its own RingBuffer of PacketMsg but for - // now we are focusing on optimizing the code for OusterDriver - std::memcpy(imu_packet.buf.data(), raw_imu_packet, - imu_packet.buf.size()); - imu_packet_pub.publish(imu_packet); - } - void read_packets(PcapReader& pcap, const sensor::packet_format& pf) { size_t payload_size = pcap.next_packet(); auto packet_info = pcap.current_info(); @@ -189,20 +127,17 @@ class OusterPcap : public OusterSensorNodeletBase { using namespace std::chrono_literals; const auto UPDATE_PERIOD = duration_cast(1s / 3); - while (payload_size) { + while (ros::ok() && payload_size) { auto start = high_resolution_clock::now(); if (packet_info.dst_port == info.config.udp_port_imu) { - imu_packets->write_overwrite( - [this, &pcap, &pf, &packet_info](uint8_t* buffer) { - std::memcpy(buffer, pcap.current_data(), - pf.imu_packet_size); - }); + std::memcpy(imu_packet.buf.data(), pcap.current_data(), + pf.imu_packet_size); + imu_packet_pub.publish(imu_packet); } else if (packet_info.dst_port == info.config.udp_port_lidar) { - lidar_packets->write_overwrite( - [this, &pcap, &pf, &packet_info](uint8_t* buffer) { - std::memcpy(buffer, pcap.current_data(), - pf.lidar_packet_size); - }); + std::memcpy(lidar_packet.buf.data(), + pcap.current_data(), + pf.lidar_packet_size); + lidar_packet_pub.publish(lidar_packet); } else { NODELET_WARN_STREAM_THROTTLE(1, "unknown packet /w port: " @@ -233,17 +168,8 @@ class OusterPcap : public OusterSensorNodeletBase { ros::Publisher lidar_packet_pub; ros::Publisher imu_packet_pub; - std::unique_ptr lidar_packets; - std::unique_ptr imu_packets; - std::atomic packet_read_active = {false}; std::unique_ptr packet_read_thread; - - std::atomic imu_packets_processing_thread_active = {false}; - std::unique_ptr imu_packets_processing_thread; - - std::atomic lidar_packets_processing_thread_active = {false}; - std::unique_ptr lidar_packets_processing_thread; }; } // namespace ouster_ros diff --git a/src/os_replay_nodelet.cpp b/src/os_replay_nodelet.cpp index d913039f..8e36e292 100644 --- a/src/os_replay_nodelet.cpp +++ b/src/os_replay_nodelet.cpp @@ -30,8 +30,8 @@ class OusterReplay : public OusterSensorNodeletBase { auto meta_file = getPrivateNodeHandle().param("metadata", std::string{}); if (!is_arg_set(meta_file)) { - NODELET_ERROR("Must specify metadata file in replay mode"); - throw std::runtime_error("metadata no specificed"); + NODELET_FATAL("Must specify metadata file in replay mode"); + throw std::runtime_error("metadata not specificed"); } return meta_file; } diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index 8e618ad4..57cde7eb 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -20,23 +20,19 @@ #include "ouster_ros/PacketMsg.h" #include "os_sensor_nodelet.h" -namespace sensor = ouster::sensor; -using nonstd::optional; - +using std::to_string; using namespace std::chrono_literals; using namespace std::string_literals; -using std::to_string; + +namespace sensor = ouster::sensor; +using sensor::LidarPacket; +using sensor::ImuPacket; namespace ouster_ros { OusterSensor::~OusterSensor() { NODELET_DEBUG("OusterDriver::~OusterSensor() called"); - halt(); -} - -void OusterSensor::halt() { - stop_packet_processing_threads(); stop_sensor_connection_thread(); } @@ -54,14 +50,14 @@ bool OusterSensor::start() { if (!get_active_config_no_throw(sensor_hostname, config)) return false; + NODELET_INFO("Retrived sensor active config"); + // Unfortunately it seems we need to invoke this to force the auto // TODO[UN]: find a shortcut // Only reset udp_dest if auto_udp was allowed on startup if (auto_udp_allowed) config.udp_dest.reset(); if (!configure_sensor(sensor_hostname, config)) return false; - - NODELET_INFO("Retrived sensor active config"); } reset_last_init_id = true; @@ -73,14 +69,13 @@ bool OusterSensor::start() { update_metadata(*sensor_client); allocate_buffers(); - start_packet_processing_threads(); start_sensor_connection_thread(); return true; } void OusterSensor::stop() { // deactivate - halt(); + stop_sensor_connection_thread(); sensor_client.reset(); } @@ -133,7 +128,7 @@ std::string OusterSensor::get_sensor_hostname() { auto hostname = nh.param("sensor_hostname", std::string{}); if (!is_arg_set(hostname)) { auto error_msg = "Must specify a sensor hostname"; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -321,7 +316,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { auto error_msg = "Invalid lidar port number! port value should be in the range " "[0, 65535]."; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -329,7 +324,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { auto error_msg = "Invalid imu port number! port value should be in the range " "[0, 65535]."; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -341,7 +336,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (!udp_profile_lidar) { auto error_msg = "Invalid udp profile lidar: " + udp_profile_lidar_arg; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } } @@ -352,7 +347,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); if (!lidar_mode) { auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } } @@ -372,7 +367,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (!timestamp_mode) { auto error_msg = "Invalid timestamp mode: " + timestamp_mode_arg; - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } } @@ -422,7 +417,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { azimuth_window_end < MIN_AZW || azimuth_window_end > MAX_AZW) { auto error_msg = "azimuth window values must be between " + to_string(MIN_AZW) + " and " + to_string(MAX_AZW); - NODELET_ERROR_STREAM(error_msg); + NODELET_FATAL_STREAM(error_msg); throw std::runtime_error(error_msg); } @@ -476,10 +471,10 @@ bool OusterSensor::configure_sensor(const std::string& hostname, if (!get_config(hostname, config, true)) { NODELET_ERROR("Error getting active config"); return false; - } else { - NODELET_INFO("Retrived active config of sensor"); - return true; } + + NODELET_INFO("Retrived active config of sensor"); + return true; } uint8_t config_flags = compose_config_flags(config); @@ -502,7 +497,7 @@ void OusterSensor::populate_metadata_defaults( if (!info.name.size()) info.name = "UNKNOWN"; if (!info.sn.size()) info.sn = "UNKNOWN"; - ouster::util::version v = ouster::util::version_of_string(info.fw_rev); + ouster::util::version v = ouster::util::version_from_string(info.fw_rev); if (v == ouster::util::invalid_version) NODELET_WARN( "Unknown sensor firmware version; output may not be reliable"); @@ -544,21 +539,15 @@ void OusterSensor::create_publishers() { void OusterSensor::allocate_buffers() { auto& pf = sensor::get_format(info); - lidar_packet.buf.resize(pf.lidar_packet_size); - // TODO: gauge necessary queue size for lidar packets - lidar_packets = - std::make_unique(pf.lidar_packet_size, 1024); - + lidar_packet_msg.buf.resize(pf.lidar_packet_size); imu_packet.buf.resize(pf.imu_packet_size); - // TODO: gauge necessary queue size for lidar packets - imu_packets = - std::make_unique(pf.imu_packet_size, 1024); + imu_packet_msg.buf.resize(pf.imu_packet_size); } bool OusterSensor::init_id_changed(const sensor::packet_format& pf, - const uint8_t* lidar_buf) { - uint32_t current_init_id = pf.init_id(lidar_buf); + const LidarPacket& lidar_packet) { + uint32_t current_init_id = pf.init_id(lidar_packet.buf.data()); if (!last_init_id) { last_init_id = current_init_id + 1; } @@ -577,7 +566,7 @@ void OusterSensor::handle_poll_client_error() { // in case error continues for a while attempt to recover by // performing sensor reset if (++poll_client_error_count > max_poll_client_error_count) { - NODELET_ERROR_STREAM( + NODELET_ERROR( "maximum number of allowed errors from " "sensor::poll_client() reached, performing self reset..."); poll_client_error_count = 0; @@ -587,41 +576,38 @@ void OusterSensor::handle_poll_client_error() { void OusterSensor::handle_lidar_packet(sensor::client& cli, const sensor::packet_format& pf) { - lidar_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) { - bool success = sensor::read_lidar_packet(cli, buffer, pf); - if (success) { + if (sensor::read_lidar_packet(cli, lidar_packet)) { + read_lidar_packet_errors = 0; + if (!is_legacy_lidar_profile(info) && init_id_changed(pf, lidar_packet)) { + // TODO: short circut reset if no breaking changes occured? + NODELET_WARN("sensor init_id has changed! reactivating.."); + reset_sensor(false); + } + on_lidar_packet_msg(lidar_packet); + } else { + if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { + NODELET_ERROR( + "maximum number of allowed errors from " + "sensor::read_lidar_packet() reached, reactivating..."); read_lidar_packet_errors = 0; - if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) { - // TODO: short circut reset if no breaking changes occured? - NODELET_WARN("sensor init_id has changed! reactivating.."); - reset_sensor(false); - } - } else { - if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { - NODELET_ERROR( - "maximum number of allowed errors from " - "sensor::read_lidar_packet() reached, reactivating..."); - read_lidar_packet_errors = 0; - reactivate_sensor(true); - } + reset_sensor(true); } - }); + } } void OusterSensor::handle_imu_packet(sensor::client& cli, - const sensor::packet_format& pf) { - imu_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) { - bool success = sensor::read_imu_packet(cli, buffer, pf); - if (!success) { - if (++read_imu_packet_errors > max_read_imu_packet_errors) { - NODELET_ERROR_STREAM( - "maximum number of allowed errors from " - "sensor::read_imu_packet() reached, reactivating..."); - read_imu_packet_errors = 0; - reactivate_sensor(true); - } + const sensor::packet_format&) { + if (sensor::read_imu_packet(cli, imu_packet)) { + on_imu_packet_msg(imu_packet); + } else { + if (++read_imu_packet_errors > max_read_imu_packet_errors) { + NODELET_ERROR( + "maximum number of allowed errors from " + "sensor::read_imu_packet() reached, reactivating..."); + read_imu_packet_errors = 0; + reactivate_sensor(true); } - }); + } } void OusterSensor::connection_loop(sensor::client& cli, @@ -649,7 +635,7 @@ void OusterSensor::start_sensor_connection_thread() { sensor_connection_thread = std::make_unique([this]() { NODELET_DEBUG("sensor_connection_thread active."); auto& pf = sensor::get_format(info); - while (sensor_connection_active) { + while (ros::ok() && sensor_connection_active) { connection_loop(*sensor_client, pf); } NODELET_DEBUG("sensor_connection_thread done."); @@ -664,60 +650,14 @@ void OusterSensor::stop_sensor_connection_thread() { } } -void OusterSensor::start_packet_processing_threads() { - imu_packets_processing_thread_active = true; - imu_packets_processing_thread = std::make_unique([this]() { - while (imu_packets_processing_thread_active) { - imu_packets->read_timeout([this](const uint8_t* buffer) { - if (buffer) on_imu_packet_msg(buffer); - }, 1s); - } - NODELET_DEBUG("imu_packets_processing_thread done."); - }); - - lidar_packets_processing_thread_active = true; - lidar_packets_processing_thread = std::make_unique([this]() { - while (lidar_packets_processing_thread_active) { - lidar_packets->read_timeout([this](const uint8_t* buffer) { - if (buffer) on_lidar_packet_msg(buffer); - }, 1s); - } - - NODELET_DEBUG("lidar_packets_processing_thread done."); - }); -} - -void OusterSensor::stop_packet_processing_threads() { - NODELET_DEBUG("stopping packet processing threads."); - - if (imu_packets_processing_thread->joinable()) { - imu_packets_processing_thread_active = false; - imu_packets_processing_thread->join(); - } - - if (lidar_packets_processing_thread->joinable()) { - lidar_packets_processing_thread_active = false; - lidar_packets_processing_thread->join(); - } -} - -void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) { - // copying the data from queue buffer into the message buffer - // this can be avoided by constructing an abstraction where - // OusterSensor has its own RingBuffer of PacketMsg but for - // now we are focusing on optimizing the code for OusterDriver - std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, - lidar_packet.buf.size()); - lidar_packet_pub.publish(lidar_packet); +void OusterSensor::on_lidar_packet_msg(const LidarPacket&) { + lidar_packet_msg.buf.swap(lidar_packet.buf); + lidar_packet_pub.publish(lidar_packet_msg); } -void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { - // copying the data from queue buffer into the message buffer - // this can be avoided by constructing an abstraction where - // OusterSensor has its own RingBuffer of PacketMsg but for - // now we are focusing on optimizing the code for OusterDriver - std::memcpy(imu_packet.buf.data(), raw_imu_packet, imu_packet.buf.size()); - imu_packet_pub.publish(imu_packet); +void OusterSensor::on_imu_packet_msg(const ImuPacket&) { + imu_packet_msg.buf.swap(imu_packet.buf); + imu_packet_pub.publish(imu_packet_msg); } // param init_id_reset is overriden to true when force_reinit is true diff --git a/src/os_sensor_nodelet.h b/src/os_sensor_nodelet.h index 93a09822..825e7e2b 100644 --- a/src/os_sensor_nodelet.h +++ b/src/os_sensor_nodelet.h @@ -22,8 +22,6 @@ #include "ouster_ros/PacketMsg.h" #include "ouster_ros/os_sensor_nodelet_base.h" -#include "thread_safe_ring_buffer.h" - namespace sensor = ouster::sensor; namespace ouster_ros { @@ -41,9 +39,9 @@ class OusterSensor : public OusterSensorNodeletBase { virtual void create_publishers(); - virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet); + virtual void on_lidar_packet_msg(const sensor::LidarPacket& lidar_packet); - virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet); + virtual void on_imu_packet_msg(const sensor::ImuPacket& imu_packet); bool start(); @@ -53,7 +51,9 @@ class OusterSensor : public OusterSensorNodeletBase { void schedule_stop(); - void halt(); + void start_sensor_connection_thread(); + + void stop_sensor_connection_thread(); private: std::string get_sensor_hostname(); @@ -78,8 +78,6 @@ class OusterSensor : public OusterSensorNodeletBase { sensor::sensor_config parse_config_from_ros_parameters(); - sensor::sensor_config parse_config_from_staged_config_string(); - uint8_t compose_config_flags(const sensor::sensor_config& config); bool configure_sensor(const std::string& hostname, @@ -94,7 +92,7 @@ class OusterSensor : public OusterSensorNodeletBase { void allocate_buffers(); bool init_id_changed(const sensor::packet_format& pf, - const uint8_t* lidar_buf); + const sensor::LidarPacket& lidar_packet); void handle_poll_client_error(); @@ -109,14 +107,6 @@ class OusterSensor : public OusterSensorNodeletBase { void connection_loop(sensor::client& client, const sensor::packet_format& pf); - void start_sensor_connection_thread(); - - void stop_sensor_connection_thread(); - - void start_packet_processing_threads(); - - void stop_packet_processing_threads(); - bool get_active_config_no_throw(const std::string& sensor_hostname, sensor::sensor_config& config); @@ -126,18 +116,16 @@ class OusterSensor : public OusterSensorNodeletBase { std::string mtp_dest; bool mtp_main; std::shared_ptr sensor_client; - PacketMsg lidar_packet; - PacketMsg imu_packet; + PacketMsg lidar_packet_msg; + PacketMsg imu_packet_msg; + ouster::sensor::LidarPacket lidar_packet; + ouster::sensor::ImuPacket imu_packet; ros::Publisher lidar_packet_pub; ros::Publisher imu_packet_pub; ros::ServiceServer reset_srv; ros::ServiceServer get_config_srv; ros::ServiceServer set_config_srv; - // TODO: implement & utilize a lock-free ring buffer in future - std::unique_ptr lidar_packets; - std::unique_ptr imu_packets; - std::atomic sensor_connection_active = {false}; std::unique_ptr sensor_connection_thread; @@ -151,7 +139,6 @@ class OusterSensor : public OusterSensorNodeletBase { bool force_sensor_reinit = false; bool auto_udp_allowed = false; bool reset_last_init_id = true; - std::optional last_init_id; // TODO: add as a ros parameter diff --git a/src/point_cloud_compose.h b/src/point_cloud_compose.h index c6fed507..45b0b8aa 100644 --- a/src/point_cloud_compose.h +++ b/src/point_cloud_compose.h @@ -114,24 +114,47 @@ void copy_lidar_scan_fields_to_point(PointT& pt, const Tuple& tp, int idx) { template using Cloud = pcl::PointCloud; +// TODO[UN]: make this a functor 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) { +void scan_to_cloud_f(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, + bool organized = false, bool destagger = true, + int rows_step = 1) { 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; + if (!organized) cloud.clear(); + cloud.is_dense = true; + + for (auto u = 0; u < ls.h; u += rows_step) { + for (auto v = 0; v < ls.w; ++v) { // TODO[UN]: consider cols_step in future + const auto v_shift = destagger ? + (v + ls.w - pixel_shift_by_row[u]) % ls.w : v; const auto src_idx = u * ls.w + v_shift; - const auto tgt_idx = u * ls.w + v; const auto xyz = points.row(src_idx); + const auto tgt_idx = organized ? (u / rows_step) * ls.w + v : cloud.size(); + + if (xyz.isNaN().any()) { + if (organized) { + cloud.is_dense = false; + auto& pt = cloud.points[tgt_idx]; + pt.x = static_cast(xyz(0)); + pt.y = static_cast(xyz(1)); + pt.z = static_cast(xyz(2)); + } + continue; + } else { + if (!organized) + cloud.points.emplace_back(); + } + + auto ts = timestamp[v_shift]; + ts = ts > scan_ts ? ts - scan_ts : 0UL; + // 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( @@ -141,7 +164,7 @@ void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, 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 + // values if known 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); diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h index e4824439..c919a51c 100644 --- a/src/point_cloud_processor.h +++ b/src/point_cloud_processor.h @@ -39,11 +39,14 @@ class PointCloudProcessor { PointCloudProcessor(const ouster::sensor::sensor_info& info, const std::string& frame_id, bool apply_lidar_to_sensor_transform, + uint32_t min_range, uint32_t max_range, int rows_step, 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}, + cloud{info.format.columns_per_frame, + info.format.pixels_per_column / rows_step}, + min_range_(min_range), max_range_(max_range), pc_msgs(get_n_returns(info)), scan_to_cloud_fn(scan_to_cloud_fn_), post_processing_fn(post_processing_fn_) { @@ -77,9 +80,10 @@ 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) { - auto range_channel = static_cast(sensor::ChanField::RANGE + i); - auto range = lidar_scan.field(range_channel); + auto range_ch = static_cast(sensor::ChanField::RANGE + i); + auto range = lidar_scan.field(range_ch); ouster::cartesianT(points, range, lut_direction, lut_offset, + min_range_, max_range_, std::numeric_limits::quiet_NaN()); scan_to_cloud_fn(cloud, points, scan_ts, lidar_scan, @@ -97,10 +101,13 @@ class PointCloudProcessor { static LidarScanProcessor create(const ouster::sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, - ScanToCloudFn scan_to_cloud_fn_, + uint32_t min_range, uint32_t max_range, + int rows_step, ScanToCloudFn scan_to_cloud_fn_, PointCloudProcessor_PostProcessingFn post_processing_fn) { auto handler = std::make_shared( - info, frame, apply_lidar_to_sensor_transform, scan_to_cloud_fn_, post_processing_fn); + info, frame, apply_lidar_to_sensor_transform, + min_range, max_range, rows_step, + scan_to_cloud_fn_, post_processing_fn); return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const ros::Time& msg_ts) { @@ -120,6 +127,8 @@ class PointCloudProcessor { ouster::PointsF points; std::vector pixel_shift_by_row; ouster_ros::Cloud cloud; + uint32_t min_range_; + uint32_t max_range_; PointCloudProcessor_OutputType pc_msgs; ScanToCloudFn scan_to_cloud_fn; PointCloudProcessor_PostProcessingFn post_processing_fn; diff --git a/src/point_cloud_processor_factory.h b/src/point_cloud_processor_factory.h index ba59f280..03023141 100644 --- a/src/point_cloud_processor_factory.h +++ b/src/point_cloud_processor_factory.h @@ -10,94 +10,100 @@ using sensor::UDPProfileLidar; class PointCloudProcessorFactory { template static typename PointCloudProcessor::ScanToCloudFn - make_scan_to_cloud_fn(const sensor::sensor_info& info) { + make_scan_to_cloud_fn(const sensor::sensor_info& info, + bool organized, bool destagger, int rows_step) { 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); + return [organized, destagger, rows_step]( + 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_LEGACY staging_pt; - scan_to_cloud_f_destaggered( + scan_to_cloud_f( cloud, staging_pt, points, scan_ts, ls, - pixel_shift_by_row); + pixel_shift_by_row, organized, destagger, rows_step); }; 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) { + return [organized, destagger, rows_step]( + 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< + scan_to_cloud_f< Profile_RNG19_RFL8_SIG16_NIR16_DUAL.size(), Profile_RNG19_RFL8_SIG16_NIR16_DUAL>( cloud, staging_pt, points, scan_ts, ls, - pixel_shift_by_row); + pixel_shift_by_row, organized, destagger, rows_step); } else { - scan_to_cloud_f_destaggered< - Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETURN - .size(), + scan_to_cloud_f< + 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); + pixel_shift_by_row, organized, destagger, rows_step); } }; 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); + return [organized, destagger, rows_step]( + 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 staging_pt; - scan_to_cloud_f_destaggered< + scan_to_cloud_f< Profile_RNG19_RFL8_SIG16_NIR16.size(), - Profile_RNG19_RFL8_SIG16_NIR16>(cloud, staging_pt, - points, scan_ts, ls, - pixel_shift_by_row); + Profile_RNG19_RFL8_SIG16_NIR16>( + cloud, staging_pt, points, scan_ts, ls, + pixel_shift_by_row, organized, destagger, rows_step); }; 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); + return [organized, destagger, rows_step]( + 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_RNG15_RFL8_NIR8 staging_pt; - scan_to_cloud_f_destaggered( + scan_to_cloud_f< + Profile_RNG15_RFL8_NIR8.size(), + Profile_RNG15_RFL8_NIR8>( cloud, staging_pt, points, scan_ts, ls, - pixel_shift_by_row); + pixel_shift_by_row, organized, destagger, rows_step); }; case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL: - return [](ouster_ros::Cloud& cloud, - const ouster::PointsF& points, uint64_t scan_ts, - const ouster::LidarScan& ls, - const std::vector& pixel_shift_by_row, - int return_index) { + return [organized, destagger, rows_step]( + ouster_ros::Cloud& cloud, + const ouster::PointsF& points, uint64_t scan_ts, + const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { + Point_FUSA_RNG15_RFL8_NIR8_DUAL staging_pt; if (return_index == 0) { - scan_to_cloud_f_destaggered< + scan_to_cloud_f< Profile_FUSA_RNG15_RFL8_NIR8_DUAL.size(), Profile_FUSA_RNG15_RFL8_NIR8_DUAL>( cloud, staging_pt, points, scan_ts, ls, - pixel_shift_by_row); + pixel_shift_by_row, organized, destagger, rows_step); } else { - scan_to_cloud_f_destaggered< - Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN - .size(), + scan_to_cloud_f< + Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN.size(), Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN>( cloud, staging_pt, points, scan_ts, ls, - pixel_shift_by_row); + pixel_shift_by_row, organized, destagger, rows_step); } }; @@ -110,11 +116,15 @@ class PointCloudProcessorFactory { static LidarScanProcessor make_point_cloud_processor( const sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, + bool organized, bool destagger, + uint32_t min_range, uint32_t max_range, int rows_step, PointCloudProcessor_PostProcessingFn post_processing_fn) { - auto scan_to_cloud_fn = make_scan_to_cloud_fn(info); + auto scan_to_cloud_fn = make_scan_to_cloud_fn( + info, organized, destagger, rows_step); return PointCloudProcessor::create( - info, frame, apply_lidar_to_sensor_transform, scan_to_cloud_fn, - post_processing_fn); + info, frame, apply_lidar_to_sensor_transform, + min_range, max_range, rows_step, + scan_to_cloud_fn, post_processing_fn); } public: @@ -132,31 +142,38 @@ class PointCloudProcessorFactory { 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, + bool organized, bool destagger, + uint32_t min_range, uint32_t max_range, int rows_step, PointCloudProcessor_PostProcessingFn post_processing_fn) { if (point_type == "native") { switch (info.format.udp_profile_lidar) { case UDPProfileLidar::PROFILE_LIDAR_LEGACY: return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL: return make_point_cloud_processor< Point_RNG19_RFL8_SIG16_NIR16_DUAL>( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); case UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16: return make_point_cloud_processor< Point_RNG19_RFL8_SIG16_NIR16>( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); case UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8: return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL: return make_point_cloud_processor< Point_FUSA_RNG15_RFL8_NIR8_DUAL>( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); default: // TODO: implement fallback? @@ -165,18 +182,22 @@ class PointCloudProcessorFactory { } else if (point_type == "xyz") { return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); } else if (point_type == "xyzi") { return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); } else if (point_type == "xyzir") { return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); } else if (point_type == "original") { return make_point_cloud_processor( info, frame, apply_lidar_to_sensor_transform, + organized, destagger, min_range, max_range, rows_step, post_processing_fn); } diff --git a/src/thread_safe_ring_buffer.h b/src/thread_safe_ring_buffer.h deleted file mode 100644 index 26b5c88d..00000000 --- a/src/thread_safe_ring_buffer.h +++ /dev/null @@ -1,146 +0,0 @@ -/** - * Copyright (c) 2018-2023, Ouster, Inc. - * All rights reserved. - * - * @file thread_safe_ring_buffer.h - * @brief File contains thread safe implementation of a ring buffer - */ - -#pragma once - -#include -#include -#include - -/** - * @class ThreadSafeRingBuffer thread safe ring buffer. - */ -class ThreadSafeRingBuffer { - public: - ThreadSafeRingBuffer(size_t item_size_, size_t items_count_) - : buffer(item_size_ * items_count_), - item_size(item_size_), - max_items_count(items_count_), - active_items_count(0), - write_idx(0), - read_idx(0) {} - - /** - * Gets the maximum number of items that this ring buffer can hold. - */ - size_t capacity() const { return max_items_count; } - - /** - * Gets the number of item that currently occupy the ring buffer. This - * number would vary between 0 and the capacity(). - * - * @remarks - * if returned value was 0 or the value was equal to the buffer capacity(), - * this does not guarantee that a subsequent call to read() or write() - * wouldn't cause the calling thread to be blocked. - */ - size_t size() const { - std::lock_guard lock(mutex); - return active_items_count; - } - - /** - * Checks if the ring buffer is empty. - * - * @remarks - * if empty() returns true this does not guarantee that calling the write() - * operation directly right after wouldn't block the calling thread. - */ - bool empty() const { - std::lock_guard lock(mutex); - return active_items_count == 0; - } - - /** - * Checks if the ring buffer is full. - * - * @remarks - * if full() returns true this does not guarantee that calling the read() - * operation directly right after wouldn't block the calling thread. - */ - bool full() const { - std::lock_guard lock(mutex); - return active_items_count == max_items_count; - } - - /** - * Writes to the buffer safely, the method will keep blocking until the - * there is a space available within the buffer. - */ - template - void write(BufferWriteFn&& buffer_write) { - std::unique_lock lock(mutex); - fullCondition.wait(lock, - [this] { return active_items_count < capacity(); }); - buffer_write(&buffer[write_idx * item_size]); - write_idx = (write_idx + 1) % capacity(); - ++active_items_count; - emptyCondition.notify_one(); - } - - /** - * Writes to the buffer safely, if there is not space left then this method - * will overite the last item. - */ - template - void write_overwrite(BufferWriteFn&& buffer_write) { - std::unique_lock lock(mutex); - buffer_write(&buffer[write_idx * item_size]); - write_idx = (write_idx + 1) % capacity(); - if (active_items_count < capacity()) { - ++active_items_count; - } else { - read_idx = (read_idx + 1) % capacity(); - } - emptyCondition.notify_one(); - } - - /** - * Gives access to read the buffer through a callback, the method will block - * until there is something to read is available. - */ - template - void read(BufferReadFn&& buffer_read) { - std::unique_lock lock(mutex); - emptyCondition.wait(lock, [this] { return active_items_count > 0; }); - buffer_read(&buffer[read_idx * item_size]); - read_idx = (read_idx + 1) % capacity(); - --active_items_count; - fullCondition.notify_one(); - } - - /** - * Gives access to read the buffer through a callback, if buffer is - * inaccessible the method will timeout and buffer_read gets a nullptr. - */ - template - void read_timeout(BufferReadFn&& buffer_read, - std::chrono::seconds timeout) { - std::unique_lock lock(mutex); - if (emptyCondition.wait_for( - lock, timeout, [this] { return active_items_count > 0; })) { - buffer_read(&buffer[read_idx * item_size]); - read_idx = (read_idx + 1) % capacity(); - --active_items_count; - fullCondition.notify_one(); - } else { - buffer_read((uint8_t*)nullptr); - } - } - - private: - std::vector buffer; - size_t item_size; - size_t max_items_count; - size_t active_items_count; - size_t write_idx; - size_t read_idx; - mutable std::mutex mutex; - std::condition_variable fullCondition; - std::condition_variable emptyCondition; -}; \ No newline at end of file diff --git a/tests/ring_buffer_test.cpp b/tests/ring_buffer_test.cpp deleted file mode 100644 index e68ab29a..00000000 --- a/tests/ring_buffer_test.cpp +++ /dev/null @@ -1,175 +0,0 @@ -#include -#include -#include -#include -#include "../src/thread_safe_ring_buffer.h" - -using namespace std::chrono_literals; - -class ThreadSafeRingBufferTest : public ::testing::Test { - protected: - static constexpr int ITEM_SIZE = 4; // predefined size for all items used in - static constexpr int ITEM_COUNT = 3; // number of item the buffer could hold - - void SetUp() override { - buffer = std::make_unique(ITEM_SIZE, ITEM_COUNT); - } - - void TearDown() override { - buffer.reset(); - } - - std::string rand_str(int size) { - const std::string characters = - "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789"; - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_int_distribution dist(0, characters.size() - 1); - - std::string result; - for (int i = 0; i < size; ++i) { - result += characters[dist(gen)]; - } - return result; - } - - std::vector rand_vector_str(int vec_size, int str_size) { - std::vector output(vec_size); - for (auto i = 0; i < vec_size; ++i) - output[i] = rand_str(str_size); - return output; - } - - std::vector known_vector_str(int vec_size, const std::string& known) { - std::vector output(vec_size); - for (auto i = 0; i < vec_size; ++i) - output[i] = known; - return output; - } - - std::unique_ptr buffer; -}; - -TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferSimple) { - - assert (ITEM_COUNT > 1 && "or this test can't run"); - - const int TOTAL_ITEMS = 10; // total items to process - const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); - std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); - - EXPECT_TRUE(buffer->empty()); - EXPECT_FALSE(buffer->full()); - - for (int i = 0; i < ITEM_COUNT; ++i) { - buffer->write([i, &source](uint8_t* buffer) { - std::memcpy(buffer, &source[i][0], ITEM_SIZE); - }); - } - - EXPECT_FALSE(buffer->empty()); - EXPECT_TRUE(buffer->full()); - - // remove one item - buffer->read([&target](uint8_t* buffer){ - std::memcpy(&target[0][0], buffer, ITEM_SIZE); - }); - - EXPECT_FALSE(buffer->empty()); - EXPECT_FALSE(buffer->full()); - - for (int i = 1; i < ITEM_COUNT; ++i) { - buffer->read([i, &target](uint8_t* buffer){ - std::memcpy(&target[i][0], buffer, ITEM_SIZE); - }); - } - - EXPECT_TRUE(buffer->empty()); - EXPECT_FALSE(buffer->full()); - - for (int i = 0; i < ITEM_COUNT; ++i) { - std::cout << "source " << source[i] << ", target " << target[i] << std::endl; - EXPECT_EQ(target[i], source[i]); - } -} - -TEST_F(ThreadSafeRingBufferTest, ReadWriteToBuffer) { - - const int TOTAL_ITEMS = 10; // total items to process - const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); - std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); - - EXPECT_TRUE(buffer->empty()); - EXPECT_FALSE(buffer->full()); - - std::thread producer([this, &source]() { - for (int i = 0; i < TOTAL_ITEMS; ++i) { - buffer->write([i, &source](uint8_t* buffer){ - std::memcpy(buffer, &source[i][0], ITEM_SIZE); - }); - } - }); - - std::thread consumer([this, &target]() { - for (int i = 0; i < TOTAL_ITEMS; ++i) { - buffer->read([i, &target](uint8_t* buffer){ - std::memcpy(&target[i][0], buffer, ITEM_SIZE); - }); - } - }); - - producer.join(); - consumer.join(); - - for (int i = 0; i < TOTAL_ITEMS; ++i) { - std::cout << "source " << source[i] << ", target " << target[i] << std::endl; - EXPECT_EQ(target[i], source[i]); - } -} - -TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferWithOverwrite) { - - const int TOTAL_ITEMS = 10; // total items to process - const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); - std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); - - EXPECT_TRUE(buffer->empty()); - EXPECT_FALSE(buffer->full()); - - std::thread producer([this, &source]() { - for (int i = 0; i < TOTAL_ITEMS; ++i) { - buffer->write_overwrite([i, &source](uint8_t* buffer){ - std::memcpy(buffer, &source[i][0], ITEM_SIZE); - }); - } - }); - - // wait for 1 second before starting the consumer thread - // allowing sufficient time for the producer thread to be - // completely done - std::this_thread::sleep_for(1s); - std::thread consumer([this, &target]() { - for (int i = 0; i < TOTAL_ITEMS; ++i) { - buffer->read_timeout([i, &target](uint8_t* buffer){ - if (buffer != nullptr) - std::memcpy(&target[i][0], buffer, ITEM_SIZE); - }, 1s); - } - }); - - producer.join(); - consumer.join(); - - // Since our buffer can host only up to ITEM_COUNT simultanously only the - // last ITEM_COUNT items would have remained in the buffer by the time - // the consumer started processing. - for (int i = 0; i < ITEM_COUNT; ++i) { - std::cout << "source " << source[i] << ", target " << target[i] << std::endl; - EXPECT_EQ(target[i], source[TOTAL_ITEMS-ITEM_COUNT+i]); - } - - for (int i = ITEM_COUNT; i < TOTAL_ITEMS; ++i) { - std::cout << "source " << source[i] << ", target " << target[i] << std::endl; - EXPECT_EQ(target[i], "0000"); - } -}