Use sensor data QOS profile for sensor_msgs::Image and sensor_msgs::P…

…ointCloud (moveit#664) Co-authored-by: Henry Moore <[email protected]> Co-authored-by: Tyler Weaver <[email protected]>
adigeak · Dec 1, 2022 · 50b03a1 · 50b03a1
1 parent 8dcbe40
commit 50b03a1
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 15 deletions.
diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
@@ -129,7 +129,6 @@ bool DepthImageOctomapUpdater::initialize(const rclcpp::Node::SharedPtr& node)
 
 void DepthImageOctomapUpdater::start()
 {
-  rmw_qos_profile_t custom_qos = rmw_qos_profile_system_default;
   pub_model_depth_image_ = model_depth_transport_->advertiseCamera("model_depth", 1);
 
   std::string prefix = "";
@@ -154,7 +153,7 @@ void DepthImageOctomapUpdater::start()
              const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) {
         return depthImageCallback(depth_msg, info_msg);
       },
-      "raw", custom_qos);
+      "raw", rmw_qos_profile_sensor_data);
 }
 
 void DepthImageOctomapUpdater::stop()

diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/depth_self_filter_nodelet.h
@@ -102,7 +102,6 @@ class DepthSelfFiltering : public nodelet::Nodelet
 
   /** \brief required to avoid listener registration before we are all set*/
   std::mutex connect_mutex_;
-  int queue_size_;
   TransformProvider transform_provider_;
 
   std::shared_ptr<cv_bridge::CvImage> filtered_depth_ptr_;

diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp
@@ -61,7 +61,7 @@ void mesh_filter::DepthSelfFiltering::onInit()
   model_label_transport_ = std::make_shared<image_transport::ImageTransport>(nh);
 
   // Read parameters
-  private_nh.param("queue_size", queue_size_, 1);
+  private_nh.param("queue_size", rmw_qos_profile_sensor_data.depth, 1);
   private_nh.param("near_clipping_plane_distance", near_clipping_plane_distance_, 0.4);
   private_nh.param("far_clipping_plane_distance", far_clipping_plane_distance_, 5.0);
   ;
@@ -76,14 +76,14 @@ void mesh_filter::DepthSelfFiltering::onInit()
   ros::SubscriberStatusCallback rssc = [this](auto&&) { connectCb(); };
 
   std::lock_guard<std::mutex> lock(connect_mutex_);
-  pub_filtered_depth_image_ =
-      filtered_depth_transport_->advertiseCamera("/filtered/depth", queue_size_, itssc, itssc, rssc, rssc);
-  pub_filtered_label_image_ =
-      filtered_label_transport_->advertiseCamera("/filtered/labels", queue_size_, itssc, itssc, rssc, rssc);
-  pub_model_depth_image_ =
-      model_depth_transport_->advertiseCamera("/model/depth", queue_size_, itssc, itssc, rssc, rssc);
-  pub_model_label_image_ =
-      model_depth_transport_->advertiseCamera("/model/label", queue_size_, itssc, itssc, rssc, rssc);
+  pub_filtered_depth_image_ = filtered_depth_transport_->advertiseCamera(
+      "/filtered/depth", rmw_qos_profile_sensor_data.depth, itssc, itssc, rssc, rssc, rmw_qos_profile_sensor_data);
+  pub_filtered_label_image_ = filtered_label_transport_->advertiseCamera(
+      "/filtered/labels", rmw_qos_profile_sensor_data.depth, itssc, itssc, rssc, rssc, rmw_qos_profile_sensor_data);
+  pub_model_depth_image_ = model_depth_transport_->advertiseCamera(
+      "/model/depth", rmw_qos_profile_sensor_data.depth, itssc, itssc, rssc, rssc, rmw_qos_profile_sensor_data);
+  pub_model_label_image_ = model_depth_transport_->advertiseCamera(
+      "/model/label", rmw_qos_profile_sensor_data.depth, itssc, itssc, rssc, rssc, rmw_qos_profile_sensor_data);
 
   filtered_depth_ptr_ = std::make_shared<cv_bridge::CvImage>();
   filtered_label_ptr_ = std::make_shared<cv_bridge::CvImage>();
@@ -209,7 +209,8 @@ void mesh_filter::DepthSelfFiltering::connectCb()
   {
     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
     sub_depth_image_ =
-        input_depth_transport_->subscribeCamera("depth", queue_size_, &DepthSelfFiltering::depthCb, this, hints);
+        input_depth_transport_->subscribeCamera("depth", rmw_qos_profile_sensor_data.depth,
+                                                &DepthSelfFiltering::depthCb, this, hints, rmw_qos_profile_sensor_data);
   }
 }
 

diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
@@ -100,16 +100,18 @@ void PointCloudOctomapUpdater::start()
   if (!ns_.empty())
     prefix = ns_ + "/";
 
+  rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
   if (!filtered_cloud_topic_.empty())
   {
     filtered_cloud_publisher_ =
-        node_->create_publisher<sensor_msgs::msg::PointCloud2>(prefix + filtered_cloud_topic_, 10);
+        node_->create_publisher<sensor_msgs::msg::PointCloud2>(prefix + filtered_cloud_topic_, rclcpp::SensorDataQoS());
   }
 
   if (point_cloud_subscriber_)
     return;
   /* subscribe to point cloud topic using tf filter*/
-  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(node_, point_cloud_topic_);
+  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(node_, point_cloud_topic_,
+                                                                                           rmw_qos_profile_sensor_data);
   if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty())
   {
     point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(