diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 7bd6aa06..9757c10a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,6 +5,10 @@ Changelog [unreleased] ============ * breaking: publish PCL point clouds destaggered. +* introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds + to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode + is used. +* fix: destagger columns timestamp when generating destaggered point clouds. ouster_ros v0.10.0 diff --git a/Dockerfile b/Dockerfile index e9430cff..7f13aae8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,7 +3,7 @@ ARG ROS_DISTRO=rolling FROM ros:${ROS_DISTRO}-ros-core AS build-env ENV DEBIAN_FRONTEND=noninteractive \ BUILD_HOME=/var/lib/build \ - OUSTER_ROS_PATH=/opt/catkin_ws/src/ouster-ros + OUSTER_ROS_PATH=/opt/ros2_ws/src/ouster-ros RUN set -xue \ # Turn off installing extra packages globally to slim down rosdep install @@ -28,21 +28,15 @@ RUN set -xe \ && groupadd -o -g ${BUILD_GID} build \ && useradd -o -u ${BUILD_UID} -d ${BUILD_HOME} -rm -s /bin/bash -g build build -# Install build dependencies using rosdep -COPY --chown=build:build \ - ouster-ros/package.xml \ - $OUSTER_ROS_PATH/ouster-ros/package.xml +# Set up build environment +COPY --chown=build:build . $OUSTER_ROS_PATH RUN set -xe \ && apt-get update \ && rosdep init \ && rosdep update --rosdistro=$ROS_DISTRO \ -&& rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src -r -# using -r for now to prevent rosdep from complaining within iron - +&& rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src -# Set up build environment -COPY --chown=build:build . $OUSTER_ROS_PATH USER build:build WORKDIR ${BUILD_HOME} diff --git a/LICENSE b/LICENSE index 2d09cf12..50fd48e4 100644 --- a/LICENSE +++ b/LICENSE @@ -4,7 +4,7 @@ Upstream-Contact: Ouster Sensor SDK Developers Source: https://github.com/ouster-lidar/ouster_example Files: * -Copyright: 2018-2022 Ouster, Inc +Copyright: 2018-2023 Ouster, Inc License: BSD-3-Clause Files: include/optional-lite/* diff --git a/README.md b/README.md index af473d32..ca202634 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [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 (foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy) +[ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)

