Fix QoS for humble and disable lazy subscribers

Author: AntoBrandi

pcl_ros/src/pcl_ros/filters/filter.cpp

@@ -124,8 +124,8 @@ pcl_ros::Filter::subscribe()
   if (use_indices_) {
     // Subscribe to the input using a filter
     auto sensor_qos_profile = rclcpp::SensorDataQoS().keep_last(max_queue_size_);
-    sub_input_filter_.subscribe(this, "input", sensor_qos_profile);
-    sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile);
+    sub_input_filter_.subscribe(this, "input", sensor_qos_profile.get_rmw_qos_profile());
+    sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile.get_rmw_qos_profile());
 
     if (approximate_sync_) {
       sync_input_indices_a_ =
@@ -182,23 +182,16 @@ pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & optio
 void
 pcl_ros::Filter::createPublishers()
 {
-  auto pub_options = rclcpp::PublisherOptions();
-  pub_options.event_callbacks.matched_callback = [this](rclcpp::MatchedInfo & /*info*/) {
-      if (pub_output_->get_subscription_count() == 0) {
-        unsubscribe();
-      } else {
-        if (use_indices_) {
-          if (!sub_input_filter_.getSubscriber() || !sub_indices_filter_.getSubscriber()) {
-            subscribe();
-          }
-        } else {
-          if (!sub_input_) {
-            subscribe();
-          }
-        }
-      }
-    };
-  pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_, pub_options);
+  if (use_indices_) {
+    if (!sub_input_filter_.getSubscriber() || !sub_indices_filter_.getSubscriber()) {
+      subscribe();
+    }
+  } else {
+    if (!sub_input_) {
+      subscribe();
+    }
+  }
+  pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

pcl_ros/src/pcl_ros/filters/project_inliers.cpp

@@ -108,9 +108,9 @@ pcl_ros::ProjectInliers::subscribe()
   auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(max_queue_size_), rmw_qos_profile_default);
   auto sensor_qos_profile =
     rclcpp::QoS(rclcpp::KeepLast(max_queue_size_), rmw_qos_profile_sensor_data);
-  sub_input_filter_.subscribe(this, "input", sensor_qos_profile);
-  sub_indices_filter_.subscribe(this, "indices", qos_profile);
-  sub_model_.subscribe(this, "model", qos_profile);
+  sub_input_filter_.subscribe(this, "input", sensor_qos_profile.get_rmw_qos_profile());
+  sub_indices_filter_.subscribe(this, "indices", qos_profile.get_rmw_qos_profile());
+  sub_model_.subscribe(this, "model", qos_profile.get_rmw_qos_profile());
 
   if (approximate_sync_) {
     sync_input_indices_model_a_ = std::make_shared<

pcl_ros/tools/bag_to_pcd.cpp

@@ -106,7 +106,7 @@ class BagToPCD : public rclcpp::Node
       pcl_conversions::moveToPCL(pointcloud_msg, cloud);
 
       std::stringstream ss;
-      ss << output_directory_ << "/" << msg->recv_timestamp << ".pcd";
+      ss << output_directory_ << "/" << msg->time_stamp << ".pcd";
       RCLCPP_INFO(this->get_logger(), "Writing to: %s", ss.str().c_str());
       pcl::io::savePCDFile(ss.str(), cloud);
       break;