Skip to content

Commit

Permalink
Finish QoS updates (#1019)
Browse files Browse the repository at this point in the history
This implements the remainder of #847:

 - Make sure publishers default to system defaults (reliable)
- Add QoS overriding where possible (some of the image_transport /
message_filters stuff doesn't really support that)
 - Use the matching heuristic for subscribers consistently
  • Loading branch information
mikeferguson committed Aug 19, 2024
1 parent ac6707b commit 95d4769
Show file tree
Hide file tree
Showing 26 changed files with 191 additions and 61 deletions.
4 changes: 4 additions & 0 deletions depth_image_proc/doc/tutorials.rst
Original file line number Diff line number Diff line change
Expand Up @@ -57,3 +57,7 @@ parameter is used:
Remapping camera_info Topics
----------------------------
See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#remapping-camera-info-topics>`_.

Using QoS Overrides
-------------------
See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#using-qos-overrides>`_.
1 change: 1 addition & 0 deletions depth_image_proc/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
<depend>image_geometry</depend>
<depend>image_transport</depend>
<depend>libopencv-dev</depend>
<depend>image_proc</depend>
<depend>message_filters</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand Down
15 changes: 11 additions & 4 deletions depth_image_proc/src/convert_metric.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@

#include <rclcpp/rclcpp.hpp>
#include <image_transport/image_transport.hpp>
#include <image_proc/utils.hpp>
#include <sensor_msgs/image_encodings.hpp>

namespace depth_image_proc
Expand Down Expand Up @@ -68,7 +69,7 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
// TransportHints does not actually declare the parameter
this->declare_parameter<std::string>("image_transport", "raw");

// Create publisher with connect callback
// Setup lazy subscriber using publisher connection callback
rclcpp::PublisherOptions pub_options;
pub_options.event_callbacks.matched_callback =
[this](rclcpp::MatchedInfo &)
Expand All @@ -83,18 +84,24 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
std::string topic = node_base->resolve_topic_or_service_name("image_raw", false);
// Get transport hints
image_transport::TransportHints hints(this);
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
sub_raw_ = image_transport::create_subscription(
this, topic,
std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
hints.getTransport());
hints.getTransport(),
qos_profile);
}
};
// For compressed topics to remap appropriately, we need to pass a
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("image", false);
pub_depth_ =
image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_depth_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default,
pub_options);
}

void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
Expand Down
16 changes: 12 additions & 4 deletions depth_image_proc/src/crop_foremost.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include "depth_image_proc/visibility.h"

#include <rclcpp/rclcpp.hpp>
#include <image_proc/utils.hpp>
#include <image_transport/image_transport.hpp>
#include <opencv2/imgproc/imgproc.hpp>

Expand Down Expand Up @@ -74,7 +75,8 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)

distance_ = this->declare_parameter("distance", 0.0);

// Create publisher with connect callback

// Setup lazy subscriber using publisher connection callback
rclcpp::PublisherOptions pub_options;
pub_options.event_callbacks.matched_callback =
[this](rclcpp::MatchedInfo &)
Expand All @@ -89,18 +91,24 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
std::string topic = node_base->resolve_topic_or_service_name("image_raw", false);
// Get transport hints
image_transport::TransportHints hints(this);
// Create publisher with same QoS as subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
sub_raw_ = image_transport::create_subscription(
this, topic,
std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
hints.getTransport());
hints.getTransport(),
qos_profile);
}
};
// For compressed topics to remap appropriately, we need to pass a
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("image", false);
pub_depth_ =
image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);

// Create publisher - allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_depth_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default,
pub_options);
}

void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
Expand Down
4 changes: 3 additions & 1 deletion depth_image_proc/src/disparity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -119,8 +119,10 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
sub_info_.subscribe(this, "right/camera_info");
}
};
// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_disparity_ = create_publisher<stereo_msgs::msg::DisparityImage>(
"left/disparity", rclcpp::SensorDataQoS(), pub_options);
"left/disparity", rclcpp::SystemDefaultsQoS(), pub_options);
}

