Skip to content

Commit

Permalink
Image publishers use SensorDataQoSProfile (#1031)
Browse files Browse the repository at this point in the history
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 <jacob@openrobotics.org>
  • Loading branch information
jacobperron committed Apr 1, 2020
1 parent 00d0d67 commit 14e50ca
Showing 1 changed file with 7 additions and 3 deletions.
10 changes: 7 additions & 3 deletions gazebo_plugins/src/gazebo_ros_camera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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]",
Expand All @@ -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());
Expand Down

0 comments on commit 14e50ca

Please sign in to comment.