From fd76cc9eb0588590094ee012e9312bda276518da Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Tue, 28 Feb 2023 20:07:46 -0800 Subject: [PATCH 01/11] Remove the duplicate sensor_info object --- .github/workflows/docker-image.yml | 2 +- README.md | 2 +- ouster-ros/CMakeLists.txt | 2 +- ouster-ros/launch/sensor.independent.launch.xml | 2 +- ouster-ros/src/os_cloud_node.cpp | 1 - ouster-ros/src/os_image_node.cpp | 2 -- ouster-ros/src/os_replay_node.cpp | 10 +++++----- 7 files changed, 9 insertions(+), 12 deletions(-) diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml index f762d7f0..8122b175 100644 --- a/.github/workflows/docker-image.yml +++ b/.github/workflows/docker-image.yml @@ -1,4 +1,4 @@ -name: Docker Image CI +name: ouster-ros on: push: diff --git a/README.md b/README.md index 6c0f6c9e..581c89bf 100644 --- a/README.md +++ b/README.md @@ -88,7 +88,7 @@ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release Once the build succeeds, you must source the _install_ folder of your ros2 workspace to add launch commands to your environment: ```bash -source ros2_ws/install/setup.bash # replace ros-distro with 'rolling' or 'humble' +source ros2_ws/install/setup.bash ``` ## Usage diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 995b9805..8d64c43f 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -143,7 +143,7 @@ rclcpp_components_register_node(os_cloud_component # ==== os_image_component ==== create_ros2_component(os_image_component - "src/os_processing_node_base;src/os_image_node.cpp" + "src/os_processing_node_base.cpp;src/os_image_node.cpp" "" ) rclcpp_components_register_node(os_image_component diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index c3b15a2a..29ee1c01 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -42,7 +42,7 @@ - + diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 72e0a136..e1c9d509 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -198,7 +198,6 @@ class OusterCloud : public OusterProcessingNodeBase { sensor_msgs::msg::PointCloud2 pc_msg; - sensor::sensor_info info; int n_returns = 0; ouster::PointsF lut_direction; diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index 73af7634..33c11a8a 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -194,8 +194,6 @@ class OusterImage : public OusterProcessingNodeBase { std::vector::SharedPtr> pc_subs; - sensor::sensor_info info; - ouster_ros::Cloud cloud; viz::AutoExposure nearir_ae, signal_ae, reflec_ae; viz::BeamUniformityCorrector nearir_buc; diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp index b3dc07d9..831d8942 100644 --- a/ouster-ros/src/os_replay_node.cpp +++ b/ouster-ros/src/os_replay_node.cpp @@ -47,35 +47,35 @@ class OusterReplay : public OusterSensorNodeBase { LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State& state) { - RCLCPP_INFO(get_logger(), "on_activate() is called."); + RCLCPP_DEBUG(get_logger(), "on_activate() is called."); LifecycleNode::on_activate(state); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } LifecycleNodeInterface::CallbackReturn on_error( const rclcpp_lifecycle::State&) { - RCLCPP_INFO(get_logger(), "on_error() is called."); + RCLCPP_DEBUG(get_logger(), "on_error() is called."); // Always return failure for now return LifecycleNodeInterface::CallbackReturn::FAILURE; } LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State& state) { - RCLCPP_INFO(get_logger(), "on_deactivate() is called."); + RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); LifecycleNode::on_deactivate(state); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State&) { - RCLCPP_INFO(get_logger(), "on_cleanup() is called."); + RCLCPP_DEBUG(get_logger(), "on_cleanup() is called."); cleanup(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } LifecycleNodeInterface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State& state) { - RCLCPP_INFO_STREAM(get_logger(), "on_shutdown() is called."); + RCLCPP_DEBUG(get_logger(), "on_shutdown() is called."); if (state.label() == "unconfigured") { // nothing to do, return success From 7a10d2ce17cc14a7412d421590bdc54fdb0671c6 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Tue, 4 Apr 2023 15:42:49 -0700 Subject: [PATCH 02/11] SW-4747: update the ros 2 driver(s) to the 20230403 sdk release (#94) * Update to the latest ouster sdk * Forward multicast funcitonality + Other improvements and fixes * Add service_msgs dependency to package.xml * Correct sensor_mtp.launch for ros2 launch file format * Move to most recent SDK update * Declare and fill defaults for mtp paramters + fix uninitialzed compute_to_scan * Launch file rename and README corrections --- CHANGELOG.rst | 19 +- Dockerfile | 2 +- README.md | 67 ++++++- ouster-ros/config/parameters.yaml | 2 + ouster-ros/include/ouster_ros/os_ros.h | 37 ++-- ouster-ros/launch/sensor.composite.launch.xml | 2 + .../launch/sensor.independent.launch.xml | 2 + ouster-ros/launch/sensor_mtp.launch.xml | 88 +++++++++ ouster-ros/ouster-sdk | 2 +- ouster-ros/package.xml | 4 +- ouster-ros/src/os_cloud_node.cpp | 169 ++++++++++++++++-- ouster-ros/src/os_image_node.cpp | 16 +- ouster-ros/src/os_ros.cpp | 77 +++++++- ouster-ros/src/os_sensor_node.cpp | 55 +++++- 14 files changed, 483 insertions(+), 59 deletions(-) create mode 100644 ouster-ros/launch/sensor_mtp.launch.xml diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c40849f9..75029997 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -9,7 +9,22 @@ ouster_ros(2) ------------- * MVP ouster-ros targeting ros2 distros (beta release) * introduced a ``reset`` service to the ``os_sensor`` node +* breaking change: updated to ouster sdk release 20230403 +* EOL notice: ouster-ros driver will drop support for ``ROS foxy`` by May 2023. +* bugfix: Address an issue causing the driver to warn about missing non-legacy fields even they exist + in the original metadata file. +* added a new launch file ``sensor_mtp.launch.xml`` for multicast use case (experimental). +* added a technique to estimate the the value of the lidar scan timestamp when it is missing packets + at the beginning +* add frame_id to image topics +* fixed a potential issue of time values within generated point clouds that could result in a value + overflow ouster_client --------------- -* based off 20230114 release +------------- +* added a new method ``mtp_init_client`` to init the client with multicast support (experimental). +* the class ``SensorHttp`` which provides easy access to REST APIs of the sensor has been made public + under the ``ouster::sensor::util`` namespace. +* breaking change: get_metadata defaults to outputting non-legacy metadata +* add debug five_word profile which will be removed later +* breaking change: remove deprecations on LidarScan diff --git a/Dockerfile b/Dockerfile index c8eac12b..1d5e5b38 100644 --- a/Dockerfile +++ b/Dockerfile @@ -37,7 +37,7 @@ RUN set -xe \ && rosdep update --rosdistro=$ROS_DISTRO \ && rosdep install -y --from-paths $OUSTER_ROS_PATH \ # use -r for now to prevent rosdep from complaining about ouster_srvs - --ignore-src -r + -r # Set up build environment COPY --chown=build:build . $OUSTER_ROS_PATH diff --git a/README.md b/README.md index 581c89bf..2143eeb1 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,35 @@ -# Official ROS2 driver for Ouster sensors (BETA) - -[Requirements](#requirements) | [Getting Started](#getting-started) | [Usage](#usage) | [License](#license) +# Official ROS driver for Ouster sensors +[ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) | +[ROS2 (rolling/humble)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) | +[ROS2 (foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)

