diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index ba5fd4b51..b45034ee7 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -182,7 +182,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ image_transport::create_publisher( impl_->ros_node_.get(), camera_topic, - qos.get_publisher_qos(camera_topic).get_rmw_qos_profile())); + qos.get_publisher_qos(camera_topic, rclcpp::SensorDataQoS()).get_rmw_qos_profile())); // TODO(louise) Uncomment this once image_transport::Publisher has a function to return the // full topic. @@ -208,7 +208,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ impl_->image_pub_.push_back( image_transport::create_publisher( impl_->ros_node_.get(), - camera_topic, qos.get_publisher_qos(camera_topic).get_rmw_qos_profile())); + camera_topic, + qos.get_publisher_qos(camera_topic, rclcpp::SensorDataQoS()).get_rmw_qos_profile())); // RCLCPP_INFO( // impl_->ros_node_->get_logger(), "Publishing %s camera images to [%s]", @@ -235,7 +236,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ impl_->depth_image_pub_ = image_transport::create_publisher( impl_->ros_node_.get(), depth_topic, - qos.get_publisher_qos(depth_topic).get_rmw_qos_profile()); + qos.get_publisher_qos(depth_topic, 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());