@@ -10,7 +10,7 @@ |:-----------:|:------:| | 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 (foxy) | [![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) +| ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) - [Overview](#overview) - [Requirements](#requirements) @@ -50,7 +50,7 @@ remainder of this guide. > If you have _rosdep_ tool installed on your system you can then use the following command to get all required dependencies: ``` - rosdep install -y --from-paths $OUSTER_ROS_PATH -r + rosdep install --from-paths $OUSTER_ROS_PATH -y --ignore-src ``` ### Linux diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 13edc256..0520ab0d 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -26,6 +26,9 @@ ouster/os_driver: # packet of a LidarScan as the timestamp of the IMU, # PointCloud2 and LaserScan messages. timestamp_mode: '' + # ptp_utc_tai_offset[optional]: UTC/TAI offset in seconds to apply when + # TIME_FROM_PTP_1588 timestamp mode is used. + ptp_utc_tai_offset: -37.0 # udp_profile_lidar[optional]: lidar packet profile; possible values: # - LEGACY: not recommended # - RNG19_RFL8_SIG16_NIR16_DUAL diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index a2deb92a..13fdcf58 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -22,6 +22,7 @@ ouster/os_cloud: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode + ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588 proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options use_system_default_qos: False # needs to match the value defined for os_sensor node scan_ring: 0 # Use this parameter in conjunction with the SCAN flag and choose a diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index f11b5ae3..214d5b37 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -211,6 +211,17 @@ inline ouster::img_t get_or_fill_zero(sensor::ChanField field, ouster::impl::visit_field(ls, field, read_and_cast(), result); return result; } + +/** + * simple utility function that ensures we don't wrap around uint64_t due + * to a negative value being bigger than ts value in absolute terms. + * @remark method does not check upper boundary + */ +inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) { + return offset < 0 && ts < static_cast(std::abs(offset)) ? 0 : ts + offset; +} + + } // namespace impl } // namespace ouster_ros diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index d33b9950..51196283 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -33,6 +33,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 5b5ebbcf..649ba6b6 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -10,6 +10,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + @@ -87,6 +90,7 @@ + --qos-profile-overrides-path + $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/> diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 9e11b97f..a96642f7 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -33,6 +33,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 4859da5f..6c1bf364 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -33,6 +33,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index cc623d89..a7da2b7a 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -41,6 +41,8 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> + + diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 8025c8bc..5abf165c 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.10.1 + 0.10.2 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/image_processor.h b/ouster-ros/src/image_processor.h index b2f4aa98..5249a60a 100644 --- a/ouster-ros/src/image_processor.h +++ b/ouster-ros/src/image_processor.h @@ -149,7 +149,7 @@ class ImageProcessor { for (size_t v = 0; v < W; v++) { const size_t vv = (v + W - px_offset[u]) % W; const size_t idx = u * W + vv; - // TODO: re-examine this truncation later + // TODO: re-examine this truncation later // 16 bit img: use 4mm resolution and throw out returns > 260m auto r = (rg[idx] + 0b10) >> 2; range_image_map(u, v) = r > pixel_value_max ? 0 : r; diff --git a/ouster-ros/src/imu_packet_handler.h b/ouster-ros/src/imu_packet_handler.h index 68bbac29..6b7195bd 100644 --- a/ouster-ros/src/imu_packet_handler.h +++ b/ouster-ros/src/imu_packet_handler.h @@ -8,7 +8,7 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -24,21 +24,33 @@ class ImuPacketHandler { public: static HandlerType create_handler(const ouster::sensor::sensor_info& info, const std::string& frame, - bool use_ros_time) { + const std::string& timestamp_mode, + int64_t ptp_utc_tai_offset) { const auto& pf = ouster::sensor::get_format(info); using Timestamper = std::function; - // clang-format off - auto timestamper = use_ros_time ? - Timestamper{[](const uint8_t* /*imu_buf*/) { - return rclcpp::Clock(RCL_ROS_TIME).now(); }} : - Timestamper{[pf](const uint8_t* imu_buf) { - return rclcpp::Time(pf.imu_gyro_ts(imu_buf)); }}; - // clang-format on + Timestamper timestamper; + + if (timestamp_mode == "TIME_FROM_ROS_TIME") { + timestamper = Timestamper{[](const uint8_t* /*imu_buf*/) { + return rclcpp::Clock(RCL_ROS_TIME).now(); + }}; + } 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); + 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)); + }}; + } + return [&pf, &frame, timestamper](const uint8_t* imu_buf) { - return packet_to_imu_msg(pf, timestamper(imu_buf), - frame, imu_buf); + return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf); }; } }; -} // namespace ouster_ros \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/laser_scan_processor.h b/ouster-ros/src/laser_scan_processor.h index 6e1502bc..63dae838 100644 --- a/ouster-ros/src/laser_scan_processor.h +++ b/ouster-ros/src/laser_scan_processor.h @@ -14,7 +14,6 @@ #include "ouster_ros/os_ros.h" // clang-format on - namespace ouster_ros { class LaserScanProcessor { @@ -66,4 +65,4 @@ class LaserScanProcessor { PostProcessingFn post_processing_fn; }; -} // namespace ouster_ros \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 75de6a58..6d7cf802 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -8,7 +8,7 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -92,7 +92,10 @@ class LidarPacketHandler { public: LidarPacketHandler(const ouster::sensor::sensor_info& info, - bool use_ros_time) { + const std::vector& handlers, + const std::string& timestamp_mode, + int64_t ptp_utc_tai_offset) + : lidar_scan_handlers{handlers} { // initialize lidar_scan processor and buffer scan_batcher = std::make_unique(info); lidar_scan = std::make_unique( @@ -106,14 +109,23 @@ class LidarPacketHandler { }; const sensor::packet_format& pf = sensor::get_format(info); - lidar_packet_accumlator = - use_ros_time - ? LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { - return lidar_handler_ros_time(pf, lidar_buf); - }} - : LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { - return lidar_handler_sensor_time(pf, lidar_buf); - }}; + 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); + }}; + } 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, + ptp_utc_tai_offset); + }}; + } else { + lidar_packet_accumlator = + LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_sensor_time(pf, lidar_buf); + }}; + } } LidarPacketHandler(const LidarPacketHandler&) = delete; @@ -128,12 +140,11 @@ class LidarPacketHandler { public: static HandlerType create_handler( - const ouster::sensor::sensor_info& info, bool use_ros_time, - const std::vector& handlers) { - auto handler = std::make_shared(info, use_ros_time); - - handler->lidar_scan_handlers = handlers; - + const ouster::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)) { for (auto h : handler->lidar_scan_handlers) { @@ -201,7 +212,6 @@ class LidarPacketHandler { assert(idx != ts_v.data() + ts_v.size()); // should never happen int curr_scan_first_nonzero_idx = idx - ts_v.data(); uint64_t curr_scan_first_nonzero_value = *idx; - uint64_t scan_ns = curr_scan_first_nonzero_idx == 0 ? curr_scan_first_nonzero_value : impute_value(last_scan_last_nonzero_idx, @@ -209,7 +219,6 @@ class LidarPacketHandler { curr_scan_first_nonzero_idx, curr_scan_first_nonzero_value, static_cast(ts_v.size())); - last_scan_last_nonzero_idx = find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); assert(last_scan_last_nonzero_idx >= 0); // should never happen @@ -240,6 +249,18 @@ class LidarPacketHandler { return true; } + bool lidar_handler_sensor_time_ptp(const sensor::packet_format&, + const uint8_t* lidar_buf, + int64_t ptp_utc_tai_offset) { + if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; + auto ts_v = lidar_scan->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); + lidar_scan_estimated_ts = compute_scan_ts(ts_v); + lidar_scan_estimated_msg_ts = rclcpp::Time(lidar_scan_estimated_ts); + return true; + } + 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(); @@ -287,4 +308,4 @@ class LidarPacketHandler { LidarPacketAccumlator lidar_packet_accumlator; }; -} // namespace ouster_ros \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 9a381d20..a111242b 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -39,7 +39,7 @@ class OusterCloud : public OusterProcessingNodeBase { public: OUSTER_ROS_PUBLIC explicit OusterCloud(const rclcpp::NodeOptions& options) - : OusterProcessingNodeBase("os_cloud", options), os_tf_bcast(this) { + : OusterProcessingNodeBase("os_cloud", options), tf_bcast(this) { on_init(); } @@ -50,37 +50,34 @@ class OusterCloud : public OusterProcessingNodeBase { void on_init() { declare_parameters(); - parse_parameters(); + tf_bcast.parse_parameters(); create_metadata_subscriber( [this](const auto& msg) { metadata_handler(msg); }); RCLCPP_INFO(get_logger(), "OusterCloud: node initialized!"); } void declare_parameters() { - os_tf_bcast.declare_parameters(); + tf_bcast.declare_parameters(); declare_parameter("timestamp_mode", ""); + declare_parameter("ptp_utc_tai_offset", -37.0); declare_parameter("proc_mask", "IMU|PCL|SCAN"); declare_parameter("use_system_default_qos", false); declare_parameter("scan_ring", 0); } - void parse_parameters() { - os_tf_bcast.parse_parameters(); - auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || - timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; - } - void metadata_handler( const std_msgs::msg::String::ConstSharedPtr& metadata_msg) { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); - os_tf_bcast.broadcast_transforms(info); + tf_bcast.broadcast_transforms(info); create_publishers_subscriptions(info); } void create_publishers_subscriptions(const sensor::sensor_info& info) { + auto timestamp_mode = get_parameter("timestamp_mode").as_string(); + auto ptp_utc_tai_offset = + get_parameter("ptp_utc_tai_offset").as_double(); bool use_system_default_qos = get_parameter("use_system_default_qos").as_bool(); @@ -93,9 +90,11 @@ class OusterCloud : public OusterProcessingNodeBase { auto tokens = parse_tokens(proc_mask, '|'); if (check_token(tokens, "IMU")) { - imu_pub = create_publisher("imu", selected_qos); + imu_pub = + create_publisher("imu", selected_qos); imu_packet_handler = ImuPacketHandler::create_handler( - info, os_tf_bcast.imu_frame_id(), use_ros_time); + info, tf_bcast.imu_frame_id(), timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); imu_packet_sub = create_subscription( "imu_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { @@ -114,8 +113,8 @@ class OusterCloud : public OusterProcessingNodeBase { topic_for_return("points", i), selected_qos); } processors.push_back(PointCloudProcessor::create( - info, os_tf_bcast.point_cloud_frame_id(), - os_tf_bcast.apply_lidar_to_sensor_transform(), + info, tf_bcast.point_cloud_frame_id(), + tf_bcast.apply_lidar_to_sensor_transform(), [this](PointCloudProcessor::OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i]->publish(*msgs[i]); @@ -140,12 +139,8 @@ class OusterCloud : public OusterProcessingNodeBase { } processors.push_back(LaserScanProcessor::create( - info, - os_tf_bcast - .point_cloud_frame_id(), // TODO: should we allow having a - // different frame for the laser - // scan than point cloud??? - scan_ring, [this](LaserScanProcessor::OutputType msgs) { + info, tf_bcast.lidar_frame_id(), scan_ring, + [this](LaserScanProcessor::OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]); })); @@ -153,7 +148,8 @@ class OusterCloud : public OusterProcessingNodeBase { if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) { lidar_packet_handler = LidarPacketHandler::create_handler( - info, use_ros_time, processors); + info, processors, timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); lidar_packet_sub = create_subscription( "lidar_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { @@ -172,8 +168,7 @@ class OusterCloud : public OusterProcessingNodeBase { std::vector::SharedPtr> scan_pubs; - OusterStaticTransformsBroadcaster os_tf_bcast; - bool use_ros_time; + OusterStaticTransformsBroadcaster tf_bcast; ImuPacketHandler::HandlerType imu_packet_handler; LidarPacketHandler::HandlerType lidar_packet_handler; diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 8d47a13d..839d33e6 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -3,11 +3,11 @@ * All rights reserved. * * @file os_driver.cpp - * @brief This node combines the capabilities of os_sensor, os_cloud and os_image - * into a single ROS node/component + * @brief This node combines the capabilities of os_sensor, os_cloud and + * os_image into a single ROS node/component */ -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -30,16 +30,17 @@ class OusterDriver : public OusterSensor { public: OUSTER_ROS_PUBLIC explicit OusterDriver(const rclcpp::NodeOptions& options) - : OusterSensor("os_driver", options), os_tf_bcast(this) { - os_tf_bcast.declare_parameters(); - os_tf_bcast.parse_parameters(); + : OusterSensor("os_driver", options), tf_bcast(this) { + tf_bcast.declare_parameters(); + tf_bcast.parse_parameters(); declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN"); declare_parameter("scan_ring", 0); + declare_parameter("ptp_utc_tai_offset", -37.0); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { OusterSensor::on_metadata_updated(info); - os_tf_bcast.broadcast_transforms(info); + tf_bcast.broadcast_transforms(info); } virtual void create_publishers() override { @@ -53,15 +54,16 @@ class OusterDriver : public OusterSensor { auto selected_qos = use_system_default_qos ? system_default_qos : sensor_data_qos; - auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || - timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; + auto timestamp_mode = get_parameter("timestamp_mode").as_string(); + auto ptp_utc_tai_offset = + get_parameter("ptp_utc_tai_offset").as_double(); if (check_token(tokens, "IMU")) { imu_pub = create_publisher("imu", selected_qos); imu_packet_handler = ImuPacketHandler::create_handler( - info, os_tf_bcast.imu_frame_id(), use_ros_time); + info, tf_bcast.imu_frame_id(), timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); } int num_returns = get_n_returns(info); @@ -75,8 +77,8 @@ class OusterDriver : public OusterSensor { } processors.push_back(PointCloudProcessor::create( - info, os_tf_bcast.point_cloud_frame_id(), - os_tf_bcast.apply_lidar_to_sensor_transform(), + info, tf_bcast.point_cloud_frame_id(), + tf_bcast.apply_lidar_to_sensor_transform(), [this](PointCloudProcessor::OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) lidar_pubs[i]->publish(*msgs[i]); @@ -106,12 +108,8 @@ class OusterDriver : public OusterSensor { } processors.push_back(LaserScanProcessor::create( - info, - os_tf_bcast - .point_cloud_frame_id(), // TODO: should we have different - // frame for the laser scan than - // point cloud??? - scan_ring, [this](LaserScanProcessor::OutputType msgs) { + info, tf_bcast.lidar_frame_id(), scan_ring, + [this](LaserScanProcessor::OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]); })); @@ -144,7 +142,7 @@ class OusterDriver : public OusterSensor { } processors.push_back(ImageProcessor::create( - info, os_tf_bcast.point_cloud_frame_id(), + info, tf_bcast.point_cloud_frame_id(), [this](ImageProcessor::OutputType msgs) { for (auto it = msgs.begin(); it != msgs.end(); ++it) { image_pubs[it->first]->publish(*it->second); @@ -155,7 +153,8 @@ class OusterDriver : public OusterSensor { if (check_token(tokens, "PCL") || check_token(tokens, "SCAN") || check_token(tokens, "IMG")) lidar_packet_handler = LidarPacketHandler::create_handler( - info, use_ros_time, processors); + info, processors, timestamp_mode, + static_cast(ptp_utc_tai_offset * 1e+9)); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { @@ -178,8 +177,7 @@ class OusterDriver : public OusterSensor { } private: - OusterStaticTransformsBroadcaster - os_tf_bcast; + OusterStaticTransformsBroadcaster tf_bcast; rclcpp::Publisher::SharedPtr imu_pub; std::vector::SharedPtr> diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp index 88d8e14a..4f78397f 100644 --- a/ouster-ros/src/os_replay_node.cpp +++ b/ouster-ros/src/os_replay_node.cpp @@ -96,9 +96,7 @@ class OusterReplay : public OusterSensorNodeBase { } private: - void declare_parameters() { - declare_parameter("metadata"); - } + void declare_parameters() { declare_parameter("metadata"); } std::string parse_parameters() { auto meta_file = get_parameter("metadata").as_string(); @@ -123,9 +121,7 @@ class OusterReplay : public OusterSensorNodeBase { } } - void cleanup() { - get_metadata_srv.reset(); - } + void cleanup() { get_metadata_srv.reset(); } }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index ad320f10..9ea69425 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -112,7 +112,7 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { throw std::runtime_error("Unreachable"); } } -} +} // namespace impl template @@ -192,15 +192,13 @@ void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, template -void copy_scan_to_cloud_destaggered(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, - uint64_t scan_ts, const PointT& points, - const ouster::img_t& range, - const ouster::img_t& reflectivity, - const ouster::img_t& near_ir, - const ouster::img_t& signal, - const std::vector& pixel_shift_by_row) { +void copy_scan_to_cloud_destaggered( + ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, uint64_t scan_ts, + const PointT& points, const ouster::img_t& range, + const ouster::img_t& reflectivity, + const ouster::img_t& near_ir, const ouster::img_t& signal, + const std::vector& pixel_shift_by_row) { auto timestamp = ls.timestamp(); - const auto rg = range.data(); const auto rf = reflectivity.data(); const auto nr = near_ir.data(); @@ -211,9 +209,9 @@ void copy_scan_to_cloud_destaggered(ouster_ros::Cloud& cloud, const ouster::Lida #endif for (auto u = 0; u < ls.h; u++) { for (auto v = 0; v < ls.w; v++) { - const auto col_ts = timestamp[v]; - const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL; - const auto src_idx = u * ls.w + (v + ls.w - pixel_shift_by_row[u]) % ls.w; + const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w; + auto ts = timestamp[v_shift]; ts = ts > scan_ts ? ts - scan_ts : 0UL; + const auto src_idx = u * ls.w + v_shift; const auto tgt_idx = u * ls.w + v; const auto xyz = points.row(src_idx); cloud.points[tgt_idx] = ouster_ros::Point{ @@ -293,14 +291,13 @@ void scan_to_cloud_f(ouster::PointsF& points, signal); } - void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, - ouster::PointsF& points, - const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& ls, - const std::vector& pixel_shift_by_row, - int return_index) { + ouster::PointsF& points, + const ouster::PointsF& lut_direction, + const ouster::PointsF& lut_offset, + uint64_t scan_ts, const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row, + int return_index) { bool second = (return_index == 1); assert(cloud.width == static_cast(ls.w) && @@ -323,11 +320,11 @@ void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, ouster::cartesianT(points, range, lut_direction, lut_offset); - copy_scan_to_cloud_destaggered(cloud, ls, scan_ts, points, range, reflectivity, near_ir, - signal, pixel_shift_by_row); + copy_scan_to_cloud_destaggered(cloud, ls, scan_ts, points, range, + reflectivity, near_ir, signal, + pixel_shift_by_row); } - sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, const rclcpp::Time& timestamp, const std::string& frame) { diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 191a0f6c..c40e1a3a 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -409,7 +409,8 @@ std::shared_ptr OusterSensor::create_sensor_client( } else { // use the full init_client to generate and assign random ports to // sensor - cli = sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, + cli = + sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, sensor::TIME_FROM_UNSPEC, lidar_port, imu_port); } diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index b4a4d6f8..25bdc33d 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -127,7 +127,8 @@ class OusterSensor : public OusterSensorNodeBase { void handle_imu_packet(sensor::client& client, const sensor::packet_format& pf); - void connection_loop(sensor::client& client, const sensor::packet_format& pf); + void connection_loop(sensor::client& client, + const sensor::packet_format& pf); void start_sensor_connection_thread(); diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp index ae32cb91..364de005 100644 --- a/ouster-ros/src/os_sensor_node_base.cpp +++ b/ouster-ros/src/os_sensor_node_base.cpp @@ -6,12 +6,12 @@ * @brief implementation of OusterSensorNodeBase interface */ -#include - #include "ouster_ros/os_sensor_node_base.h" #include +#include + namespace sensor = ouster::sensor; using ouster_msgs::srv::GetMetadata; using sensor::UDPProfileLidar; @@ -66,7 +66,7 @@ std::string OusterSensorNodeBase::read_text_file(const std::string& text_file) { } bool OusterSensorNodeBase::write_text_to_file(const std::string& file_path, - const std::string& text) { + const std::string& text) { std::ofstream ofs(file_path); if (!ofs.is_open()) return false; ofs << text << std::endl; diff --git a/ouster-ros/src/os_static_transforms_broadcaster.h b/ouster-ros/src/os_static_transforms_broadcaster.h index c600c1f6..760b5cda 100644 --- a/ouster-ros/src/os_static_transforms_broadcaster.h +++ b/ouster-ros/src/os_static_transforms_broadcaster.h @@ -12,11 +12,11 @@ namespace ouster_ros { -template +template class OusterStaticTransformsBroadcaster { public: - OusterStaticTransformsBroadcaster(NodeT* parent) : node(parent), tf_bcast(parent){ - } + OusterStaticTransformsBroadcaster(NodeT* parent) + : node(parent), tf_bcast(parent) {} void declare_parameters() { node->declare_parameter("sensor_frame", "os_sensor"); @@ -29,7 +29,8 @@ class OusterStaticTransformsBroadcaster { sensor_frame = node->get_parameter("sensor_frame").as_string(); lidar_frame = node->get_parameter("lidar_frame").as_string(); imu_frame = node->get_parameter("imu_frame").as_string(); - point_cloud_frame = node->get_parameter("point_cloud_frame").as_string(); + point_cloud_frame = + node->get_parameter("point_cloud_frame").as_string(); // validate point_cloud_frame if (point_cloud_frame.empty()) { @@ -55,16 +56,23 @@ class OusterStaticTransformsBroadcaster { } const std::string& imu_frame_id() const { return imu_frame; } - const std::string& point_cloud_frame_id() const { return point_cloud_frame; } - bool apply_lidar_to_sensor_transform() const { return point_cloud_frame == sensor_frame; } + const std::string& lidar_frame_id() const { return lidar_frame; } + const std::string& sensor_frame_id() const { return sensor_frame; } + + const std::string& point_cloud_frame_id() const { + return point_cloud_frame; + } + bool apply_lidar_to_sensor_transform() const { + return point_cloud_frame == sensor_frame; + } private: NodeT* node; tf2_ros::StaticTransformBroadcaster tf_bcast; - std::string sensor_frame; std::string imu_frame; std::string lidar_frame; + std::string sensor_frame; std::string point_cloud_frame; }; -} // namespace ouster_ros +} // namespace ouster_ros diff --git a/ouster-ros/src/point_cloud_processor.h b/ouster-ros/src/point_cloud_processor.h index 7fd0fb6a..6faba7be 100644 --- a/ouster-ros/src/point_cloud_processor.h +++ b/ouster-ros/src/point_cloud_processor.h @@ -63,9 +63,9 @@ class PointCloudProcessor { void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) { for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { - scan_to_cloud_f_destaggered(cloud, - points, lut_direction, lut_offset, - scan_ts, lidar_scan, pixel_shift_by_row, i); + scan_to_cloud_f_destaggered(cloud, points, lut_direction, + lut_offset, scan_ts, lidar_scan, + pixel_shift_by_row, i); pcl_toROSMsg(cloud, *pc_msgs[i]); pc_msgs[i]->header.stamp = msg_ts; @@ -107,4 +107,4 @@ class PointCloudProcessor { PostProcessingFn post_processing_fn; }; -} // namespace ouster_ros \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/thread_safe_ring_buffer.h b/ouster-ros/src/thread_safe_ring_buffer.h index 38bd1fcb..26b5c88d 100644 --- a/ouster-ros/src/thread_safe_ring_buffer.h +++ b/ouster-ros/src/thread_safe_ring_buffer.h @@ -8,20 +8,22 @@ #pragma once -#include -#include #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) { - } + 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. @@ -31,7 +33,7 @@ class ThreadSafeRingBuffer { /** * 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() @@ -44,7 +46,7 @@ class ThreadSafeRingBuffer { /** * 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. @@ -56,7 +58,7 @@ class ThreadSafeRingBuffer { /** * 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. @@ -67,20 +69,20 @@ class ThreadSafeRingBuffer { } /** - * Writes to the buffer safely, the method will keep blocking until the there - * is a space available within the buffer. + * 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(); }); + 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. @@ -117,9 +119,11 @@ class ThreadSafeRingBuffer { * inaccessible the method will timeout and buffer_read gets a nullptr. */ template - void read_timeout(BufferReadFn&& buffer_read, std::chrono::seconds timeout) { + 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; })) { + 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; @@ -129,7 +133,7 @@ class ThreadSafeRingBuffer { } } -private: + private: std::vector buffer; size_t item_size; size_t max_items_count;