+| ROS Version | Build Status (Linux) | +|:-----------:|:------:| +| ROS1 (melodic/noetic) | [![melodic/noetic](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=master)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml) +| ROS2 (rolling/humble) | [![rolling/humble](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) + +- [Overview](#overview) +- [Requirements](#requirements) +- [Getting Started](#getting-started) +- [Usage](#usage) + - [Launching Nodes](#launching-nodes) + - [Sensor Mode](#sensor-mode) + - [Recording Mode](#recording-mode) + - [Replay Mode](#replay-mode) + - [Multicast Mode (experimental)](#multicast-mode-experimental) + - [Invoking Services](#invoking-services) + - [GetMetadata](#getmetadata) + - [GetConfig](#getconfig) + - [SetConfig (experimental)](#setconfig-experimental) +- [License](#license) + + +## Overview + This ROS package provide support for all Ouster sensors with FW v2.0 or later targeting ros2 distros. Upon launch the driver will configure and connect to the selected sensor device, once connected the driver will handle incoming IMU and lidar packets, decode lidar frames and publish corresponding ROS @@ -20,7 +45,7 @@ documentation on how to setup ros on your machine before proceeding with the rem > 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 --ignore-src -r + rosdep install -y --from-paths $OUSTER_ROS_PATH -r ``` ### Linux @@ -94,8 +119,8 @@ source ros2_ws/install/setup.bash ## Usage ### Launching Nodes -The package supports three modes of interaction, you can connect to a live sensor, replay a recorded -bag or record a new bag file using the corresponding launch files. The commands are listed below: +The package supports three modes of interaction, you can connect to a _live sensor_, _replay_ a recorded bag or _record_ a new bag file using the corresponding launch files. Recently, we have +added a new mode that supports multicast. The commands are listed below: #### Sensor Mode ```bash @@ -118,6 +143,36 @@ ros2 launch ouster_ros replay.launch.xml \ bag_file:= ``` +#### Multicast Mode (experimental) +The multicast launch mode supports configuring the sensor to broadcast lidar packets from the same +sensor (live) to multiple active clients. You initiate this mode by using `sensor_mtp.launch.xml` +file to start the node. You will need to specify a valid multicast group for the **udp_dest** +argument which the sensor is going to broadcast data to it. You will also need to set **mtp_main** +argument to **true**, this is need to configure the sensor with the specified **udp_dest** and any +other sensor settings. You can control on which ip (IP4 only) you wish to receive the data on this +machine from the multicast group using the **mtp_dest** argument as follows: +```bash +roslaunch ouster_ros sensor_mtp.launch.xml \ + sensor_hostname:= \ + udp_dest:= \ + mtp_main:=true \ + mtp_dest:= # mtp_dest is optional +``` +Using a different machine that belongs to the same netwok subnet, you can start another instance of +the client to start receiving sensor messages through the multicast group as shown below (note that +**mtp_main** is set to **false**): +```bash +roslaunch ouster_ros sensor_mtp.launch.xml \ + sensor_hostname:= \ + udp_dest:= \ + mtp_main:=false \ + mtp_dest:= # mtp_dest is optional +``` + +> **Note:** +> In both cases the **mtp_dest** is optional and if left unset the client will utilize the first +available interface. + ### Invoking Services To execute any of the following service, first you need to open a new terminal and source the ros2 workspace again by running the command diff --git a/ouster-ros/config/parameters.yaml b/ouster-ros/config/parameters.yaml index d01ee8fa..44189e15 100644 --- a/ouster-ros/config/parameters.yaml +++ b/ouster-ros/config/parameters.yaml @@ -2,6 +2,8 @@ ouster/os_sensor: ros__parameters: sensor_hostname: '' udp_dest: '' + mtp_dest: '' + mtp_main: false lidar_mode: '' timestamp_mode: '' udp_profile_lidar: '' diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 5d14a904..48d933cb 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -10,26 +10,23 @@ #pragma once #define PCL_NO_PRECOMPILE +#include +#include +#include +#include +#include #include #include #include #include -#include - #include #include -#include -#include -#include - #include "ouster_msgs/msg/packet_msg.hpp" #include "ouster_ros/os_point.h" -#include - namespace ouster_ros { namespace sensor = ouster::sensor; @@ -72,6 +69,23 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, const std::string& frame, const sensor::packet_format& pf); +/** + * Populate a PCL point cloud from a LidarScan. + * @param[in, out] points The points parameters is used to store the results of + * the cartesian product before it gets packed into the cloud object. + * @param[in] lut_direction the direction of the xyz lut (with single precision) + * @param[in] lut_offset the offset of the xyz lut (with single precision) + * @param[in] scan_ts scan start used to caluclate relative timestamps for + * points. + * @param[in] lidar_scan input lidar data + * @param[out] cloud output pcl pointcloud to populate + * @param[in] return_index index of return desired starting at 0 + */ +[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f( + ouster::PointsF& points, const ouster::PointsF& lut_direction, + const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts, + const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, int return_index); + /** * Populate a PCL point cloud from a LidarScan. * @param[in, out] points The points parameters is used to store the results of @@ -80,15 +94,14 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, * @param[in] lut_offset the offset of the xyz lut (with single precision) * @param[in] scan_ts scan start used to caluclate relative timestamps for * points - * @param[in] ls input lidar data + * @param[in] lidar_scan input lidar data * @param[out] cloud output pcl pointcloud to populate * @param[in] return_index index of return desired starting at 0 */ void scan_to_cloud_f(ouster::PointsF& points, const ouster::PointsF& lut_direction, - const ouster::PointsF& lut_offset, - ouster::LidarScan::ts_t scan_ts, - const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, + const ouster::PointsF& lut_offset, uint64_t scan_ts, + const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, int return_index); /** diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 01366336..cd2965c1 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -47,6 +47,8 @@ + + diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 29ee1c01..a40c09f7 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -46,6 +46,8 @@ + + diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml new file mode 100644 index 00000000..7fe115f7 --- /dev/null +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ouster-ros/ouster-sdk b/ouster-ros/ouster-sdk index 202c536b..d7307986 160000 --- a/ouster-ros/ouster-sdk +++ b/ouster-ros/ouster-sdk @@ -1 +1 @@ -Subproject commit 202c536b358b537b3620964cea3919a24699a7dc +Subproject commit d73079865ebadd961d7af81b235bb34747f7cb6a diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 5efde14e..c0a715b1 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.7.3 + 0.8.0 Ouster ROS2 driver ouster developers BSD @@ -10,7 +10,6 @@ ament_cmake rosidl_default_generators - rclcpp rclcpp_components rclcpp_lifecycle @@ -20,6 +19,7 @@ pcl_ros pcl_conversions std_srvs + service_msgs ouster_msgs ouster_srvs diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index e1c9d509..2d1040e1 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "ouster_msgs/msg/packet_msg.hpp" #include "ouster_srvs/srv/get_metadata.hpp" @@ -30,6 +31,43 @@ namespace sensor = ouster::sensor; using ouster_msgs::msg::PacketMsg; using ouster_srvs::srv::GetMetadata; +namespace { + +template +int find_if_reverse(const Eigen::Array& array, + UnaryPredicate predicate) { + auto p = array.data() + array.size() - 1; + do { + if (predicate(*p)) return p - array.data(); + } while (p-- != array.data()); + return -1; +} + +uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { + uint64_t min_v, max_v; + double sign; + if (y1 > y0) { + min_v = y0; + max_v = y1; + sign = +1; + } else { + min_v = y1; + max_v = y0; + sign = -1; + } + 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 { class OusterCloud : public OusterProcessingNodeBase { @@ -52,6 +90,9 @@ class OusterCloud : public OusterProcessingNodeBase { info = sensor::parse_metadata(metadata); n_returns = get_n_returns(); create_lidarscan_objects(); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; create_publishers(); create_subscriptions(); } @@ -72,6 +113,13 @@ class OusterCloud : public OusterProcessingNodeBase { use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; } + static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { + const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); + const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); + const double one_sec_in_ns = 1e+9; + return one_sec_in_ns / (scan_width * scan_frequency); + } + void create_lidarscan_objects() { // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point @@ -131,7 +179,7 @@ class OusterCloud : public OusterProcessingNodeBase { pcl_conversions::moveFromPCL(pcl_pc2, cloud); } - void convert_scan_to_pointcloud_publish(std::chrono::nanoseconds scan_ts, + void convert_scan_to_pointcloud_publish(uint64_t scan_ts, const rclcpp::Time& msg_ts) { for (int i = 0; i < n_returns; ++i) { scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, @@ -146,27 +194,107 @@ class OusterCloud : public OusterProcessingNodeBase { info.lidar_to_sensor_transform, sensor_frame, lidar_frame, msg_ts)); } - void lidar_handler_sensor_time(const PacketMsg::ConstSharedPtr packet) { - if (!(*scan_batcher)(packet->buf.data(), *lidar_scan)) return; - auto ts_v = lidar_scan->timestamp(); + uint64_t impute_value(int last_scan_last_nonzero_idx, + uint64_t last_scan_last_nonzero_value, + int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value, + int scan_width) { + assert(scan_width + curr_scan_first_nonzero_idx > + last_scan_last_nonzero_idx); + double interpolated_value = linear_interpolate( + 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); + } + + uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value) { + double extrapolated_value = + curr_scan_first_nonzero_value - + scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; + return ulround(extrapolated_value); + } + + // compute_scan_ts_0 for first scan + uint64_t compute_scan_ts_0( + const ouster::LidarScan::Header& ts_v) { auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), [](uint64_t h) { return h != 0; }); - if (idx == ts_v.data() + ts_v.size()) return; - auto scan_ts = std::chrono::nanoseconds{ts_v(idx - ts_v.data())}; - convert_scan_to_pointcloud_publish(scan_ts, to_ros_time(scan_ts)); + 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 + : extrapolate_value(curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value); + + 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 + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_n(ts_v); + }; + return scan_ns; } - void lidar_handler_ros_time(const PacketMsg::ConstSharedPtr packet) { - auto packet_receive_time = get_clock()->now(); - static auto frame_ts = packet_receive_time; // first point cloud time - if (!(*scan_batcher)(packet->buf.data(), *lidar_scan)) return; - auto ts_v = lidar_scan->timestamp(); + // compute_scan_ts_n applied to all subsequent scans except first one + uint64_t compute_scan_ts_n( + const ouster::LidarScan::Header& ts_v) { auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), [](uint64_t h) { return h != 0; }); - if (idx == ts_v.data() + ts_v.size()) return; - auto scan_ts = std::chrono::nanoseconds{ts_v(idx - ts_v.data())}; + 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, + last_scan_last_nonzero_value, + 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 + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + return scan_ns; + } + + void lidar_handler_sensor_time(const PacketMsg::ConstPtr& packet) { + if (!(*scan_batcher)(packet->buf.data(), *lidar_scan)) return; + auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); + convert_scan_to_pointcloud_publish(scan_ts, to_ros_time(scan_ts)); + } + + uint16_t packet_col_index(const uint8_t* packet_buf) { + const auto& pf = sensor::get_format(info); + return pf.col_measurement_id(pf.nth_col(0, packet_buf)); + } + + rclcpp::Time extrapolate_frame_ts(const uint8_t* lidar_buf, + const rclcpp::Time current_time) { + auto curr_scan_first_arrived_idx = packet_col_index(lidar_buf); + auto delta_time = rclcpp::Duration( + 0, + std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); + return current_time - delta_time; + } + + void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) { + auto packet_receive_time = rclcpp::Clock(RCL_ROS_TIME).now(); + const uint8_t* packet_buf = packet->buf.data(); + static auto frame_ts = extrapolate_frame_ts( + packet_buf, packet_receive_time); // first point cloud time + if (!(*scan_batcher)(packet_buf, *lidar_scan)) return; + auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); convert_scan_to_pointcloud_publish(scan_ts, frame_ts); - frame_ts = packet_receive_time; // set time for next point cloud msg + // set time for next point cloud msg + frame_ts = extrapolate_frame_ts(packet_buf, packet_receive_time); } void imu_handler(const PacketMsg::ConstSharedPtr packet) { @@ -185,10 +313,6 @@ class OusterCloud : public OusterProcessingNodeBase { return rclcpp::Time(ts); } - static inline rclcpp::Time to_ros_time(std::chrono::nanoseconds ts) { - return to_ros_time(ts.count()); - } - private: rclcpp::Subscription::SharedPtr lidar_packet_sub; std::vector::SharedPtr> @@ -214,6 +338,13 @@ class OusterCloud : public OusterProcessingNodeBase { tf2_ros::TransformBroadcaster tf_bcast; bool use_ros_time; + + int last_scan_last_nonzero_idx = -1; + uint64_t last_scan_last_nonzero_value = 0; + std::function&)> + compute_scan_ts; + double scan_col_ts_spacing_ns; // interval or spacing between columns of a + // scan }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index 33c11a8a..522d2d77 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -108,10 +108,14 @@ class OusterImage : public OusterProcessingNodeBase { uint32_t H = info.format.pixels_per_column; uint32_t W = info.format.columns_per_frame; - auto range_image = make_image_msg(H, W, m->header.stamp); - auto signal_image = make_image_msg(H, W, m->header.stamp); - auto reflec_image = make_image_msg(H, W, m->header.stamp); - auto nearir_image = make_image_msg(H, W, m->header.stamp); + auto range_image = + make_image_msg(H, W, m->header.stamp, m->header.frame_id); + auto signal_image = + make_image_msg(H, W, m->header.stamp, m->header.frame_id); + auto reflec_image = + make_image_msg(H, W, m->header.stamp, m->header.frame_id); + auto nearir_image = + make_image_msg(H, W, m->header.stamp, m->header.frame_id); ouster::img_t nearir_image_eigen(H, W); ouster::img_t signal_image_eigen(H, W); @@ -172,7 +176,8 @@ class OusterImage : public OusterProcessingNodeBase { } static sensor_msgs::msg::Image::UniquePtr make_image_msg( - size_t H, size_t W, const rclcpp::Time& stamp) { + size_t H, size_t W, const rclcpp::Time& stamp, + const std::string& frame) { auto msg = std::make_unique(); msg->width = W; msg->height = H; @@ -180,6 +185,7 @@ class OusterImage : public OusterProcessingNodeBase { msg->encoding = sensor_msgs::image_encodings::MONO16; msg->data.resize(W * H * sizeof(pixel_type)); msg->header.stamp = stamp; + msg->header.frame_id = frame; return msg; } diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index 29b147e7..f13cf1a3 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -125,19 +125,19 @@ void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, const ouster::img_t& near_ir, const ouster::img_t& signal) { auto timestamp = ls.timestamp(); - const auto rg = range.data(); const auto rf = reflectivity.data(); const auto nr = near_ir.data(); const auto sg = signal.data(); + const auto t_zero = std::chrono::duration::zero(); #ifdef __OUSTER_UTILIZE_OPENMP__ #pragma omp parallel for collapse(2) #endif for (auto u = 0; u < ls.h; u++) { for (auto v = 0; v < ls.w; v++) { - const auto ts = std::min( - std::chrono::nanoseconds(timestamp[v]) - scan_ts, scan_ts); + const auto col_ts = std::chrono::nanoseconds(timestamp[v]); + const auto ts = col_ts > scan_ts ? col_ts - scan_ts : t_zero; const auto idx = u * ls.w + v; const auto xyz = points.row(idx); cloud.points[idx] = ouster_ros::Point{ @@ -154,10 +154,79 @@ void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, } } +template +void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, + uint64_t scan_ts, const PointT& points, + const ouster::img_t& range, + const ouster::img_t& reflectivity, + const ouster::img_t& near_ir, + const ouster::img_t& signal) { + auto timestamp = ls.timestamp(); + + const auto rg = range.data(); + const auto rf = reflectivity.data(); + const auto nr = near_ir.data(); + const auto sg = signal.data(); + +#ifdef __OUSTER_UTILIZE_OPENMP__ +#pragma omp parallel for collapse(2) +#endif + for (auto u = 0; u < ls.h; u++) { + for (auto v = 0; v < ls.w; v++) { + const auto col_ts = timestamp[v]; + const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL; + const auto idx = u * ls.w + v; + const auto xyz = points.row(idx); + cloud.points[idx] = ouster_ros::Point{ + {static_cast(xyz(0)), static_cast(xyz(1)), + static_cast(xyz(2)), 1.0f}, + static_cast(sg[idx]), + static_cast(ts), + static_cast(rf[idx]), + static_cast(u), + static_cast(nr[idx]), + static_cast(rg[idx]), + }; + } + } +} + void scan_to_cloud_f(ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, - ouster::LidarScan::ts_t scan_ts, + std::chrono::nanoseconds scan_ts, + const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, + int return_index) { + bool second = (return_index == 1); + + assert(cloud.width == static_cast(ls.w) && + cloud.height == static_cast(ls.h) && + "point cloud and lidar scan size mismatch"); + + // across supported lidar profiles range is always 32-bit + auto range_channel_field = + second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE; + ouster::img_t range = ls.field(range_channel_field); + + ouster::img_t reflectivity = get_or_fill_zero( + suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); + + ouster::img_t signal = get_or_fill_zero( + suitable_return(sensor::ChanField::SIGNAL, second), ls); + + ouster::img_t near_ir = get_or_fill_zero( + suitable_return(sensor::ChanField::NEAR_IR, second), ls); + + ouster::cartesianT(points, range, lut_direction, lut_offset); + + copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir, + signal); +} + +void scan_to_cloud_f(ouster::PointsF& points, + const ouster::PointsF& lut_direction, + const ouster::PointsF& lut_offset, uint64_t scan_ts, const ouster::LidarScan& ls, ouster_ros::Cloud& cloud, int return_index) { bool second = (return_index == 1); diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index ef292651..08de1be1 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -169,6 +169,8 @@ class OusterSensor : public OusterSensorNodeBase { declare_parameter("sensor_hostname"); declare_parameter("metadata"); declare_parameter("udp_dest"); + declare_parameter("mtp_dest"); + declare_parameter("mtp_main"); declare_parameter("lidar_port", 0); declare_parameter("imu_port", 0); declare_parameter("lidar_mode"); @@ -415,9 +417,14 @@ class OusterSensor : public OusterSensorNodeBase { int lidar_port = config.udp_port_lidar ? config.udp_port_lidar.value() : 0; int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; + auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; std::shared_ptr cli; - if (lidar_port != 0 && imu_port != 0) { + if (sensor::in_multicast(udp_dest)) { + // use the mtp_init_client to recieve data via multicast + // if mtp_main is true when sensor will be configured + cli = sensor::mtp_init_client(hostname, config, mtp_dest, mtp_main); + } else if (lidar_port != 0 && imu_port != 0) { // use no-config version of init_client to bind to pre-configured // ports cli = sensor::init_client(hostname, lidar_port, imu_port); @@ -441,6 +448,8 @@ class OusterSensor : public OusterSensorNodeBase { sensor::sensor_config parse_config_from_ros_parameters() { auto udp_dest = get_parameter("udp_dest").as_string(); + auto mtp_dest_arg = get_parameter("mtp_dest").as_string(); + auto mtp_main_arg = get_parameter("mtp_main").as_bool(); auto lidar_port = get_parameter("lidar_port").as_int(); auto imu_port = get_parameter("imu_port").as_int(); auto lidar_mode_arg = get_parameter("lidar_mode").as_string(); @@ -511,8 +520,8 @@ class OusterSensor : public OusterSensorNodeBase { sensor::sensor_config config; if (lidar_port == 0) { - RCLCPP_WARN( - get_logger(), + RCLCPP_WARN_EXPRESSION( + get_logger(), !is_arg_set(mtp_dest_arg), "lidar port set to zero, the client will assign a random port " "number!"); } else { @@ -520,8 +529,8 @@ class OusterSensor : public OusterSensorNodeBase { } if (imu_port == 0) { - RCLCPP_WARN( - get_logger(), + RCLCPP_WARN_EXPRESSION( + get_logger(), !is_arg_set(mtp_dest_arg), "imu port set to zero, the client will assign a random port " "number!"); } else { @@ -532,7 +541,14 @@ class OusterSensor : public OusterSensorNodeBase { config.operating_mode = sensor::OPERATING_NORMAL; if (lidar_mode) config.ld_mode = lidar_mode; if (timestamp_mode) config.ts_mode = timestamp_mode; - if (is_arg_set(udp_dest)) config.udp_dest = udp_dest; + if (is_arg_set(udp_dest)) { + config.udp_dest = udp_dest; + if (sensor::in_multicast(udp_dest)) { + mtp_dest = + is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{}; + mtp_main = mtp_main_arg; + } + } return config; } @@ -548,6 +564,19 @@ class OusterSensor : public OusterSensorNodeBase { if (config.udp_dest) { RCLCPP_INFO_STREAM(get_logger(), "Will send UDP data to " << config.udp_dest.value()); + // TODO: revise multicast setup inference + if (sensor::in_multicast(*config.udp_dest)) { + if (is_arg_set(mtp_dest)) { + RCLCPP_INFO_STREAM( + get_logger(), + "Will recieve data via multicast on " << mtp_dest); + } else { + RCLCPP_INFO( + get_logger(), + "mtp_dest was not set, will recieve data via multicast " + "on first available interface"); + } + } } else { RCLCPP_INFO(get_logger(), "Will use automatic UDP destination"); config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; @@ -563,7 +592,17 @@ class OusterSensor : public OusterSensorNodeBase { } void configure_sensor(const std::string& hostname, - const sensor::sensor_config& config) { + sensor::sensor_config& config) { + if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && + !mtp_main) { + if (!get_config(hostname, config, true)) { + RCLCPP_ERROR(get_logger(), "Error getting active config"); + } else { + RCLCPP_INFO(get_logger(), "Retrived active config of sensor"); + } + return; + } + uint8_t config_flags = compose_config_flags(config); if (!set_config(hostname, config, config_flags)) { throw std::runtime_error("Error connecting to sensor " + hostname); @@ -760,6 +799,8 @@ class OusterSensor : public OusterSensorNodeBase { std::string sensor_hostname; std::string staged_config; std::string active_config; + std::string mtp_dest; + bool mtp_main; std::shared_ptr sensor_client; PacketMsg lidar_packet; PacketMsg imu_packet; From 592e316c6e46ad7e1a353eb93ddf135735a119bf Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Tue, 11 Apr 2023 18:18:24 -0700 Subject: [PATCH 03/11] Explicity set cxx compile standard if the env isn't (#99) --- ouster-ros/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 8d64c43f..e24829ca 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -24,9 +24,12 @@ find_package(tf2_eigen REQUIRED) # ==== Options ==== add_compile_options(-Wall -Wextra) +if(NOT DEFINED CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) - set(_ouster_ros_INCLUDE_DIRS include ouster-sdk/ouster_client/include From 175a197182fc0912a4eef48932a4fa7244f3b137 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Wed, 19 Apr 2023 12:28:13 -0700 Subject: [PATCH 04/11] SW-4837: replace the use of ros service to retrieve sensor metadata with latched topics (#102) * Working port of latched metadata topic on ros2 * Update replay and record launch files to providing metadata file an optional parameter * Remove extra white space in replay record command * Undo changes to the metadata-qos-override * minor code syntax improvements * Add missing metadata topic when bag file isn't specified * Use concise syntax and formatting * Reverse logic for easier read * Apply node transition if it exists --- CHANGELOG.rst | 3 + README.md | 15 +++-- ouster-ros/CMakeLists.txt | 2 + ouster-ros/config/metadata-qos-override.yaml | 5 ++ .../ouster_ros/os_processing_node_base.h | 20 +++---- ouster-ros/include/ouster_ros/os_ros.h | 7 ++- .../include/ouster_ros/os_sensor_node_base.h | 8 ++- ouster-ros/launch/record.composite.launch.xml | 11 +++- ouster-ros/launch/replay.composite.launch.xml | 19 ++++--- ouster-ros/package.xml | 3 +- ouster-ros/src/os_cloud_node.cpp | 27 +++++---- ouster-ros/src/os_image_node.cpp | 12 +++- ouster-ros/src/os_processing_node_base.cpp | 57 +++---------------- ouster-ros/src/os_replay_node.cpp | 7 ++- ouster-ros/src/os_sensor_node.cpp | 3 + ouster-ros/src/os_sensor_node_base.cpp | 14 +++++ 16 files changed, 118 insertions(+), 95 deletions(-) create mode 100644 ouster-ros/config/metadata-qos-override.yaml diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 75029997..b2e2fafb 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -19,6 +19,9 @@ ouster_ros(2) * add frame_id to image topics * fixed a potential issue of time values within generated point clouds that could result in a value overflow +* added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it + to the bag file on record +* make specifying metadata file optional during record and replay modes as of package version 8.1 ouster_client ------------- diff --git a/README.md b/README.md index 2143eeb1..4ac7ba1e 100644 --- a/README.md +++ b/README.md @@ -129,18 +129,25 @@ ros2 launch ouster_ros sensor.launch.xml \ ``` #### Recording Mode +> Note +> As of package version 8.1, specifiying metadata file is optional since the introduction of the +> metadata topic ```bash ros2 launch ouster_ros record.launch.xml \ sensor_hostname:= \ - metadata:= \ - bag_file:= + bag_file:= \ + metadata:= # optional ``` #### Replay Mode +> Note +> As of package version 8.1, specifiying metadata file is optional if the bag file being replayed +> already contains the metadata topic + ```bash ros2 launch ouster_ros replay.launch.xml \ - metadata:= \ - bag_file:= + bag_file:= \ + metadata:= # optional if bag file has /metadata topic ``` #### Multicast Mode (experimental) diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index e24829ca..d7f70c4a 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(ouster_msgs REQUIRED) @@ -101,6 +102,7 @@ function(create_ros2_component class_loader rclcpp_components rclcpp_lifecycle + std_msgs sensor_msgs geometry_msgs ${additonal_dependencies} diff --git a/ouster-ros/config/metadata-qos-override.yaml b/ouster-ros/config/metadata-qos-override.yaml new file mode 100644 index 00000000..c11e521f --- /dev/null +++ b/ouster-ros/config/metadata-qos-override.yaml @@ -0,0 +1,5 @@ +/ouster/metadata: + history: keep_last + depth: 1 + reliability: reliable + durability: transient_local diff --git a/ouster-ros/include/ouster_ros/os_processing_node_base.h b/ouster-ros/include/ouster_ros/os_processing_node_base.h index 1a0cbeef..66237edd 100644 --- a/ouster-ros/include/ouster_ros/os_processing_node_base.h +++ b/ouster-ros/include/ouster_ros/os_processing_node_base.h @@ -11,8 +11,7 @@ #include #include - -#include "ouster_srvs/srv/get_metadata.hpp" +#include namespace ouster_ros { @@ -22,24 +21,19 @@ class OusterProcessingNodeBase : public rclcpp::Node { const rclcpp::NodeOptions& options) : rclcpp::Node(name, options) {} - protected: - bool spin_till_attempts_exahused(const std::string& log_msg, - std::function lambda); - - std::string get_metadata(); + void create_metadata_subscriber( + std::function + on_sensor_metadata); - int get_n_returns(); + int get_n_returns() const; - std::string topic_for_return(std::string base, int idx) { + static std::string topic_for_return(std::string base, int idx) { if (idx == 0) return base; return base + std::to_string(idx + 1); } protected: - // TODO: Add as node parameters? - static constexpr auto wait_time_per_attempt = std::chrono::seconds(10); - static constexpr auto total_attempts = 10; - + rclcpp::Subscription::SharedPtr metadata_sub; ouster::sensor::sensor_info info; }; diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 48d933cb..7f17cd0d 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -84,7 +84,8 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, [[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f( ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, int return_index); + const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, + int return_index); /** * Populate a PCL point cloud from a LidarScan. @@ -101,8 +102,8 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, void scan_to_cloud_f(ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, - int return_index); + const ouster::LidarScan& lidar_scan, + ouster_ros::Cloud& cloud, int return_index); /** * Serialize a PCL point cloud to a ROS message diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h index fd175adb..8ec78e07 100644 --- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h @@ -11,6 +11,7 @@ #include #include +#include #include "ouster_srvs/srv/get_metadata.hpp" @@ -23,18 +24,23 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { : rclcpp_lifecycle::LifecycleNode(name, options) {} protected: - bool is_arg_set(const std::string& arg) { + bool is_arg_set(const std::string& arg) const { return arg.find_first_not_of(' ') != std::string::npos; } void create_get_metadata_service(); + void create_metadata_publisher(); + + void publish_metadata(); + void display_lidar_info(const ouster::sensor::sensor_info& info); protected: ouster::sensor::sensor_info info; rclcpp::Service::SharedPtr get_metadata_srv; std::string cached_metadata; + rclcpp::Publisher::SharedPtr metadata_pub; }; } // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index 9d6fe01d..d219d093 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -49,6 +49,8 @@ + + @@ -83,9 +85,14 @@ + /$(var ouster_ns)/imu_packets + /$(var ouster_ns)/lidar_packets + /$(var ouster_ns)/metadata"/> + cmd="ros2 bag record + /$(var ouster_ns)/imu_packets + /$(var ouster_ns)/lidar_packets + /$(var ouster_ns)/metadata"/> diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 84a60a20..2941ba30 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -10,7 +10,7 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> - @@ -20,12 +20,14 @@ description="optional rviz config file"/> + + + + + - - - @@ -36,9 +38,11 @@ - - @@ -51,6 +55,7 @@ + cmd="ros2 bag play $(var bag_file) --clock + --qos-profile-overrides-path $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/> diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index c0a715b1..e6855890 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.0 + 0.8.1 Ouster ROS2 driver ouster developers BSD @@ -13,6 +13,7 @@ rclcpp rclcpp_components rclcpp_lifecycle + std_msgs sensor_msgs geometry_msgs tf2_ros diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 2d1040e1..d4a451c4 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -29,7 +29,6 @@ namespace sensor = ouster::sensor; using ouster_msgs::msg::PacketMsg; -using ouster_srvs::srv::GetMetadata; namespace { @@ -86,15 +85,9 @@ class OusterCloud : public OusterProcessingNodeBase { void on_init() { declare_parameters(); parse_parameters(); - auto metadata = get_metadata(); - info = sensor::parse_metadata(metadata); - n_returns = get_n_returns(); - create_lidarscan_objects(); - compute_scan_ts = [this](const auto& ts_v) { - return compute_scan_ts_0(ts_v); - }; - create_publishers(); - create_subscriptions(); + create_metadata_subscriber( + [this](const auto& msg) { metadata_handler(msg); }); + RCLCPP_INFO(get_logger(), "OusterCloud: node initialized!"); } void declare_parameters() { @@ -120,6 +113,20 @@ class OusterCloud : public OusterProcessingNodeBase { return one_sec_in_ns / (scan_width * scan_frequency); } + 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); + n_returns = get_n_returns(); + create_lidarscan_objects(); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; + create_publishers(); + create_subscriptions(); + } + void create_lidarscan_objects() { // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index 522d2d77..4289adf8 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -36,7 +36,6 @@ namespace sensor = ouster::sensor; namespace viz = ouster::viz; -using ouster_srvs::srv::GetMetadata; using pixel_type = uint16_t; const size_t pixel_value_max = std::numeric_limits::max(); @@ -53,8 +52,15 @@ class OusterImage : public OusterProcessingNodeBase { private: void on_init() { - auto metadata = get_metadata(); - info = sensor::parse_metadata(metadata); + create_metadata_subscriber( + [this](const auto& msg) { metadata_handler(msg); }); + RCLCPP_INFO(get_logger(), "OusterImage: node initialized!"); + } + + void metadata_handler(const std_msgs::msg::String::ConstPtr& metadata_msg) { + RCLCPP_INFO(get_logger(), + "OusterImage: retrieved new sensor metadata!"); + info = sensor::parse_metadata(metadata_msg->data); create_cloud_object(); const int n_returns = get_n_returns(); create_publishers(n_returns); diff --git a/ouster-ros/src/os_processing_node_base.cpp b/ouster-ros/src/os_processing_node_base.cpp index 4024620b..a066980d 100644 --- a/ouster-ros/src/os_processing_node_base.cpp +++ b/ouster-ros/src/os_processing_node_base.cpp @@ -8,60 +8,21 @@ #include "ouster_ros/os_processing_node_base.h" -using namespace std::chrono_literals; using ouster::sensor::UDPProfileLidar; -using ouster_srvs::srv::GetMetadata; -using rclcpp::FutureReturnCode; namespace ouster_ros { -bool OusterProcessingNodeBase::spin_till_attempts_exahused( - const std::string& log_msg, std::function lambda) { - int remaining_attempts = total_attempts; - bool done; - do { - RCLCPP_INFO_STREAM(get_logger(), - log_msg << "; attempt no: " - << total_attempts - remaining_attempts + 1 - << "/" << total_attempts); - done = lambda(); - } while (rclcpp::ok() && !done && --remaining_attempts > 0); - return done; +void OusterProcessingNodeBase::create_metadata_subscriber( + std::function + on_sensor_metadata) { + auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1)); + latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + metadata_sub = create_subscription( + "metadata", latching_qos, on_sensor_metadata); } -std::string OusterProcessingNodeBase::get_metadata() { - auto client = create_client("get_metadata"); - if (!spin_till_attempts_exahused( - "contacting get_metadata service", [this, &client]() -> bool { - return client->wait_for_service(wait_time_per_attempt); - })) { - auto error_msg = "get_metadata service is unavailable"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - auto request = std::make_shared(); - auto result = client->async_send_request(request); - - rclcpp::FutureReturnCode return_code; - spin_till_attempts_exahused( - "waiting for get_metadata service to respond", - [this, &return_code, &result]() -> bool { - return_code = rclcpp::spin_until_future_complete( - get_node_base_interface(), result, wait_time_per_attempt); - return return_code != FutureReturnCode::TIMEOUT; - }); - - if (return_code != FutureReturnCode::SUCCESS) { - auto error_msg = "get_metadata service timed out or interrupted"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - - RCLCPP_INFO(get_logger(), "retrieved sensor metadata!"); - return result.get()->metadata; -} - -int OusterProcessingNodeBase::get_n_returns() { +int OusterProcessingNodeBase::get_n_returns() const { return info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL ? 2 diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp index 831d8942..c13b1b24 100644 --- a/ouster-ros/src/os_replay_node.cpp +++ b/ouster-ros/src/os_replay_node.cpp @@ -30,7 +30,9 @@ class OusterReplay : public OusterSensorNodeBase { try { auto meta_file = parse_parameters(); - populate_metadata_from_json(meta_file); + create_metadata_publisher(); + load_metadata_from_file(meta_file); + publish_metadata(); create_get_metadata_service(); RCLCPP_INFO(get_logger(), "Running in replay mode"); } catch (const std::exception& ex) { @@ -110,7 +112,7 @@ class OusterReplay : public OusterSensorNodeBase { return meta_file; } - void populate_metadata_from_json(const std::string& meta_file) { + void load_metadata_from_file(const std::string& meta_file) { try { std::ifstream in_file(meta_file); std::stringstream buffer; @@ -129,7 +131,6 @@ class OusterReplay : public OusterSensorNodeBase { void cleanup() { get_metadata_srv.reset(); } - }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 08de1be1..ffe3c716 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -63,7 +63,9 @@ class OusterSensor : public OusterSensorNodeBase { sensor_client = create_sensor_client(sensor_hostname, config); if (!sensor_client) return LifecycleNodeInterface::CallbackReturn::FAILURE; + create_metadata_publisher(); update_config_and_metadata(*sensor_client); + publish_metadata(); save_metadata(); create_reset_service(); create_get_metadata_service(); @@ -340,6 +342,7 @@ class OusterSensor : public OusterSensorNodeBase { connection_loop_timer->cancel(); reset_last_init_id = init_id_reset; update_config_and_metadata(*sensor_client); + publish_metadata(); save_metadata(); auto request_transitions = std::vector{ lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp index faee0154..729a28f4 100644 --- a/ouster-ros/src/os_sensor_node_base.cpp +++ b/ouster-ros/src/os_sensor_node_base.cpp @@ -28,6 +28,20 @@ void OusterSensorNodeBase::create_get_metadata_service() { RCLCPP_INFO(get_logger(), "get_metadata service created"); } +void OusterSensorNodeBase::create_metadata_publisher() { + auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1)); + latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + metadata_pub = + create_publisher("metadata", latching_qos); +} + +void OusterSensorNodeBase::publish_metadata() { + std_msgs::msg::String metadata_msg; + metadata_msg.data = cached_metadata; + metadata_pub->publish(metadata_msg); +} + void OusterSensorNodeBase::display_lidar_info(const sensor::sensor_info& info) { auto lidar_profile = info.format.udp_profile_lidar; RCLCPP_INFO_STREAM( From 8a9edfbb29f5a25c5efaa59ecbcb8e6de9dfbe05 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Fri, 28 Apr 2023 15:34:27 -0700 Subject: [PATCH 05/11] SW-4924: Replace tf_prefix by sensor_frame lidar_frame and imu_frame parameters (#115) * deprecate tf_prefix from os_cloud (#96) Co-authored-by: Guillaume Doisy * Squashed commit of the following: commit 6280bfa1178bdee4fe695cb4752efd5ff15279db Author: Ussama Naal Date: Fri Apr 28 07:54:34 2023 -0700 Merge branch 'deprecate_tf_prefix' commit 35f2fd2ba50eaf3e4b65909269eb9609bff7a010 Author: Guillaume Doisy Date: Mon Apr 3 18:12:44 2023 +0100 deprecate tf_prefix from os_cloud * Update ChangeLog and package version * Propagate the parameters to launch files * Add a TODO note --------- Co-authored-by: Guillaume Doisy --- CHANGELOG.rst | 2 ++ ouster-ros/config/parameters.yaml | 4 +++- ouster-ros/launch/record.composite.launch.xml | 12 ++++++++++-- ouster-ros/launch/replay.composite.launch.xml | 12 ++++++++++-- ouster-ros/launch/rviz.launch.py | 2 ++ ouster-ros/launch/rviz.launch.xml | 8 ++++++-- ouster-ros/launch/sensor.composite.launch.xml | 12 ++++++++++-- ouster-ros/launch/sensor.independent.launch.xml | 12 ++++++++++-- ouster-ros/launch/sensor_mtp.launch.xml | 8 +++++++- ouster-ros/package.xml | 2 +- ouster-ros/src/os_cloud_node.cpp | 14 +++++++------- 11 files changed, 68 insertions(+), 20 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b2e2fafb..f6964420 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -22,6 +22,8 @@ ouster_ros(2) * added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it to the bag file on record * make specifying metadata file optional during record and replay modes as of package version 8.1 +* replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame`` + launch parameters. ouster_client ------------- diff --git a/ouster-ros/config/parameters.yaml b/ouster-ros/config/parameters.yaml index 44189e15..7789016d 100644 --- a/ouster-ros/config/parameters.yaml +++ b/ouster-ros/config/parameters.yaml @@ -12,5 +12,7 @@ ouster/os_sensor: imu_port: 0 ouster/os_cloud: ros__parameters: - tf_prefix: '' + sensor_frame: 'os_sensor' + lidar_frame: 'os_lidar' + imu_frame: 'os_imu' timestamp_mode: '' diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index d219d093..a27d47f1 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -41,7 +41,10 @@ description="whether to run a rviz"/> - + + + + @@ -59,7 +62,9 @@ - + + + @@ -77,6 +82,9 @@ + + + diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 2941ba30..5789b620 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -18,7 +18,10 @@ description="whether to run a rviz"/> - + + + + @@ -29,7 +32,9 @@ - + + + @@ -49,6 +54,9 @@ + + + diff --git a/ouster-ros/launch/rviz.launch.py b/ouster-ros/launch/rviz.launch.py index 0642ea40..0cb08eec 100644 --- a/ouster-ros/launch/rviz.launch.py +++ b/ouster-ros/launch/rviz.launch.py @@ -31,6 +31,8 @@ def generate_launch_description(): # NOTE: the two static tf publishers are rather a workaround to let rviz2 # get going and not complain while waiting for the actual sensor frames # to be published that is when running rviz2 using a parent launch file + # TODO: need to be able to propagate the modified frame names from the + # parameters file to RVIZ launch py. sensor_imu_tf = Node( package="tf2_ros", executable="static_transform_publisher", diff --git a/ouster-ros/launch/rviz.launch.xml b/ouster-ros/launch/rviz.launch.xml index 0b80ada1..b8ce09ce 100644 --- a/ouster-ros/launch/rviz.launch.xml +++ b/ouster-ros/launch/rviz.launch.xml @@ -7,6 +7,10 @@ + + + + + args="--frame-id $(var sensor_frame) --child-frame-id $(var imu_frame)"/> + args="--frame-id $(var sensor_frame) --child-frame-id $(var lidar_frame)"/> \ No newline at end of file diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index cd2965c1..208f7082 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -39,7 +39,10 @@ description="whether to run a rviz"/> - + + + + @@ -57,7 +60,9 @@ - + + + @@ -75,6 +80,9 @@ + + + diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index a40c09f7..62a1f110 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -39,7 +39,10 @@ description="whether to run a rviz"/> - + + + + @@ -56,7 +59,9 @@ - + + + @@ -73,6 +78,9 @@ + + + diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index 7fe115f7..5d7885c4 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -47,7 +47,10 @@ description="whether to run a rviz"/> - + + + + @@ -83,6 +86,9 @@ + + + diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index e6855890..61f31a72 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.1 + 0.8.2 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index d4a451c4..3af44b97 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -91,17 +91,17 @@ class OusterCloud : public OusterProcessingNodeBase { } void declare_parameters() { - declare_parameter("tf_prefix"); + declare_parameter("sensor_frame"); + declare_parameter("lidar_frame"); + declare_parameter("imu_frame"); declare_parameter("timestamp_mode"); } void parse_parameters() { - auto tf_prefix = get_parameter("tf_prefix").as_string(); - if (is_arg_set(tf_prefix) && tf_prefix.back() != '/') - tf_prefix.append("/"); - sensor_frame = tf_prefix + "os_sensor"; - imu_frame = tf_prefix + "os_imu"; - lidar_frame = tf_prefix + "os_lidar"; + sensor_frame = get_parameter("sensor_frame").as_string(); + lidar_frame = get_parameter("lidar_frame").as_string(); + imu_frame = get_parameter("imu_frame").as_string(); + auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; } From 513c826bd59f2679575320abe1191ee208be8cd8 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Sun, 30 Apr 2023 11:07:30 -0700 Subject: [PATCH 06/11] Drop service_msgs dep (#117) --- ouster-ros/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 61f31a72..1ffd719f 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -20,7 +20,6 @@ pcl_ros pcl_conversions std_srvs - service_msgs ouster_msgs ouster_srvs From 0363daff80bbb96431dc1ced2bb5976e98be8e6a Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Tue, 2 May 2023 12:23:05 -0700 Subject: [PATCH 07/11] SW-4859: enable having multiple components of same-type under same process (#108) * Remove the use of static vars within components * Resolve conflicts and update changelog and version * Fix a typo 'instance' --- CHANGELOG.rst | 2 ++ ouster-ros/package.xml | 2 +- ouster-ros/src/os_cloud_node.cpp | 25 ++++++++++++++++++------- ouster-ros/src/os_sensor_node.cpp | 8 +++++++- 4 files changed, 28 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f6964420..7b6fdbdf 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -24,6 +24,8 @@ ouster_ros(2) * make specifying metadata file optional during record and replay modes as of package version 8.1 * replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame`` launch parameters. +* bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components + in the same process. ouster_client ------------- diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 1ffd719f..e79b8980 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.2 + 0.8.3 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 3af44b97..f4267857 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -181,9 +181,8 @@ class OusterCloud : public OusterProcessingNodeBase { void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, sensor_msgs::msg::PointCloud2& cloud) { // TODO: remove the staging step in the future - static pcl::PCLPointCloud2 pcl_pc2; - pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2); - pcl_conversions::moveFromPCL(pcl_pc2, cloud); + pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); + pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud); } void convert_scan_to_pointcloud_publish(uint64_t scan_ts, @@ -295,13 +294,18 @@ class OusterCloud : public OusterProcessingNodeBase { void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) { auto packet_receive_time = rclcpp::Clock(RCL_ROS_TIME).now(); const uint8_t* packet_buf = packet->buf.data(); - static auto frame_ts = extrapolate_frame_ts( - packet_buf, packet_receive_time); // first point cloud time + if (!lidar_handler_ros_time_frame_ts_initialized) { + lidar_handler_ros_time_frame_ts = extrapolate_frame_ts( + packet_buf, packet_receive_time); // first point cloud time + lidar_handler_ros_time_frame_ts_initialized = true; + } if (!(*scan_batcher)(packet_buf, *lidar_scan)) return; auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); - convert_scan_to_pointcloud_publish(scan_ts, frame_ts); + convert_scan_to_pointcloud_publish(scan_ts, + lidar_handler_ros_time_frame_ts); // set time for next point cloud msg - frame_ts = extrapolate_frame_ts(packet_buf, packet_receive_time); + lidar_handler_ros_time_frame_ts = + extrapolate_frame_ts(packet_buf, packet_receive_time); } void imu_handler(const PacketMsg::ConstSharedPtr packet) { @@ -352,6 +356,13 @@ class OusterCloud : public OusterProcessingNodeBase { compute_scan_ts; double scan_col_ts_spacing_ns; // interval or spacing between columns of a // scan + + pcl::PCLPointCloud2 + staging_pcl_pc2; // a buffer used for staging during the conversion + // from a PCL point cloud to a ros point cloud message + + bool lidar_handler_ros_time_frame_ts_initialized = false; + rclcpp::Time lidar_handler_ros_time_frame_ts; }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index ffe3c716..0d4aa8e7 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -697,7 +697,10 @@ class OusterSensor : public OusterSensorNodeBase { bool init_id_changed(const sensor::packet_format& pf, const uint8_t* lidar_buf) { uint32_t current_init_id = pf.init_id(lidar_buf); - static uint32_t last_init_id = current_init_id + 1; + if (!last_init_id_initialized) { + last_init_id = current_init_id + 1; + last_init_id_initialized = true; + } if (reset_last_init_id && last_init_id != current_init_id) { last_init_id = current_init_id; reset_last_init_id = false; @@ -819,6 +822,9 @@ class OusterSensor : public OusterSensorNodeBase { bool reset_last_init_id = true; std::atomic reset_in_progress = {false}; + bool last_init_id_initialized = false; + uint32_t last_init_id; + // TODO: add as a ros parameter const int max_poll_client_error_count = 10; int poll_client_error_count = 0; From a8b643cd5a53b78ad247669e5767b06c61802bbd Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 5 May 2023 11:04:01 -0600 Subject: [PATCH 08/11] use static tf broadcaster for ros2 (#112) * use separate params for tf frames * send static transforms once * Revert "use separate params for tf frames" This reverts commit 4361969cbd03e7667531d11fd2a508aafd53117d. --- ouster-ros/src/os_cloud_node.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index f4267857..ad3f71b0 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -118,6 +118,7 @@ class OusterCloud : public OusterProcessingNodeBase { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); + send_static_transforms(); n_returns = get_n_returns(); create_lidarscan_objects(); compute_scan_ts = [this](const auto& ts_v) { @@ -127,6 +128,13 @@ class OusterCloud : public OusterProcessingNodeBase { create_subscriptions(); } + void send_static_transforms() { + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now())); + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame, now())); + } + void create_lidarscan_objects() { // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point @@ -195,9 +203,6 @@ class OusterCloud : public OusterProcessingNodeBase { pc_msg.header.frame_id = sensor_frame; lidar_pubs[i]->publish(pc_msg); } - - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.lidar_to_sensor_transform, sensor_frame, lidar_frame, msg_ts)); } uint64_t impute_value(int last_scan_last_nonzero_idx, @@ -316,8 +321,6 @@ class OusterCloud : public OusterProcessingNodeBase { auto imu_msg = ouster_ros::packet_to_imu_msg(*packet, msg_ts, imu_frame, pf); imu_pub->publish(imu_msg); - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.imu_to_sensor_transform, sensor_frame, imu_frame, msg_ts)); }; static inline rclcpp::Time to_ros_time(uint64_t ts) { @@ -346,7 +349,7 @@ class OusterCloud : public OusterProcessingNodeBase { std::string imu_frame; std::string lidar_frame; - tf2_ros::TransformBroadcaster tf_bcast; + tf2_ros::StaticTransformBroadcaster tf_bcast; bool use_ros_time; From bef8dbc9722cb552962d952b544d2443aa51e0a3 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 5 May 2023 10:08:43 -0700 Subject: [PATCH 09/11] Disable static transform publishers and update changelog and package version --- CHANGELOG.rst | 1 + ouster-ros/launch/replay.composite.launch.xml | 2 +- ouster-ros/launch/sensor.composite.launch.py | 2 +- ouster-ros/launch/sensor.composite.launch.xml | 2 +- ouster-ros/launch/sensor.independent.launch.py | 2 +- ouster-ros/package.xml | 2 +- 6 files changed, 6 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 7b6fdbdf..72735d2b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -26,6 +26,7 @@ ouster_ros(2) launch parameters. * bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components in the same process. +* switch to using static transform publisher for the ros2 driver. ouster_client ------------- diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 5789b620..9b0bd198 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -53,7 +53,7 @@ - + diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py index a6e00e7f..c54441c0 100644 --- a/ouster-ros/launch/sensor.composite.launch.py +++ b/ouster-ros/launch/sensor.composite.launch.py @@ -78,7 +78,7 @@ def generate_launch_description(): rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), condition=IfCondition(rviz_enable), - launch_arguments={'_enable_static_tf_publishers': 'true'}.items() + launch_arguments={'_enable_static_tf_publishers': 'false'}.items() ) # HACK: to configure and activate the the sensor since state transition diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 208f7082..2935c6db 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -79,7 +79,7 @@ - + diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py index 632a619a..f7d239fe 100644 --- a/ouster-ros/launch/sensor.independent.launch.py +++ b/ouster-ros/launch/sensor.independent.launch.py @@ -106,7 +106,7 @@ def generate_launch_description(): rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), condition=IfCondition(rviz_enable), - launch_arguments={'_enable_static_tf_publishers': 'true'}.items() + launch_arguments={'_enable_static_tf_publishers': 'false'}.items() ) return launch.LaunchDescription([ diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index e79b8980..dd589cf4 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.3 + 0.8.4 Ouster ROS2 driver ouster developers BSD From aa095d2f09bd9e8d2a8609368a51d1df3f6c0bdc Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 5 May 2023 12:34:21 -0700 Subject: [PATCH 10/11] Disable rviz static transform publisher --- ouster-ros/launch/record.independent.launch.xml | 2 +- ouster-ros/launch/sensor.independent.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ouster-ros/launch/record.independent.launch.xml b/ouster-ros/launch/record.independent.launch.xml index 16a234d8..b6a11ced 100644 --- a/ouster-ros/launch/record.independent.launch.xml +++ b/ouster-ros/launch/record.independent.launch.xml @@ -74,7 +74,7 @@ - + diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 62a1f110..e192dd9d 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -77,7 +77,7 @@ - + From c4308d4f8f1ddcd457278c108015dbce8a0339e3 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 5 May 2023 12:45:02 -0700 Subject: [PATCH 11/11] Remove rviz static transform publishers hack --- .../launch/record.independent.launch.xml | 1 - .../launch/replay.independent.launch.xml | 1 - ouster-ros/launch/rviz.launch.py | 27 ------------------- ouster-ros/launch/rviz.launch.xml | 11 -------- ouster-ros/launch/sensor.composite.launch.py | 3 +-- ouster-ros/launch/sensor.composite.launch.xml | 1 - .../launch/sensor.independent.launch.py | 3 +-- .../launch/sensor.independent.launch.xml | 1 - ouster-ros/launch/sensor_mtp.launch.xml | 1 - 9 files changed, 2 insertions(+), 47 deletions(-) diff --git a/ouster-ros/launch/record.independent.launch.xml b/ouster-ros/launch/record.independent.launch.xml index b6a11ced..7c03f590 100644 --- a/ouster-ros/launch/record.independent.launch.xml +++ b/ouster-ros/launch/record.independent.launch.xml @@ -74,7 +74,6 @@ - diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml index 5541b5ae..787ffac6 100644 --- a/ouster-ros/launch/replay.independent.launch.xml +++ b/ouster-ros/launch/replay.independent.launch.xml @@ -51,7 +51,6 @@ - diff --git a/ouster-ros/launch/rviz.launch.py b/ouster-ros/launch/rviz.launch.py index 2d753728..bf8ca655 100644 --- a/ouster-ros/launch/rviz.launch.py +++ b/ouster-ros/launch/rviz.launch.py @@ -24,30 +24,6 @@ def generate_launch_description(): ouster_ns_arg = DeclareLaunchArgument( 'ouster_ns', default_value='ouster') - enable_static_tf = LaunchConfiguration('_enable_static_tf_publishers') - enable_static_tf_arg = DeclareLaunchArgument( - '_enable_static_tf_publishers', default_value='false') - - # NOTE: the two static tf publishers are rather a workaround to let rviz2 - # get going and not complain while waiting for the actual sensor frames - # to be published that is when running rviz2 using a parent launch file - # TODO: need to be able to propagate the modified frame names from the - # parameters file to RVIZ launch py. - sensor_imu_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="stp_sensor_imu", - namespace=ouster_ns, - condition=IfCondition(enable_static_tf), - arguments=["0", "0", "0", "0", "0", "0", "os_sensor", "os_imu"]) - sensor_ldr_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="stp_sensor_lidar", - namespace=ouster_ns, - condition=IfCondition(enable_static_tf), - arguments=["0", "0", "0", "0", "0", "0", "os_sensor", "os_lidar"]) - rviz_node = Node( package='rviz2', namespace=ouster_ns, @@ -59,8 +35,5 @@ def generate_launch_description(): return LaunchDescription([ ouster_ns_arg, rviz_config_arg, - enable_static_tf_arg, - sensor_imu_tf, - sensor_ldr_tf, rviz_node ]) diff --git a/ouster-ros/launch/rviz.launch.xml b/ouster-ros/launch/rviz.launch.xml index 82197418..54b490b7 100644 --- a/ouster-ros/launch/rviz.launch.xml +++ b/ouster-ros/launch/rviz.launch.xml @@ -4,8 +4,6 @@ description="Override the default namespace of all ouster nodes"/> - @@ -15,15 +13,6 @@ - - - \ No newline at end of file diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py index c54441c0..0db299ac 100644 --- a/ouster-ros/launch/sensor.composite.launch.py +++ b/ouster-ros/launch/sensor.composite.launch.py @@ -77,8 +77,7 @@ def generate_launch_description(): Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable), - launch_arguments={'_enable_static_tf_publishers': 'false'}.items() + condition=IfCondition(rviz_enable) ) # HACK: to configure and activate the the sensor since state transition diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 2935c6db..717cabd0 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -79,7 +79,6 @@ - diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py index f7d239fe..8ef67d4f 100644 --- a/ouster-ros/launch/sensor.independent.launch.py +++ b/ouster-ros/launch/sensor.independent.launch.py @@ -105,8 +105,7 @@ def generate_launch_description(): Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable), - launch_arguments={'_enable_static_tf_publishers': 'false'}.items() + condition=IfCondition(rviz_enable) ) return launch.LaunchDescription([ diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index e192dd9d..1bb6f464 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -77,7 +77,6 @@ - diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index a6f5f186..77af0335 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -87,7 +87,6 @@ -