From a46852099c7f9fdb99b9916fe84524bb82b943c5 Mon Sep 17 00:00:00 2001 From: Tom Panzarella Date: Fri, 13 Mar 2020 18:07:03 -0400 Subject: [PATCH] Runtime selectable QoS to allow for recording sensor data with rosbag2 (#26) * Rule-of-Zero on DataProcessorInterface * Added runtime-selectable QoS support to allow for recording sensor data via rosbag2 in Eloquent. Apparently, as of this writing, rosbag2 cannot record topic data whose QoS has a reliability level of anything but `RELIABLE`. See: https://github.com/ros2/rosbag2/issues/125. The `rmw_qos_profile_sensor_data` (which we are using in this project *a priori*) employs `BEST_EFFORT` reliability. I've added a new parameter `use_system_default_qos_for_sensor_data` to the node and parameter file. By default this is set to `False` which maintains the original behavior of this package. However, if set to `True`, rather than `rmw_qos_profile_sensor_data` we use ``rmw_qos_profile_default` as our QoS profile for the sensor topics. This (supposedly) most closely mimics ROS (classic) behavior but more importantly, allows rosbag2 to record data from these topics. Hopefully this issue with rosbag2 gets addressed by Foxy and we can deprecate this feature. But for now, not having the ability to record data for offline analysis is very limiting and we need something like this in this driver package. * Style fix * more style fixes * Resolved comments from Steve on PR #26 * Lowered log level from WARN to INFO when using system defaults QoS --- README.md | 43 ++++++++++--------- .../ros2_ouster/OS1/processor_factories.hpp | 32 ++++++++------ .../OS1/processors/image_processor.hpp | 13 +++--- .../OS1/processors/imu_processor.hpp | 7 +-- .../OS1/processors/pointcloud_processor.hpp | 10 +++-- .../OS1/processors/scan_processor.hpp | 8 ++-- .../interfaces/data_processor_interface.hpp | 5 --- .../include/ros2_ouster/ouster_driver.hpp | 2 + ros2_ouster/params/os1.yaml | 5 +++ ros2_ouster/src/ouster_driver.cpp | 19 ++++++-- 10 files changed, 88 insertions(+), 56 deletions(-) diff --git a/README.md b/README.md index 7e62997..1c7cbee 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # ROS2 Ouster Drivers -These are an implementation of ROS2 drivers for the Ouster OS-1 3D lidars. This includes all models of the OS-1 from 16 to 128 beams. +These are an implementation of ROS2 drivers for the Ouster OS-1 3D lidars. This includes all models of the OS-1 from 16 to 128 beams. You can find a few videos looking over the sensor below. They both introduce the ROS1 driver but are extremely useful references regardless: @@ -9,11 +9,11 @@ OS-1 Networking Setup | OS-1 Data Overview [![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/92ajXjIxDGM/0.jpg)](http://www.youtube.com/watch?v=92ajXjIxDGM) | [![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/4VgGG8Xe4IA/0.jpg)](http://www.youtube.com/watch?v=4VgGG8Xe4IA) -I also happen to run a YouTube channel, [Robots for Robots](https://www.youtube.com/channel/UCZT16dToD1ov6lnoEcPL6rw), focusing on robotics, sensors, and industry insights. If you like this work and want to hear about some other things I am working on, I'd appreciate if you watch, like, and subscribe! +I also happen to run a YouTube channel, [Robots for Robots](https://www.youtube.com/channel/UCZT16dToD1ov6lnoEcPL6rw), focusing on robotics, sensors, and industry insights. If you like this work and want to hear about some other things I am working on, I'd appreciate if you watch, like, and subscribe! ## Documentation -Documentation can be generated using Doxygen. +Documentation can be generated using Doxygen. Run `doxygen` in the root of this repository. It will generate a `/doc/*` directory containing the documentation. Entrypoint in a browser is `index.html`. @@ -40,16 +40,17 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md). | `reset` | std_srvs/Empty | Reset the sensor's connection | | `GetMetadata` | ouster_msgs/GetMetadata | Get information about the sensor | -| Parameter | Type | Description | -|-------------------|-------------------------|-----------------------------------------------------| -| `lidar_ip` | String | IP of lidar (ex. 10.5.5.87) | -| `computer_ip` | String | IP of computer to get data (ex. 10.5.5.1) | -| `lidar_mode` | String | Mode of data capture, default `512x10` | -| `imu_port` | int | Port of IMU data, default 7503 | -| `lidar_port` | int | Port of laser data, default 7502 | -| `sensor_frame` | String | TF frame of sensor, default `laser_sensor_frame` | -| `laser_frame` | String | TF frame of laser data, default `laser_data_frame` | -| `imu_frame` | String | TF frame of imu data, default `imu_data_frame` | +| Parameter | Type | Description | +|--------------------------|---------|----------------------------------------------------------------------| +| `lidar_ip` | String | IP of lidar (ex. 10.5.5.87) | +| `computer_ip` | String | IP of computer to get data (ex. 10.5.5.1) | +| `lidar_mode` | String | Mode of data capture, default `512x10` | +| `imu_port` | int | Port of IMU data, default 7503 | +| `lidar_port` | int | Port of laser data, default 7502 | +| `sensor_frame` | String | TF frame of sensor, default `laser_sensor_frame` | +| `laser_frame` | String | TF frame of laser data, default `laser_data_frame` | +| `imu_frame` | String | TF frame of imu data, default `imu_data_frame` | +| `use_system_default_qos` | bool | Publish data with default QoS for rosbag2 recording, default `False` | @@ -60,7 +61,7 @@ Note: TF will provide you the transformations from the sensor frame to each of t This package was intentionally designed for new capabilities to be added. Whether that being supporting new classes of Ouster lidars (OS1-custom, OS2, ...) or supporting new ways of processing the data packets. ### Additional Lidar Processing -It can be imagined that if you have a stream of lidar or IMU packets, you may want to process them differently. If you're working with a high speed vehicle, you may want the packets projected into a pointcloud and published with little batching inside the driver. If you're working with pointclouds for machine learning, you may only want the pointcloud to include the `XYZ` information and not the intensity, reflectivity, and noise information to reduce dimensionality. +It can be imagined that if you have a stream of lidar or IMU packets, you may want to process them differently. If you're working with a high speed vehicle, you may want the packets projected into a pointcloud and published with little batching inside the driver. If you're working with pointclouds for machine learning, you may only want the pointcloud to include the `XYZ` information and not the intensity, reflectivity, and noise information to reduce dimensionality. In any case, I provide a set of logical default processing implementations on the lidar and IMU packets. These are implementations of the `ros2_ouster::DataProcessorInterface` class in the `interfaces` directory. To create your own processor to change the pointcloud type, buffering methodology, or some new cool thing, you must create an implementation of a data processor. @@ -77,7 +78,7 @@ Some examples: ### Additional Lidar Units To create a new lidar for this driver, you only need to make an implementation of the `ros2_ouster::SensorInterface` class and include any required SDKs. Then, in the `driver_types.hpp` file, add your new interface as a template of the `OusterDriver` and you're good to go. -You may need to add an additional `main` method for the new templated program, depending if you're using components. If it uses another underlying SDK other than `OS1` you will also need to create new processors for it as the processors are bound to a specific unit as the data formatting may be different. If they are the same, you can reuse the `OS1` processors. +You may need to add an additional `main` method for the new templated program, depending if you're using components. If it uses another underlying SDK other than `OS1` you will also need to create new processors for it as the processors are bound to a specific unit as the data formatting may be different. If they are the same, you can reuse the `OS1` processors. ## Lifecycle @@ -113,21 +114,21 @@ The `[eth name]` is the nework interface you're connecting to. On older Linux sy ip addr show dev [eth name] ``` -The output you see from `show` should look something like `[eth name] ... state DOWN ...`. Its only important that you see `DOWN` and not `UP`. Next, lets setup a static IP address for your machine so you can rely on this in the future. Ouster uses the 10.5.5.* range, and I don't see a compelling reason to argue with it. +The output you see from `show` should look something like `[eth name] ... state DOWN ...`. Its only important that you see `DOWN` and not `UP`. Next, lets setup a static IP address for your machine so you can rely on this in the future. Ouster uses the 10.5.5.* range, and I don't see a compelling reason to argue with it. ``` sudo ip addr add 10.5.5.1/24 dev [eth name] ``` -Now, lets setup the connection. At this point you may now plug in and power on your sensor. +Now, lets setup the connection. At this point you may now plug in and power on your sensor. ``` sudo ip link set [eth name] up sudo addr show dev [eth name] ``` -The output you see from `show` should look something like `[eth name] ... state UP ...`. Its only important that you see `UP` now and not `DOWN`. At this point, you've setup the networking needed for the one time setup. +The output you see from `show` should look something like `[eth name] ... state UP ...`. Its only important that you see `UP` now and not `DOWN`. At this point, you've setup the networking needed for the one time setup. ### Connection @@ -135,7 +136,7 @@ We can setup the network connection to the sensor now with the proper settings. ``` sudo dnsmasq -C /dev/null -kd -F 10.5.5.50,10.5.5.100 -i [eth name] --bind-dynamic -``` +``` Instantly you should see something similar to: @@ -176,8 +177,8 @@ Now that we have a connection over the network, lets view some data. After build ros2 launch ros2_ouster os1_launch.py ``` -Make sure to update your parameters file if you don't use the default IPs (10.5.5.1, 10.5.5.87). You may also use the `.local` version of your ouster lidar. To find your IPs, see the `dnsmasq` output or check with `nmap -SP 10.5.5.*/24`. +Make sure to update your parameters file if you don't use the default IPs (10.5.5.1, 10.5.5.87). You may also use the `.local` version of your ouster lidar. To find your IPs, see the `dnsmasq` output or check with `nmap -SP 10.5.5.*/24`. Now that your connection is up (hopefully), you can view this information in RViz. Open an RViz session and subscribe to the points, images, and IMU topics in the laser frame. -Have fun! \ No newline at end of file +Have fun! diff --git a/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp b/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp index e2da04b..e6913e9 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp @@ -18,6 +18,7 @@ #include #include +#include "rclcpp/qos.hpp" #include "ros2_ouster/conversions.hpp" #include "ros2_ouster/OS1/processors/image_processor.hpp" #include "ros2_ouster/OS1/processors/imu_processor.hpp" @@ -35,9 +36,10 @@ namespace ros2_ouster inline ros2_ouster::DataProcessorInterface * createImageProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) { - return new OS1::ImageProcessor(node, mdata, frame); + return new OS1::ImageProcessor(node, mdata, frame, qos); } /** @@ -48,9 +50,10 @@ inline ros2_ouster::DataProcessorInterface * createImageProcessor( inline ros2_ouster::DataProcessorInterface * createPointcloudProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) { - return new OS1::PointcloudProcessor(node, mdata, frame); + return new OS1::PointcloudProcessor(node, mdata, frame, qos); } /** @@ -61,9 +64,10 @@ inline ros2_ouster::DataProcessorInterface * createPointcloudProcessor( inline ros2_ouster::DataProcessorInterface * createIMUProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) { - return new OS1::IMUProcessor(node, mdata, frame); + return new OS1::IMUProcessor(node, mdata, frame, qos); } /** @@ -74,31 +78,33 @@ inline ros2_ouster::DataProcessorInterface * createIMUProcessor( inline ros2_ouster::DataProcessorInterface * createScanProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) { - return new OS1::ScanProcessor(node, mdata, frame); + return new OS1::ScanProcessor(node, mdata, frame, qos); } inline std::multimap createProcessors( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, const std::string & imu_frame, - const std::string & laser_frame) + const std::string & laser_frame, + const rclcpp::QoS & qos) { std::multimap data_processors; data_processors.insert(std::pair( ClientState::IMU_DATA, createIMUProcessor( - node, mdata, imu_frame))); + node, mdata, imu_frame, qos))); data_processors.insert(std::pair( ClientState::LIDAR_DATA, createPointcloudProcessor( - node, mdata, laser_frame))); + node, mdata, laser_frame, qos))); data_processors.insert(std::pair( ClientState::LIDAR_DATA, createImageProcessor( - node, mdata, laser_frame))); + node, mdata, laser_frame, qos))); data_processors.insert(std::pair( ClientState::LIDAR_DATA, createScanProcessor( - node, mdata, laser_frame))); + node, mdata, laser_frame, qos))); return data_processors; } diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp index 826c249..9c4e894 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp @@ -20,6 +20,8 @@ #include #include +#include "rclcpp/qos.hpp" + #include "ros2_ouster/conversions.hpp" #include "sensor_msgs/msg/image.hpp" @@ -50,7 +52,8 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface ImageProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) : DataProcessorInterface(), _node(node), _frame(frame) { _height = OS1::pixels_per_column; @@ -61,13 +64,13 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface mdata.beam_altitude_angles); _range_image_pub = _node->create_publisher( - "range_image", rclcpp::SensorDataQoS()); + "range_image", qos); _intensity_image_pub = _node->create_publisher( - "intensity_image", rclcpp::SensorDataQoS()); + "intensity_image", qos); _noise_image_pub = _node->create_publisher( - "noise_image", rclcpp::SensorDataQoS()); + "noise_image", qos); _reflectivity_image_pub = _node->create_publisher( - "reflectivity_image", rclcpp::SensorDataQoS()); + "reflectivity_image", qos); _range_image.width = _width; _range_image.height = _height; diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp index 9ad5597..d0a0fa1 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp @@ -20,6 +20,7 @@ #include "ros2_ouster/conversions.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/imu.hpp" @@ -45,11 +46,11 @@ class IMUProcessor : public ros2_ouster::DataProcessorInterface IMUProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) : DataProcessorInterface(), _node(node), _frame(frame) { - _pub = node->create_publisher( - "imu", rclcpp::SensorDataQoS()); + _pub = node->create_publisher("imu", qos); } /** diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp index c19fe0c..d555204 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp @@ -18,6 +18,8 @@ #include #include +#include "rclcpp/qos.hpp" + #include "ros2_ouster/conversions.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" @@ -46,7 +48,8 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface PointcloudProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) : DataProcessorInterface(), _node(node), _frame(frame) { _height = OS1::pixels_per_column; @@ -54,9 +57,10 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface OS1::lidar_mode_of_string(mdata.mode)); _xyz_lut = OS1::make_xyz_lut(_width, _height, mdata.beam_azimuth_angles, mdata.beam_altitude_angles); - _cloud = std::make_shared>(_width, _height); + _cloud = + std::make_shared>(_width, _height); _pub = _node->create_publisher( - "points", rclcpp::SensorDataQoS()); + "points", qos); _batch_and_publish = OS1::batch_to_iter::iterator>( diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp index 7eed87c..f60e4ab 100644 --- a/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp +++ b/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp @@ -18,6 +18,8 @@ #include #include +#include "rclcpp/qos.hpp" + #include "ros2_ouster/conversions.hpp" #include "sensor_msgs/msg/laser_scan.hpp" @@ -49,12 +51,12 @@ class ScanProcessor : public ros2_ouster::DataProcessorInterface ScanProcessor( const rclcpp_lifecycle::LifecycleNode::SharedPtr node, const ros2_ouster::Metadata & mdata, - const std::string & frame) + const std::string & frame, + const rclcpp::QoS & qos) : DataProcessorInterface(), _node(node), _frame(frame) { _mdata = mdata; - _pub = _node->create_publisher( - "scan", rclcpp::SensorDataQoS()); + _pub = _node->create_publisher("scan", qos); _height = OS1::pixels_per_column; _width = OS1::n_cols_of_lidar_mode( OS1::lidar_mode_of_string(mdata.mode)); diff --git a/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp b/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp index ad85191..5d9a46d 100644 --- a/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp +++ b/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp @@ -38,11 +38,6 @@ class DataProcessorInterface */ DataProcessorInterface() {} - /** - * @brief Destructor of the data processor interface - */ - ~DataProcessorInterface() {} - /** * @brief Process a packet with the lidar-specific APIs * @param data packet input diff --git a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp index c03067e..f7cd697 100644 --- a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp +++ b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp @@ -136,6 +136,8 @@ class OusterDriver : public lifecycle_interface::LifecycleInterface std::string _laser_sensor_frame, _laser_data_frame, _imu_data_frame; std::unique_ptr _tf_b; + + bool _use_system_default_qos; }; } // namespace ros2_ouster diff --git a/ros2_ouster/params/os1.yaml b/ros2_ouster/params/os1.yaml index 66a8275..b0828c0 100644 --- a/ros2_ouster/params/os1.yaml +++ b/ros2_ouster/params/os1.yaml @@ -8,3 +8,8 @@ ouster_driver: sensor_frame: laser_sensor_frame laser_frame: laser_data_frame imu_frame: imu_data_frame + + # if False, data are published with sensor data QoS. This is preferrable + # for production but default QoS is needed for rosbag. + # See: https://github.com/ros2/rosbag2/issues/125 + use_system_default_qos: False diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp index f5ac152..c1bebfa 100644 --- a/ros2_ouster/src/ouster_driver.cpp +++ b/ros2_ouster/src/ouster_driver.cpp @@ -17,6 +17,7 @@ #include #include +#include "rclcpp/qos.hpp" #include "ros2_ouster/exception.hpp" #include "ros2_ouster/driver_types.hpp" @@ -40,6 +41,7 @@ OusterDriver::OusterDriver(const rclcpp::NodeOptions & options) this->declare_parameter("sensor_frame", std::string("laser_sensor_frame")); this->declare_parameter("laser_frame", std::string("laser_data_frame")); this->declare_parameter("imu_frame", std::string("imu_data_frame")); + this->declare_parameter("use_system_default_qos", false); } template @@ -68,6 +70,7 @@ void OusterDriver::onConfigure() _laser_sensor_frame = get_parameter("sensor_frame").as_string(); _laser_data_frame = get_parameter("laser_frame").as_string(); _imu_data_frame = get_parameter("imu_frame").as_string(); + _use_system_default_qos = get_parameter("use_system_default_qos").as_bool(); RCLCPP_INFO(this->get_logger(), "Connecting to sensor at %s.", lidar_config.lidar_ip.c_str()); @@ -90,10 +93,20 @@ void OusterDriver::onConfigure() ros2_ouster::Metadata mdata = _sensor->getMetadata(); - _data_processors = ros2_ouster::createProcessors( - shared_from_this(), mdata, _imu_data_frame, _laser_data_frame); + if (_use_system_default_qos) { + RCLCPP_INFO( + this->get_logger(), "Using system defaults QoS for sensor data"); + _data_processors = ros2_ouster::createProcessors( + shared_from_this(), mdata, _imu_data_frame, _laser_data_frame, + rclcpp::SystemDefaultsQoS()); + } else { + _data_processors = ros2_ouster::createProcessors( + shared_from_this(), mdata, _imu_data_frame, _laser_data_frame, + rclcpp::SensorDataQoS()); + } - _tf_b = std::make_unique(shared_from_this()); + _tf_b = std::make_unique( + shared_from_this()); broadcastStaticTransforms(mdata); }