void DisparityNode::depthCb(
Expand Down
17 changes: 12 additions & 5 deletions depth_image_proc/src/point_cloud_xyz.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@

#include <depth_image_proc/point_cloud_xyz.hpp>
#include <rclcpp/rclcpp.hpp>
#include <image_proc/utils.hpp>
#include <image_transport/image_transport.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <depth_image_proc/conversions.hpp>
Expand Down Expand Up @@ -75,10 +76,12 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("image_rect", false);

// Get transport and QoS
// Get transport hints
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
auto custom_qos = rmw_qos_profile_system_default;
custom_qos.depth = queue_size_;

// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
qos_profile.depth = queue_size_;

sub_depth_ = image_transport::create_camera_subscription(
this,
Expand All @@ -87,10 +90,14 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
&PointCloudXyzNode::depthCb, this, std::placeholders::_1,
std::placeholders::_2),
depth_hints.getTransport(),
custom_qos);
qos_profile);
}
};
pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);

// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_point_cloud_ =
create_publisher<PointCloud2>("points", rclcpp::SystemDefaultsQoS(), pub_options);
}

void PointCloudXyzNode::depthCb(
Expand Down
17 changes: 11 additions & 6 deletions depth_image_proc/src/point_cloud_xyz_radial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@

#include <depth_image_proc/point_cloud_xyz_radial.hpp>
#include <rclcpp/rclcpp.hpp>
#include <image_proc/utils.hpp>
#include <image_transport/image_transport.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <depth_image_proc/depth_traits.hpp>
Expand All @@ -56,7 +57,7 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
// Read parameters
queue_size_ = this->declare_parameter<int>("queue_size", 5);

// Create publisher with connect callback
// Setup lazy subscriber using publisher connection callback
rclcpp::PublisherOptions pub_options;
pub_options.event_callbacks.matched_callback =
[this](rclcpp::MatchedInfo & s)
Expand All @@ -69,10 +70,11 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("depth/image_raw", false);
// Get transport and QoS
// Get transport hints
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
auto custom_qos = rmw_qos_profile_system_default;
custom_qos.depth = queue_size_;
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, topic);
qos_profile.depth = queue_size_;
// Create subscriber
sub_depth_ = image_transport::create_camera_subscription(
this,
Expand All @@ -81,11 +83,14 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
&PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
std::placeholders::_2),
depth_hints.getTransport(),
custom_qos);
qos_profile);
}
};

// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(
"points", rclcpp::SensorDataQoS(), pub_options);
"points", rclcpp::SystemDefaultsQoS(), pub_options);
}

void PointCloudXyzRadialNode::depthCb(
Expand Down
5 changes: 4 additions & 1 deletion depth_image_proc/src/point_cloud_xyzi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,10 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
sub_info_.subscribe(this, intensity_info_topic);
}
};
pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SensorDataQoS(), pub_options);
// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SystemDefaultsQoS(),
pub_options);
}

void PointCloudXyziNode::imageCb(
Expand Down
16 changes: 12 additions & 4 deletions depth_image_proc/src/point_cloud_xyzi_radial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@

#include "depth_image_proc/visibility.h"

#include <image_proc/utils.hpp>
#include <image_transport/camera_common.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -73,7 +74,7 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
std::placeholders::_2,
std::placeholders::_3));

// Create publisher with connect callback
// Setup lazy subscriber using publisher connection callback
rclcpp::PublisherOptions pub_options;
pub_options.event_callbacks.matched_callback =
[this](rclcpp::MatchedInfo & s)
Expand All @@ -98,16 +99,23 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o

// depth image can use different transport.(e.g. compressedDepth)
image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport());
// Create subscriber with QoS matched to subscribed topic publisher
auto depth_qos_profile = image_proc::getTopicQosProfile(this, depth_topic);
sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport(), depth_qos_profile);

// intensity uses normal ros transport hints.
image_transport::TransportHints hints(this);
sub_intensity_.subscribe(this, intensity_topic, hints.getTransport());
// Create subscriber with QoS matched to subscribed topic publisher
auto qos_profile = image_proc::getTopicQosProfile(this, intensity_topic);
sub_intensity_.subscribe(this, intensity_topic, hints.getTransport(), qos_profile);
sub_info_.subscribe(this, intensity_info_topic);
}
};

// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(
"points", rclcpp::SensorDataQoS(), pub_options);
"points", rclcpp::SystemDefaultsQoS(), pub_options);
}

