From 71550ca1b2f645c337a745bb1fe1af8b1dabd358 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 16 Sep 2024 15:08:22 -0700 Subject: [PATCH 1/3] Port ROS-363 to ROS2 --- CHANGELOG.rst | 16 ++ ouster-ros/CMakeLists.txt | 2 +- ouster-ros/config/driver_params.yaml | 11 +- ouster-ros/include/ouster_ros/os_ros.h | 8 + ouster-ros/launch/replay.composite.launch.xml | 14 ++ ouster-ros/launch/replay_pcap.launch.xml | 38 ++-- ouster-ros/launch/sensor.composite.launch.xml | 14 ++ ouster-ros/ouster-sdk | 2 +- ouster-ros/src/impl/cartesian.h | 9 +- ouster-ros/src/imu_packet_handler.h | 35 ++-- ouster-ros/src/lidar_packet_handler.h | 63 +++---- ouster-ros/src/os_cloud_node.cpp | 50 ++++- ouster-ros/src/os_driver_node.cpp | 54 +++++- ouster-ros/src/os_image_node.cpp | 9 +- ouster-ros/src/os_pcap_node.cpp | 88 ++------- ouster-ros/src/os_replay_node.cpp | 4 +- ouster-ros/src/os_sensor_node.cpp | 166 ++++++----------- ouster-ros/src/os_sensor_node.h | 23 +-- ouster-ros/src/point_cloud_compose.h | 51 +++-- ouster-ros/src/point_cloud_processor.h | 15 +- .../src/point_cloud_processor_factory.h | 129 +++++++------ ouster-ros/src/thread_safe_ring_buffer.h | 146 --------------- ouster-ros/test/ring_buffer_test.cpp | 175 ------------------ 23 files changed, 443 insertions(+), 679 deletions(-) delete mode 100644 ouster-ros/src/thread_safe_ring_buffer.h delete mode 100644 ouster-ros/test/ring_buffer_test.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 9c5255b8..800b62c6 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -23,6 +23,22 @@ Changelog - Doesn't handle detect and handle invalid configurations * Added an automatic start mode to make it easier to start the node without using time actions. - To disable set ``auto_start`` to ``false`` during launch +* 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.12.0 diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 220b0c72..de3633b6 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -46,6 +46,7 @@ set(BUILD_SHARED_LIBS OFF) option(BUILD_VIZ "Turn off building VIZ" OFF) option(BUILD_PCAP "Turn off building PCAP" OFF) +option(BUILD_OSF "Turn off building OSF" ON) find_package(OusterSDK REQUIRED) set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) @@ -189,7 +190,6 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test src/os_ros.cpp test/test_main.cpp - test/ring_buffer_test.cpp test/lock_free_ring_buffer_test.cpp test/point_accessor_test.cpp test/point_transform_test.cpp diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index b69d7ca8..da3b38ab 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -59,7 +59,7 @@ ouster/os_driver: point_cloud_frame: os_lidar # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and # SCAN to enable or disable their respective messages. - proc_mask: IMG|PCL|IMU|SCAN + proc_mask: IMU|PCL|SCAN|IMG|RAW # scan_ring[optional]: use this parameter in conjunction with the SCAN flag # to select which beam of the LidarScan to use when producing the LaserScan # message. Choose a value the range [0, sensor_beams_count). @@ -102,3 +102,12 @@ ouster/os_driver: # max_failed_reconnect_attempts[optional]: maximum number of attempts trying # to communicate with the sensor. Counter resets upon successful connection. max_failed_reconnect_attempts: 2147483647 + # organized[optional]: whether to generate an organized point cloud. default + # is organized. + organized: true + # destagger[optional]: enable or disable point cloud destaggering, default enabled. + destagger: true + # min_range[optional]: minimum lidar range to consider (meters). + min_range: 0 + # max_range[optional]: maximum lidar range to consider (meters). + max_range: 1000 diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 10fbf66b..9a08291c 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -158,6 +158,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/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 1625f16e..0dcb7859 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -62,6 +62,16 @@ xyzir }"/> + + + + + + @@ -80,6 +90,10 @@ + + + + diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml index 6c90f40d..1585b774 100644 --- a/ouster-ros/launch/replay_pcap.launch.xml +++ b/ouster-ros/launch/replay_pcap.launch.xml @@ -53,6 +53,16 @@ xyzir }"/> + + + + + + @@ -62,20 +72,24 @@ - - - - - - - - - - + + + + + + + + + + + + + + - - + + diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index cdc73854..297cd568 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -93,6 +93,16 @@ + + + + + + @@ -124,6 +134,10 @@ + + + + diff --git a/ouster-ros/ouster-sdk b/ouster-ros/ouster-sdk index 2898060f..483206f0 160000 --- a/ouster-ros/ouster-sdk +++ b/ouster-ros/ouster-sdk @@ -1 +1 @@ -Subproject commit 2898060fb47a69b4fbf9807d3c17f6fdc8249379 +Subproject commit 483206f0baeffbbc11ee0883d54a651a5216563b diff --git a/ouster-ros/src/impl/cartesian.h b/ouster-ros/src/impl/cartesian.h index 98b80616..443c8313 100644 --- a/ouster-ros/src/impl/cartesian.h +++ b/ouster-ros/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/ouster-ros/src/imu_packet_handler.h b/ouster-ros/src/imu_packet_handler.h index 6b7195bd..403772f6 100644 --- a/ouster-ros/src/imu_packet_handler.h +++ b/ouster-ros/src/imu_packet_handler.h @@ -19,36 +19,39 @@ namespace ouster_ros { class ImuPacketHandler { public: using HandlerOutput = sensor_msgs::msg::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 rclcpp::Clock(RCL_ROS_TIME).now(); - }}; + timestamper = Timestamper{ + [](const sensor::ImuPacket& imu_packet) { + return rclcpp::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 rclcpp::Time(ts); }}; } else { - timestamper = Timestamper{[pf](const uint8_t* imu_buf) { - return rclcpp::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 rclcpp::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/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 9400e16e..0544accf 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -18,6 +18,7 @@ #include "lock_free_ring_buffer.h" #include +#include namespace { @@ -46,33 +47,25 @@ 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 { namespace sensor = ouster::sensor; -using LidarScanProcessor = std::function; +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 +96,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); }}; } } @@ -134,6 +128,7 @@ class LidarPacketHandler { lidar_scans_processing_thread->join(); } } + void register_lidar_scan_handler(LidarScanProcessor handler) { lidar_scan_handlers.push_back(handler); } @@ -142,13 +137,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(); } }; @@ -175,7 +170,7 @@ class LidarPacketHandler { lidar_scan_estimated_msg_ts); } - // why we hit percent amount of the ring_buffer capacity throlle + // why we hit percent amount of the ring_buffer capacity throttle size_t read_step = 1; if (ring_buffer.size() > THROTTLE_PERCENT * ring_buffer.capacity()) { RCLCPP_WARN(rclcpp::get_logger(getName()), @@ -198,7 +193,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, @@ -206,7 +201,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 @@ -273,7 +268,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()) { RCLCPP_WARN(rclcpp::get_logger(getName()), @@ -283,7 +278,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 = rclcpp::Time(lidar_scan_estimated_ts); @@ -293,7 +288,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 +300,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 +314,11 @@ class LidarPacketHandler { } bool lidar_handler_ros_time(const sensor::packet_format& pf, - const uint8_t* lidar_buf) { - auto packet_receive_time = rclcpp::Clock(RCL_ROS_TIME).now(); + const sensor::LidarPacket& lidar_packet) { + auto packet_receive_time = rclcpp::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; } @@ -336,12 +331,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/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 39ccbf58..b28ddc72 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -60,6 +60,11 @@ class OusterCloud : public OusterProcessingNodeBase { declare_parameter("use_system_default_qos", false); declare_parameter("scan_ring", 0); declare_parameter("point_type", "original"); + declare_parameter("organized", true); + declare_parameter("destagger", true); + declare_parameter("min_range", 0.0); + declare_parameter("max_range", 1000.0); + declare_parameter("rows_step", 1); } void metadata_handler( @@ -95,8 +100,15 @@ class OusterCloud : public OusterProcessingNodeBase { imu_packet_sub = create_subscription( "imu_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { - auto imu_msg = imu_packet_handler(msg->buf.data()); - imu_pub->publish(imu_msg); + if (imu_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::ImuPacket imu_packet(msg->buf.size()); + memcpy(imu_packet.buf.data(), msg->buf.data(), msg->buf.size()); + imu_packet.host_timestamp = static_cast(rclcpp::Clock(RCL_ROS_TIME).now().nanoseconds()); + auto imu_msg = imu_packet_handler(imu_packet); + imu_pub->publish(imu_msg); + } }); } @@ -111,11 +123,30 @@ class OusterCloud : public OusterProcessingNodeBase { } auto point_type = get_parameter("point_type").as_string(); + auto organized = get_parameter("organized").as_bool(); + auto destagger = get_parameter("destagger").as_bool(); + auto min_range_m = get_parameter("min_range").as_double(); + auto max_range_m = get_parameter("max_range").as_double(); + if (min_range_m < 0.0 || max_range_m < 0.0) { + RCLCPP_FATAL(get_logger(), "min_range and max_range need to be positive"); + throw std::runtime_error("negative range limits!"); + } + if (min_range_m >= max_range_m) { + RCLCPP_FATAL(get_logger(), "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 = get_parameter("rows_step").as_int(); processors.push_back( - PointCloudProcessorFactory::create_point_cloud_processor(point_type, info, - tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), + 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]); } ) ); @@ -162,7 +193,14 @@ class OusterCloud : public OusterProcessingNodeBase { lidar_packet_sub = create_subscription( "lidar_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { - 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(rclcpp::Clock(RCL_ROS_TIME).now().nanoseconds()); + lidar_packet_handler(lidar_packet); + } }); } } diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index d4b3dd37..7e6be202 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -26,6 +26,8 @@ namespace ouster_ros { namespace sensor = ouster::sensor; +using ouster::sensor::LidarPacket; +using ouster::sensor::ImuPacket; class OusterDriver : public OusterSensor { public: @@ -34,15 +36,19 @@ class OusterDriver : public OusterSensor { : OusterSensor("os_driver", options), tf_bcast(this) { tf_bcast.declare_parameters(); tf_bcast.parse_parameters(); - declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN"); + declare_parameter("proc_mask", "IMU|PCL|SCAN|IMG|RAW"); declare_parameter("scan_ring", 0); declare_parameter("ptp_utc_tai_offset", -37.0); declare_parameter("point_type", "original"); + declare_parameter("organized", true); + declare_parameter("destagger", true); + declare_parameter("min_range", 0.0); + declare_parameter("max_range", 1000.0); + declare_parameter("rows_step", 1); } ~OusterDriver() override { RCLCPP_DEBUG(get_logger(), "OusterDriver::~OusterDriver() called"); - halt(); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { @@ -84,11 +90,30 @@ class OusterDriver : public OusterSensor { } auto point_type = get_parameter("point_type").as_string(); + auto organized = get_parameter("organized").as_bool(); + auto destagger = get_parameter("destagger").as_bool(); + auto min_range_m = get_parameter("min_range").as_double(); + auto max_range_m = get_parameter("max_range").as_double(); + if (min_range_m < 0.0 || max_range_m < 0.0) { + RCLCPP_FATAL(get_logger(), "min_range and max_range need to be positive"); + throw std::runtime_error("negative range limits!"); + } + if (min_range_m >= max_range_m) { + RCLCPP_FATAL(get_logger(), "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 = get_parameter("rows_step").as_int(); processors.push_back( - PointCloudProcessorFactory::create_point_cloud_processor(point_type, info, - tf_bcast.point_cloud_frame_id(), tf_bcast.apply_lidar_to_sensor_transform(), + 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]); } ) ); @@ -173,15 +198,24 @@ class OusterDriver : public OusterSensor { lidar_packet_handler = LidarPacketHandler::create_handler( info, processors, timestamp_mode, static_cast(ptp_utc_tai_offset * 1e+9)); + + publish_raw = impl::check_token(tokens, "RAW"); } - 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 { + virtual void on_imu_packet_msg(const ImuPacket& imu_packet) override { if (imu_packet_handler) - imu_pub->publish(imu_packet_handler(raw_imu_packet)); + imu_pub->publish(imu_packet_handler(imu_packet)); + + if(publish_raw) + OusterSensor::on_imu_packet_msg(imu_packet); } virtual void cleanup() override { @@ -207,6 +241,8 @@ class OusterDriver : public OusterSensor { image_pubs; ImuPacketHandler::HandlerType imu_packet_handler; LidarPacketHandler::HandlerType lidar_packet_handler; + + bool publish_raw = false; }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index f820a489..9078fb48 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -110,7 +110,14 @@ class OusterImage : public OusterProcessingNodeBase { lidar_packet_sub = create_subscription( "lidar_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { - 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(rclcpp::Clock(RCL_ROS_TIME).now().nanoseconds()); + lidar_packet_handler(lidar_packet); + } }); } diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp index 0265027c..f984a64a 100644 --- a/ouster-ros/src/os_pcap_node.cpp +++ b/ouster-ros/src/os_pcap_node.cpp @@ -22,7 +22,6 @@ #include "ouster_ros/os_sensor_node_base.h" #include "ouster_ros/visibility_control.h" -#include "thread_safe_ring_buffer.h" #include namespace sensor = ouster::sensor; @@ -41,9 +40,14 @@ class OusterPcap : public OusterSensorNodeBase { declare_parameters(); } + ~OusterPcap() override { + RCLCPP_DEBUG(get_logger(), "OusterPcap::~OusterPcap() is called."); + stop_packet_read_thread(); + } + LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State&) { - RCLCPP_INFO(get_logger(), "on_configure() is called."); + RCLCPP_DEBUG(get_logger(), "on_configure() is called."); try { auto meta_file = get_meta_file(); @@ -74,7 +78,6 @@ class OusterPcap : public OusterSensorNodeBase { if (imu_packet_pub) imu_packet_pub->on_activate(); if (lidar_packet_pub) lidar_packet_pub->on_activate(); allocate_buffers(); - start_packet_processing_threads(); start_packet_read_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -90,6 +93,7 @@ class OusterPcap : public OusterSensorNodeBase { const rclcpp_lifecycle::State& state) { RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); LifecycleNode::on_deactivate(state); + stop_packet_read_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -169,21 +173,13 @@ class OusterPcap : public OusterSensorNodeBase { 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() { - bool use_system_default_qos = - get_parameter("use_system_default_qos").as_bool(); + 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 = @@ -217,45 +213,6 @@ class OusterPcap : public OusterSensorNodeBase { } } - 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); - }); - } - RCLCPP_DEBUG(get_logger(), "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); - }); - } - - RCLCPP_DEBUG(get_logger(), - "lidar_packets_processing_thread done."); - }); - } - - void stop_packet_processing_threads() { - RCLCPP_DEBUG(get_logger(), "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 @@ -284,20 +241,16 @@ class OusterPcap : public OusterSensorNodeBase { using namespace std::chrono_literals; const auto UPDATE_PERIOD = duration_cast(1s); - while (payload_size) { + while (rclcpp::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 { RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 1, "unknown packet /w port:" << packet_info.dst_port); @@ -330,17 +283,8 @@ class OusterPcap : public OusterSensorNodeBase { rclcpp_lifecycle::LifecyclePublisher::SharedPtr 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/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp index f630dc64..b9a7ab65 100644 --- a/ouster-ros/src/os_replay_node.cpp +++ b/ouster-ros/src/os_replay_node.cpp @@ -101,9 +101,9 @@ class OusterReplay : public OusterSensorNodeBase { std::string parse_parameters() { auto meta_file = get_parameter("metadata").as_string(); if (!is_arg_set(meta_file)) { - RCLCPP_ERROR(get_logger(), + RCLCPP_FATAL(get_logger(), "Must specify metadata file in replay mode"); - throw std::runtime_error("metadata no specificed"); + throw std::runtime_error("metadata not specificed"); } return meta_file; } diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 343127c9..281a073d 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -20,9 +20,12 @@ using ouster_sensor_msgs::msg::PacketMsg; using ouster_sensor_msgs::srv::GetConfig; using ouster_sensor_msgs::srv::SetConfig; +using std::to_string; using namespace std::chrono_literals; using namespace std::string_literals; -using std::to_string; + +using sensor::LidarPacket; +using sensor::ImuPacket; namespace ouster_ros { @@ -58,11 +61,6 @@ OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) OusterSensor::~OusterSensor() { RCLCPP_DEBUG(get_logger(), "OusterSensor::~OusterSensor() called"); - halt(); -} - -void OusterSensor::halt() { - stop_packet_processing_threads(); stop_sensor_connection_thread(); } @@ -167,7 +165,6 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_activate( update_metadata(*sensor_client); create_publishers(); allocate_buffers(); - start_packet_processing_threads(); start_sensor_connection_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -183,7 +180,7 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_deactivate( const rclcpp_lifecycle::State& state) { RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); LifecycleNode::on_deactivate(state); - halt(); + stop_sensor_connection_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -214,7 +211,6 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_shutdown( if (state.label() == "active") { stop_sensor_connection_thread(); - stop_packet_processing_threads(); } // whether state was 'active' or 'inactive' do cleanup @@ -237,7 +233,7 @@ std::string OusterSensor::get_sensor_hostname() { hostname = get_parameter("lidar_ip").as_string(); if (!is_arg_set(hostname)) { auto error_msg = "Must specify a sensor hostname"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } } @@ -536,7 +532,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]."; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } @@ -544,7 +540,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]."; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } @@ -556,7 +552,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; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } } @@ -567,7 +563,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; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } } @@ -588,7 +584,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (!timestamp_mode) { auto error_msg = "Invalid timestamp mode: " + timestamp_mode_arg; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } } @@ -638,7 +634,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); - RCLCPP_ERROR_STREAM(get_logger(), error_msg); + RCLCPP_FATAL_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } @@ -720,7 +716,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) RCLCPP_WARN( get_logger(), @@ -773,20 +769,15 @@ void OusterSensor::create_publishers() { void OusterSensor::allocate_buffers() { auto& pf = sensor::get_format(info); + lidar_packet_msg.buf.resize(pf.lidar_packet_size); 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); + imu_packet.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; } @@ -806,7 +797,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) { - RCLCPP_ERROR_STREAM( + RCLCPP_ERROR( get_logger(), "maximum number of allowed errors from " "sensor::poll_client() reached, performing self reset..."); @@ -817,44 +808,40 @@ 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? + RCLCPP_WARN(get_logger(), "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) { + RCLCPP_ERROR( + get_logger(), + "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 circuit reset if no breaking changes occured? - RCLCPP_WARN(get_logger(), - "sensor init_id has changed! reactivating.."); - reactivate_sensor(); - } - } else { - if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { - RCLCPP_ERROR_STREAM( - get_logger(), - "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) { - RCLCPP_ERROR_STREAM( - get_logger(), - "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) { + RCLCPP_ERROR( + get_logger(), + "maximum number of allowed errors from " + "sensor::read_imu_packet() reached, reactivating..."); + read_imu_packet_errors = 0; + reactivate_sensor(true); } - }); + } } void OusterSensor::cleanup() { @@ -865,8 +852,6 @@ void OusterSensor::cleanup() { get_config_srv.reset(); set_config_srv.reset(); sensor_connection_thread.reset(); - imu_packets_processing_thread.reset(); - lidar_packets_processing_thread.reset(); } void OusterSensor::connection_loop(sensor::client& cli, @@ -894,7 +879,7 @@ void OusterSensor::start_sensor_connection_thread() { sensor_connection_thread = std::make_unique([this]() { RCLCPP_DEBUG(get_logger(), "sensor_connection_thread active."); auto& pf = sensor::get_format(info); - while (sensor_connection_active) { + while (rclcpp::ok() && sensor_connection_active) { connection_loop(*sensor_client, pf); } RCLCPP_DEBUG(get_logger(), "sensor_connection_thread done."); @@ -909,59 +894,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 != nullptr) on_imu_packet_msg(buffer); - }, 1s); - } - RCLCPP_DEBUG(get_logger(), "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 != nullptr) on_lidar_packet_msg(buffer); - }, 1s); - } - RCLCPP_DEBUG(get_logger(), "lidar_packets_processing_thread done."); - }); -} - -void OusterSensor::stop_packet_processing_threads() { - RCLCPP_DEBUG(get_logger(), "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); } } // namespace ouster_ros diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index e27e2ea4..fc2049cf 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -26,7 +26,6 @@ #include "ouster_ros/visibility_control.h" #include "ouster_ros/os_sensor_node_base.h" -#include "thread_safe_ring_buffer.h" namespace sensor = ouster::sensor; using lifecycle_msgs::srv::ChangeState; @@ -61,16 +60,14 @@ class OusterSensor : public OusterSensorNodeBase { 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); virtual void cleanup(); bool start(); - void halt(); - private: void declare_parameters(); @@ -120,7 +117,7 @@ class OusterSensor : public OusterSensorNodeBase { 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(); @@ -137,10 +134,6 @@ class OusterSensor : public OusterSensorNodeBase { 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); @@ -150,8 +143,10 @@ class OusterSensor : public OusterSensorNodeBase { std::string mtp_dest; bool mtp_main; std::shared_ptr sensor_client; - ouster_sensor_msgs::msg::PacketMsg lidar_packet; - ouster_sensor_msgs::msg::PacketMsg imu_packet; + ouster_sensor_msgs::msg::PacketMsg lidar_packet_msg; + ouster_sensor_msgs::msg::PacketMsg imu_packet_msg; + ouster::sensor::LidarPacket lidar_packet; + ouster::sensor::ImuPacket imu_packet; rclcpp::Publisher::SharedPtr lidar_packet_pub; rclcpp::Publisher::SharedPtr imu_packet_pub; rclcpp::Service::SharedPtr reset_srv; @@ -159,10 +154,6 @@ class OusterSensor : public OusterSensorNodeBase { rclcpp::Service::SharedPtr set_config_srv; std::shared_ptr> change_state_client; - // 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; diff --git a/ouster-ros/src/point_cloud_compose.h b/ouster-ros/src/point_cloud_compose.h index 8aa2a79f..6b425438 100644 --- a/ouster-ros/src/point_cloud_compose.h +++ b/ouster-ros/src/point_cloud_compose.h @@ -41,7 +41,7 @@ struct TypeSelector { /** * @brief constructs a suitable tuple at compile time that can store a reference * to all the fields of a specific LidarScan object (without conversion) - * according to the information specified by the ChanFieldTable. + * according to the information specificed by the ChanFieldTable. */ template & Table> constexpr auto make_lidar_scan_tuple() { @@ -79,7 +79,7 @@ void map_lidar_scan_fields_to_tuple(Tuple& tp, const ouster::LidarScan& ls) { /** * @brief constructs a suitable tuple at compile time that can store a reference * to all the fields of a specific LidarScan object (without conversion) - * according to the information specified by the ChanFieldTable and directly + * 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 @@ -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 knowing 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/ouster-ros/src/point_cloud_processor.h b/ouster-ros/src/point_cloud_processor.h index 558f3bec..a5491525 100644 --- a/ouster-ros/src/point_cloud_processor.h +++ b/ouster-ros/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_) { @@ -80,6 +83,7 @@ class PointCloudProcessor { auto range_channel = static_cast(sensor::ChanField::RANGE + i); auto range = lidar_scan.field(range_channel); 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 rclcpp::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/ouster-ros/src/point_cloud_processor_factory.h b/ouster-ros/src/point_cloud_processor_factory.h index ba59f280..03023141 100644 --- a/ouster-ros/src/point_cloud_processor_factory.h +++ b/ouster-ros/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/ouster-ros/src/thread_safe_ring_buffer.h b/ouster-ros/src/thread_safe_ring_buffer.h deleted file mode 100644 index 26b5c88d..00000000 --- a/ouster-ros/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/ouster-ros/test/ring_buffer_test.cpp b/ouster-ros/test/ring_buffer_test.cpp deleted file mode 100644 index e68ab29a..00000000 --- a/ouster-ros/test/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"); - } -} From 4884473458cb7efbfb3555c9d7b2315489459311 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 16 Sep 2024 15:15:44 -0700 Subject: [PATCH 2/3] Turn off OSF --- ouster-ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index de3633b6..e41dfae2 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -46,7 +46,7 @@ set(BUILD_SHARED_LIBS OFF) option(BUILD_VIZ "Turn off building VIZ" OFF) option(BUILD_PCAP "Turn off building PCAP" OFF) -option(BUILD_OSF "Turn off building OSF" ON) +option(BUILD_OSF "Turn off building OSF" OFF) find_package(OusterSDK REQUIRED) set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) From 6e313a8cc07f612621faf8d3f08386a49f426420 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 20 Sep 2024 13:46:16 -0700 Subject: [PATCH 3/3] Update package version and CHANGELOG.rst + up the default max range to 10000.0 meters --- CHANGELOG.rst | 4 ++-- ouster-ros/config/driver_params.yaml | 4 ++-- ouster-ros/launch/replay.composite.launch.xml | 2 +- ouster-ros/launch/replay_pcap.launch.xml | 2 +- ouster-ros/launch/sensor.composite.launch.xml | 2 +- ouster-ros/package.xml | 2 +- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 800b62c6..f9c87daa 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog ========= -[unreleased] -============ +ouster_ros v0.13.0 +================== * [BUGFIX]: LaserScan is not properly aligned with generated point cloud * address an issue where LaserScan appeared different on FW prior to 2.4 * [BUGFIX]: LaserScan does not work when using dual mode diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index da3b38ab..8f6f5b40 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -108,6 +108,6 @@ ouster/os_driver: # destagger[optional]: enable or disable point cloud destaggering, default enabled. destagger: true # min_range[optional]: minimum lidar range to consider (meters). - min_range: 0 + min_range: 0.0 # max_range[optional]: maximum lidar range to consider (meters). - max_range: 1000 + max_range: 1000.0 diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 0dcb7859..dc50ed9b 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -69,7 +69,7 @@ - diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml index 1585b774..bb758771 100644 --- a/ouster-ros/launch/replay_pcap.launch.xml +++ b/ouster-ros/launch/replay_pcap.launch.xml @@ -60,7 +60,7 @@ - diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 297cd568..8d1c40f6 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -100,7 +100,7 @@ - diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 20d79eb3..b9a7ba90 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.12.7 + 0.13.0 Ouster ROS2 driver ouster developers BSD