From 14e50ca52f31964aa1bb0455f85ec1630b87b7c7 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 15 Jan 2020 11:21:07 -0800 Subject: [PATCH] Image publishers use SensorDataQoSProfile (#1031) All other sensor publishers were updated previously to use the same profile (#926). I'm not sure if the image publishers were overlooked or the image_transport API didn't support setting the QoS profile at the time. Signed-off-by: Jacob Perron --- gazebo_plugins/src/gazebo_ros_camera.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 5711625a5..ea3acee49 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -176,7 +176,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ // TODO(louise) Migrate image_connect logic once SubscriberStatusCallback is ported to ROS2 impl_->image_pub_.push_back( image_transport::create_publisher( - impl_->ros_node_.get(), impl_->camera_name_ + "/image_raw")); + impl_->ros_node_.get(), + impl_->camera_name_ + "/image_raw", rclcpp::SensorDataQoS().get_rmw_qos_profile())); // TODO(louise) Uncomment this once image_transport::Publisher has a function to return the // full topic. @@ -200,7 +201,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ impl_->image_pub_.push_back( image_transport::create_publisher( impl_->ros_node_.get(), - impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw")); + impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw", + rclcpp::SensorDataQoS().get_rmw_qos_profile())); // RCLCPP_INFO( // impl_->ros_node_->get_logger(), "Publishing %s camera images to [%s]", @@ -223,7 +225,9 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ if (impl_->sensor_type_ == GazeboRosCameraPrivate::DEPTH) { // Depth image publisher impl_->depth_image_pub_ = image_transport::create_publisher( - impl_->ros_node_.get(), impl_->camera_name_ + "/depth/image_raw"); + impl_->ros_node_.get(), + impl_->camera_name_ + "/depth/image_raw", + rclcpp::SensorDataQoS().get_rmw_qos_profile()); // RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing depth images to [%s]", // impl_->depth_image_pub_.getTopic().c_str());