void PointCloudXyziRadialNode::imageCb(
Expand Down
5 changes: 4 additions & 1 deletion depth_image_proc/src/point_cloud_xyzrgb.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,10 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
sub_info_.subscribe(this, rgb_info_topic);
}
};
pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SystemDefaultsQoS(),
pub_options);
}

void PointCloudXyzrgbNode::imageCb(
Expand Down
5 changes: 4 additions & 1 deletion depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,10 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions
sub_info_.subscribe(this, rgb_info_topic);
}
};
pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SystemDefaultsQoS(),
pub_options);
}

void PointCloudXyzrgbRadialNode::imageCb(
Expand Down
2 changes: 2 additions & 0 deletions depth_image_proc/src/register.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,8 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
std::string topic =
node_base->resolve_topic_or_service_name("depth_registered/image_rect", false);

// Allow overriding QoS settings (history, depth, reliability)
pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
pub_registered_ =
image_transport::create_camera_publisher(
this, topic,
Expand Down
80 changes: 80 additions & 0 deletions image_pipeline/doc/tutorials.rst
Original file line number Diff line number Diff line change
Expand Up @@ -24,3 +24,83 @@ camera info topic. An example:
* The fully resolved name for the camera info is now ``my_camera/camera_info``.
* If your camera driver actually publishes ``another_ns/camera_info``, then
you would have to remap ``my_camera/camera_info`` to ``another_ns/camera_info``.

.. _`Using QoS Overrides`:

Using QoS Overrides
-------------------
Most components in image_pipeline follow the Quality of Service (QoS) recommendations
of `REP-2003 <https://ros.org/reps/rep-2003.html>`_ by default. This means that
subscribers are configured with the "Sensor Data" QoS (which uses "best effort"),
and publishers are configured with "System Default" QoS (which uses "reliable" transport).

These QoS settings work well for many applications, but can be overridden using the
standard features of recent ROS 2 releases. This involves adding additional parameters
to your YAML or launch file. For example, we could update the
`image_publisher_file.launch.py` launch file to change the QoS settings:

.. code-block:: python
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
import launch_ros.actions
def generate_launch_description():
filename = os.path.join(get_package_share_directory('image_publisher'), 'launch',
'splash.png')
return LaunchDescription([
launch_ros.actions.Node(
package='image_publisher', executable='image_publisher_node', output='screen',
arguments=[filename],
parameters=[{
'qos_overrides': {
'/camera/image_raw': {
'publisher': {
'reliability': 'best_effort',
'history': 'keep_last',
'depth': 100,
}
}
},
}],
remappings=[('image_raw', '/camera/image_raw'),
('camera_info', '/camera/camera_info')]),
])
If we then run the launch file, we can see our settings are applied:

.. code-block:: bash
$ ros2 topic info /camera/image_raw -v
Type: sensor_msgs/msg/Image
Publisher count: 1
Node name: ImagePublisher
Node namespace: /
Topic type: sensor_msgs/msg/Image
Topic type hash: RIHS01_d31d41a9a4c4bc8eae9be757b0beed306564f7526c88ea6a4588fb9582527d47
Endpoint type: PUBLISHER
GID: 01.10.bf.bd.b7.85.a8.33.58.34.5c.ae.00.00.17.03
QoS profile:
Reliability: BEST_EFFORT
History (Depth): KEEP_LAST (100)
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite
Subscription count: 0
A few things to note:

* The topic name (``/camera/image_raw``) must be the fully resolved topic name,
and therefore we use the remapped topic name rather than the name in the code
for the component.
* Only ``reliability``, ``history``, and ``depth`` can be overwritten.

For more information on QoS overrides, see the `design doc <https://design.ros2.org/articles/qos_configurability.html>`_.
4 changes: 4 additions & 0 deletions image_proc/doc/tutorials.rst
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ Remapping camera_info Topics
----------------------------
See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#remapping-camera-info-topics>`_.

Using QoS Overrides
-------------------
See `tutorial in image_pipline <https://docs.ros.org/en/rolling/p/image_pipeline/tutorials.html#using-qos-overrides>`_.

.. _Using image_proc Launch File:

Using image_proc Launch File
Expand Down
Loading

0 comments on commit 95d4769

Please sign in to comment.