migrate passthrough and project_inliers filters (#395)

* migrate passthrough and project_inliers filters

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* - remove throwing runtime_error (result always true)
- use get_subscription_count()

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* change comparison operator

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

---------

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

pcl_ros/CMakeLists.txt

@@ -84,8 +84,8 @@ ament_target_dependencies(pcl_ros_tf
 add_library(pcl_ros_filters SHARED
   src/pcl_ros/filters/extract_indices.cpp
   src/pcl_ros/filters/filter.cpp
-#  src/pcl_ros/filters/passthrough.cpp
+  src/pcl_ros/filters/passthrough.cpp
-#  src/pcl_ros/filters/project_inliers.cpp
+  src/pcl_ros/filters/project_inliers.cpp
 #  src/pcl_ros/filters/radius_outlier_removal.cpp
 #  src/pcl_ros/filters/statistical_outlier_removal.cpp
 #  src/pcl_ros/filters/voxel_grid.cpp
@@ -97,6 +97,14 @@ rclcpp_components_register_node(pcl_ros_filters
   PLUGIN "pcl_ros::ExtractIndices"
   EXECUTABLE filter_extract_indices_node
 )
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::PassThrough"
+  EXECUTABLE filter_passthrough_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::ProjectInliers"
+  EXECUTABLE filter_project_inliers_node
+)
 class_loader_hide_library_symbols(pcl_ros_filters)
 #
 ### Declare the pcl_ros_segmentation library

pcl_ros/include/pcl_ros/filters/filter.hpp

@@ -71,6 +71,9 @@ protected:
   void
   use_frame_params();
 
+  /** \brief Add common parameters */
+  std::vector<std::string> add_common_params();
+
   /** \brief The input PointCloud subscriber. */
   rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
 

pcl_ros/include/pcl_ros/filters/passthrough.hpp

@@ -40,6 +40,7 @@
 
 // PCL includes
 #include <pcl/filters/passthrough.h>
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
 
 namespace pcl_ros
@@ -51,9 +52,6 @@ namespace pcl_ros
 class PassThrough : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
-
   /** \brief Call the actual filter.
     * \param input the input point cloud dataset
     * \param indices the input set of indices to use from \a input
@@ -61,10 +59,10 @@ protected:
     */
   inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
-    PointCloud2 & output)
+    PointCloud2 & output) override
   {
-    boost::mutex::scoped_lock lock(mutex_);
+    std::lock_guard<std::mutex> lock(mutex_);
     pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
     pcl_conversions::toPCL(*(input), *(pcl_input));
     impl_.setInputCloud(pcl_input);
@@ -74,19 +72,13 @@ protected:
     pcl_conversions::moveFromPCL(pcl_output, output);
   }
 
-  /** \brief Child initialization routine.
+  /** \brief Parameter callback
-    * \param nh ROS node handle
+    * \param params parameter values to set
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
     */
-  bool
+  rcl_interfaces::msg::SetParametersResult
-  child_init(ros::NodeHandle & nh, bool & has_service);
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure service callback.
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
-    * \param config the dynamic reconfigure FilterConfig object
-    * \param level the dynamic reconfigure level
-    */
-  void
-  config_callback(pcl_ros::FilterConfig & config, uint32_t level);
 
 private:
   /** \brief The PCL filter implementation used. */
@@ -94,6 +86,8 @@ private:
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit PassThrough(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros
 

pcl_ros/include/pcl_ros/filters/project_inliers.hpp

@@ -38,8 +38,10 @@
 #ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
 #define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_
 
+// PCL includes
 #include <pcl/filters/project_inliers.h>
 #include <message_filters/subscriber.h>
+#include <memory>
 #include "pcl_ros/filters/filter.hpp"
 
 
@@ -55,8 +57,7 @@ namespace sync_policies = message_filters::sync_policies;
 class ProjectInliers : public Filter
 {
 public:
-  ProjectInliers()
+  explicit ProjectInliers(const rclcpp::NodeOptions & options);
-  : model_() {}
 
 protected:
   /** \brief Call the actual filter.
@@ -66,7 +67,7 @@ protected:
     */
   inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
     pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
@@ -89,25 +90,20 @@ private:
   message_filters::Subscriber<ModelCoefficients> sub_model_;
 
   /** \brief Synchronized input, indices, and model coefficients.*/
-  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
     PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
-  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
     PointIndices, ModelCoefficients>>> sync_input_indices_model_a_;
   /** \brief The PCL filter implementation used. */
   pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
 
-  /** \brief Nodelet initialization routine. */
+  void subscribe() override;
-  virtual void
+  void unsubscribe() override;
-  onInit();
-
-  /** \brief NodeletLazy connection routine. */
-  void subscribe();
-  void unsubscribe();
 
   /** \brief PointCloud2 + Indices + Model data callback. */
   void
   input_indices_model_callback(
-    const PointCloud2::ConstPtr & cloud,
+    const PointCloud2::ConstSharedPtr & cloud,
     const PointIndicesConstPtr & indices,
     const ModelCoefficientsConstPtr & model);
 

pcl_ros/src/pcl_ros/filters/extract_indices.cpp

@@ -56,6 +56,8 @@ pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options)
   if (!result.successful) {
     throw std::runtime_error(result.reason);
   }
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

pcl_ros/src/pcl_ros/filters/filter.cpp

@@ -61,7 +61,9 @@
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 void
-pcl_ros::Filter::computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices)
+pcl_ros::Filter::computePublish(
+  const PointCloud2::ConstSharedPtr & input,
+  const IndicesPtr & indices)
 {
   PointCloud2 output;
   // Call the virtual method in the child
@@ -116,21 +118,30 @@ pcl_ros::Filter::subscribe()
   // If we're supposed to look for PointIndices (indices)
   if (use_indices_) {
     // Subscribe to the input using a filter
-    sub_input_filter_.subscribe(this, "input", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
+    auto sensor_qos_profile = rclcpp::QoS(
-    sub_indices_filter_.subscribe(this, "indices", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
+      rclcpp::KeepLast(max_queue_size_),
+      rmw_qos_profile_sensor_data).get_rmw_qos_profile();
+    sub_input_filter_.subscribe(this, "input", sensor_qos_profile);
+    sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile);
 
     if (approximate_sync_) {
       sync_input_indices_a_ =
         std::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
           pcl_msgs::msg::PointIndices>>>(max_queue_size_);
       sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
-      sync_input_indices_a_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
+      sync_input_indices_a_->registerCallback(
+        std::bind(
+          &Filter::input_indices_callback, this,
+          std::placeholders::_1, std::placeholders::_2));
     } else {
       sync_input_indices_e_ =
         std::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
           pcl_msgs::msg::PointIndices>>>(max_queue_size_);
       sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
-      sync_input_indices_e_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
+      sync_input_indices_e_->registerCallback(
+        std::bind(
+          &Filter::input_indices_callback, this,
+          std::placeholders::_1, std::placeholders::_2));
     }
   } else {
     // Workaround for a callback with custom arguments ros2/rclcpp#766
@@ -162,8 +173,6 @@ pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & optio
 : PCLNode(node_name, options)
 {
   pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
   RCLCPP_DEBUG(this->get_logger(), "Node successfully created.");
 }
 
@@ -174,18 +183,25 @@ pcl_ros::Filter::use_frame_params()
   rcl_interfaces::msg::ParameterDescriptor input_frame_desc;
   input_frame_desc.name = "input_frame";
   input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
-  input_frame_desc.description = "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.";
+  input_frame_desc.description =
+    "The input TF frame the data should be transformed into before processing, "
+    "if input.header.frame_id is different.";
   declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc);
 
   rcl_interfaces::msg::ParameterDescriptor output_frame_desc;
   output_frame_desc.name = "output_frame";
   output_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
-  output_frame_desc.description = "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.";
+  output_frame_desc.description =
+    "The output TF frame the data should be transformed into after processing, "
+    "if input.header.frame_id is different.";
   declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc);
 
   // Validate initial values using same callback
   callback_handle_ =
-    add_on_set_parameters_callback(std::bind(&Filter::config_callback, this, std::placeholders::_1));
+    add_on_set_parameters_callback(
+    std::bind(
+      &Filter::config_callback, this,
+      std::placeholders::_1));
 
   std::vector<std::string> param_names{input_frame_desc.name, output_frame_desc.name};
   auto result = config_callback(get_parameters(param_names));
@@ -194,6 +210,60 @@ pcl_ros::Filter::use_frame_params()
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+std::vector<std::string>
+pcl_ros::Filter::add_common_params()
+{
+  rcl_interfaces::msg::ParameterDescriptor ffn_desc;
+  ffn_desc.name = "filter_field_name";
+  ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
+  ffn_desc.description = "The field name used for filtering";
+  declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor flmin_desc;
+  flmin_desc.name = "filter_limit_min";
+  flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  flmin_desc.description = "The minimum allowed field value a point will be considered from";
+  rcl_interfaces::msg::FloatingPointRange flmin_range;
+  flmin_range.from_value = -100000.0;
+  flmin_range.to_value = 100000.0;
+  flmin_desc.floating_point_range.push_back(flmin_range);
+  declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor flmax_desc;
+  flmax_desc.name = "filter_limit_max";
+  flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  flmax_desc.description = "The maximum allowed field value a point will be considered from";
+  rcl_interfaces::msg::FloatingPointRange flmax_range;
+  flmax_range.from_value = -100000.0;
+  flmax_range.to_value = 100000.0;
+  flmax_desc.floating_point_range.push_back(flmax_range);
+  declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor flneg_desc;
+  flneg_desc.name = "filter_limit_negative";
+  flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  flneg_desc.description =
+    "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max].";
+  declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor keep_organized_desc;
+  keep_organized_desc.name = "keep_organized";
+  keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  keep_organized_desc.description =
+    "Set whether the filtered points should be kept and set to NaN, "
+    "or removed from the PointCloud, thus potentially breaking its organized structure.";
+  declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc);
+
+  return std::vector<std::string> {
+    ffn_desc.name,
+    flmin_desc.name,
+    flmax_desc.name,
+    flneg_desc.name,
+    keep_organized_desc.name
+  };
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 rcl_interfaces::msg::SetParametersResult
 pcl_ros::Filter::config_callback(const std::vector<rclcpp::Parameter> & params)

pcl_ros/src/pcl_ros/filters/passthrough.cpp

@@ -36,95 +36,94 @@
  */
 
 #include "pcl_ros/filters/passthrough.hpp"
-#include <pluginlib/class_list_macros.h>
 
-//////////////////////////////////////////////////////////////////////////////////////////////
+pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options)
-bool
+: Filter("PassThroughNode", options)
-pcl_ros::PassThrough::child_init(ros::NodeHandle & nh, bool & has_service)
 {
-  // Enable the dynamic reconfigure service
+  use_frame_params();
-  has_service = true;
+  std::vector<std::string> param_names = add_common_params();
-  srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(nh);
-  dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
-    &PassThrough::config_callback, this, _1, _2);
-  srv_->setCallback(f);
 
-  return true;
+  callback_handle_ =
+    add_on_set_parameters_callback(
+    std::bind(
+      &PassThrough::config_callback, this,
+      std::placeholders::_1));
+
+  config_callback(get_parameters(param_names));
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-void
+rcl_interfaces::msg::SetParametersResult
-pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
+pcl_ros::PassThrough::config_callback(const std::vector<rclcpp::Parameter> & params)
 {
-  boost::mutex::scoped_lock lock(mutex_);
+  std::lock_guard<std::mutex> lock(mutex_);
 
   double filter_min, filter_max;
   impl_.getFilterLimits(filter_min, filter_max);
 
-  // Check the current values for filter min-max
+  for (const rclcpp::Parameter & param : params) {
-  if (filter_min != config.filter_limit_min) {
+    if (param.get_name() == "filter_field_name") {
-    filter_min = config.filter_limit_min;
+      // Check the current value for the filter field
-    NODELET_DEBUG(
+      if (impl_.getFilterFieldName() != param.as_string()) {
-      "[%s::config_callback] Setting the minimum filtering value a point will be "
+        // Set the filter field if different
-      "considered from to: %f.",
+        impl_.setFilterFieldName(param.as_string());
-      getName().c_str(), filter_min);
+        RCLCPP_DEBUG(
-    // Set the filter min-max if different
+          get_logger(), "Setting the filter field name to: %s.",
-    impl_.setFilterLimits(filter_min, filter_max);
+          param.as_string().c_str());
-  }
+      }
-  // Check the current values for filter min-max
+    }
-  if (filter_max != config.filter_limit_max) {
+    if (param.get_name() == "filter_limit_min") {
-    filter_max = config.filter_limit_max;
+      // Check the current values for filter min-max
-    NODELET_DEBUG(
+      if (filter_min != param.as_double()) {
-      "[%s::config_callback] Setting the maximum filtering value a point will be "
+        filter_min = param.as_double();
-      "considered from to: %f.",
+        RCLCPP_DEBUG(
-      getName().c_str(), filter_max);
+          get_logger(),
-    // Set the filter min-max if different
+          "Setting the minimum filtering value a point will be considered from to: %f.",
-    impl_.setFilterLimits(filter_min, filter_max);
+          filter_min);
-  }
+        // Set the filter min-max if different
-
+        impl_.setFilterLimits(filter_min, filter_max);
-  // Check the current value for the filter field
+      }
-  // std::string filter_field = impl_.getFilterFieldName ();
+    }
-  if (impl_.getFilterFieldName() != config.filter_field_name) {
+    if (param.get_name() == "filter_limit_max") {
-    // Set the filter field if different
+      // Check the current values for filter min-max
-    impl_.setFilterFieldName(config.filter_field_name);
+      if (filter_max != param.as_double()) {
-    NODELET_DEBUG(
+        filter_max = param.as_double();
-      "[%s::config_callback] Setting the filter field name to: %s.",
+        RCLCPP_DEBUG(
-      getName().c_str(), config.filter_field_name.c_str());
+          get_logger(),
-  }
+          "Setting the maximum filtering value a point will be considered from to: %f.",
-
+          filter_max);
-  // Check the current value for keep_organized
+        // Set the filter min-max if different
-  if (impl_.getKeepOrganized() != config.keep_organized) {
+        impl_.setFilterLimits(filter_min, filter_max);
-    NODELET_DEBUG(
+      }
-      "[%s::config_callback] Setting the filter keep_organized value to: %s.",
+    }
-      getName().c_str(), config.keep_organized ? "true" : "false");
+    if (param.get_name() == "filter_limit_negative") {
-    // Call the virtual method in the child
+      // Check the current value for the negative flag
-    impl_.setKeepOrganized(config.keep_organized);
+      if (impl_.getNegative() != param.as_bool()) {
-  }
+        RCLCPP_DEBUG(
-
+          get_logger(), "Setting the filter negative flag to: %s.",
-  // Check the current value for the negative flag
+          param.as_bool() ? "true" : "false");
-  if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) {
+        // Call the virtual method in the child
-    NODELET_DEBUG(
+        impl_.setNegative(param.as_bool());
-      "[%s::config_callback] Setting the filter negative flag to: %s.",
+      }
-      getName().c_str(), config.filter_limit_negative ? "true" : "false");
+    }
-    // Call the virtual method in the child
+    if (param.get_name() == "keep_organized") {
-    impl_.setFilterLimitsNegative(config.filter_limit_negative);
+      // Check the current value for keep_organized
-  }
+      if (impl_.getKeepOrganized() != param.as_bool()) {
-
+        RCLCPP_DEBUG(
-  // The following parameters are updated automatically for all PCL_ROS Nodelet Filters
+          get_logger(), "Setting the filter keep_organized value to: %s.",
-  // as they are inexistent in PCL
+          param.as_bool() ? "true" : "false");
-  if (tf_input_frame_ != config.input_frame) {
+        // Call the virtual method in the child
-    tf_input_frame_ = config.input_frame;
+        impl_.setKeepOrganized(param.as_bool());
-    NODELET_DEBUG(
+      }
-      "[%s::config_callback] Setting the input TF frame to: %s.",
+    }
-      getName().c_str(), tf_input_frame_.c_str());
-  }
-  if (tf_output_frame_ != config.output_frame) {
-    tf_output_frame_ = config.output_frame;
-    NODELET_DEBUG(
-      "[%s::config_callback] Setting the output TF frame to: %s.",
-      getName().c_str(), tf_output_frame_.c_str());
   }
+  // TODO(sloretz) constraint validation
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  return result;
 }
 
-typedef pcl_ros::PassThrough PassThrough;
+#include "rclcpp_components/register_node_macro.hpp"
-PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet);
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PassThrough)

pcl_ros/src/pcl_ros/filters/project_inliers.cpp

@@ -36,46 +36,39 @@
  */
 
 #include "pcl_ros/filters/project_inliers.hpp"
-#include <pluginlib/class_list_macros.h>
+#include <pcl/io/io.h>
-#include <pcl/common/io.h>
-#include <vector>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
-void
+pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options)
-pcl_ros::ProjectInliers::onInit()
+: Filter("ProjectInliersNode", options), model_()
 {
-  PCLNodelet::onInit();
-
   // ---[ Mandatory parameters
   // The type of model to use (user given parameter).
+  declare_parameter("model_type", rclcpp::ParameterType::PARAMETER_INTEGER);
   int model_type;
-  if (!pnh_->getParam("model_type", model_type)) {
+  if (!get_parameter("model_type", model_type)) {
-    NODELET_ERROR(
+    RCLCPP_ERROR(
-      "[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
+      get_logger(),
-      getName().c_str());
+      "[onConstruct] Need a 'model_type' parameter to be set before continuing!");
     return;
   }
   // ---[ Optional parameters
   // True if all data will be returned, false if only the projected inliers. Default: false.
-  bool copy_all_data = false;
+  declare_parameter("copy_all_data", rclcpp::ParameterValue(false));
+  bool copy_all_data = get_parameter("copy_all_data").as_bool();
 
   // True if all fields will be returned, false if only XYZ. Default: true.
-  bool copy_all_fields = true;
+  declare_parameter("copy_all_fields", rclcpp::ParameterValue(true));
+  bool copy_all_fields = get_parameter("copy_all_fields").as_bool();
 
-  pnh_->getParam("copy_all_data", copy_all_data);
+  pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
-  pnh_->getParam("copy_all_fields", copy_all_fields);
 
-  pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
+  RCLCPP_DEBUG(
-
+    this->get_logger(),
-  // Subscribe to the input using a filter
+    "[onConstruct] Node successfully created with the following parameters:\n"
-  sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
+    "  - model_type      : %d\n"
-
+    "  - copy_all_data   : %s\n"
-  NODELET_DEBUG(
+    "  - copy_all_fields : %s",
-    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
-    " - model_type      : %d\n"
-    " - copy_all_data   : %s\n"
-    " - copy_all_fields : %s",
-    getName().c_str(),
     model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
 
   // Set given parameters here
@@ -83,42 +76,50 @@ pcl_ros::ProjectInliers::onInit()
   impl_.setCopyAllFields(copy_all_fields);
   impl_.setCopyAllData(copy_all_data);
 
-  onInitPostProcess();
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::ProjectInliers::subscribe()
 {
+  RCLCPP_DEBUG(get_logger(), "subscribe");
 /*
   TODO : implement use_indices_
   if (use_indices_)
   {*/
 
-  sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
+  auto qos_profile = rclcpp::QoS(
-
+    rclcpp::KeepLast(max_queue_size_),
-  sub_model_.subscribe(*pnh_, "model", max_queue_size_);
+    rmw_qos_profile_default).get_rmw_qos_profile();
+  auto sensor_qos_profile = rclcpp::QoS(
+    rclcpp::KeepLast(max_queue_size_),
+    rmw_qos_profile_sensor_data).get_rmw_qos_profile();
+  sub_input_filter_.subscribe(this, "input", sensor_qos_profile);
+  sub_indices_filter_.subscribe(this, "indices", qos_profile);
+  sub_model_.subscribe(this, "model", qos_profile);
 
   if (approximate_sync_) {
-    sync_input_indices_model_a_ =
+    sync_input_indices_model_a_ = std::make_shared<
-      boost::make_shared<message_filters::Synchronizer<
+      message_filters::Synchronizer<
-          message_filters::sync_policies::ApproximateTime<
+        message_filters::sync_policies::ApproximateTime<
-            PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
+          PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
     sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
     sync_input_indices_model_a_->registerCallback(
-      bind(
+      std::bind(
-        &ProjectInliers::input_indices_model_callback,
+        &ProjectInliers::input_indices_model_callback, this,
-        this, _1, _2, _3));
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
   } else {
-    sync_input_indices_model_e_ =
+    sync_input_indices_model_e_ = std::make_shared<
-      boost::make_shared<message_filters::Synchronizer<
+      message_filters::Synchronizer<
-          message_filters::sync_policies::ExactTime<
+        message_filters::sync_policies::ExactTime<
-            PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
+          PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
     sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
     sync_input_indices_model_e_->registerCallback(
-      bind(
+      std::bind(
-        &ProjectInliers::input_indices_model_callback,
+        &ProjectInliers::input_indices_model_callback, this,
-        this, _1, _2, _3));
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
   }
 }
 
@@ -130,42 +131,39 @@ pcl_ros::ProjectInliers::unsubscribe()
   TODO : implement use_indices_
   if (use_indices_)
   {*/
-
   sub_input_filter_.unsubscribe();
+  sub_indices_filter_.unsubscribe();
   sub_model_.unsubscribe();
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::ProjectInliers::input_indices_model_callback(
-  const PointCloud2::ConstPtr & cloud,
+  const PointCloud2::ConstSharedPtr & cloud,
   const PointIndicesConstPtr & indices,
   const ModelCoefficientsConstPtr & model)
 {
-  if (pub_output_.getNumSubscribers() <= 0) {
+  if (pub_output_->get_subscription_count() == 0) {
     return;
   }
 
   if (!isValid(model) || !isValid(indices) || !isValid(cloud)) {
-    NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str());
+    RCLCPP_ERROR(
+      this->get_logger(), "[%s::input_indices_model_callback] Invalid input!", this->get_name());
     return;
   }
 
-  NODELET_DEBUG(
+  RCLCPP_DEBUG(
+    this->get_logger(),
     "[%s::input_indices_model_callback]\n"
-    "                                 - PointCloud with %d data points (%s), stamp %f, and "
+    "  - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n"
-    "frame %s on topic %s received.\n"
+    "  - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.\n"
-    "                                 - PointIndices with %zu values, stamp %f, and "
+    "  - ModelCoefficients with %zu values, stamp %d.%09d, and frame %s on topic %s received.",
-    "frame %s on topic %s received.\n"
+    this->get_name(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
-    "                                 - ModelCoefficients with %zu values, stamp %f, and "
+    cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input",
-    "frame %s on topic %s received.",
+    indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
-    getName().c_str(),
+    indices->header.frame_id.c_str(), "inliers", model->values.size(),
-    cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+    model->header.stamp.sec, model->header.stamp.nanosec, model->header.frame_id.c_str(), "model");
-    cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
-    indices->indices.size(), indices->header.stamp.toSec(),
-    indices->header.frame_id.c_str(), pnh_->resolveName("inliers").c_str(),
-    model->values.size(), model->header.stamp.toSec(),
-    model->header.frame_id.c_str(), pnh_->resolveName("model").c_str());
 
   tf_input_orig_frame_ = cloud->header.frame_id;
 
@@ -178,5 +176,5 @@ pcl_ros::ProjectInliers::input_indices_model_callback(
   computePublish(cloud, vindices);
 }
 
-typedef pcl_ros::ProjectInliers ProjectInliers;
+#include "rclcpp_components/register_node_macro.hpp"
-PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet);
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ProjectInliers)

pcl_ros/tests/filters/CMakeLists.txt

@@ -21,15 +21,44 @@ OUTPUT
 "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters"
 CONTENT "${components}")
 
-ament_add_pytest_test(test_filter_extract_indices_node_component
+# test components
+ament_add_pytest_test(test_pcl_ros::ExtractIndices
   test_filter_component.py
   ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
       FILTER_PLUGIN=pcl_ros::ExtractIndices
   APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
 )
+ament_add_pytest_test(test_pcl_ros::PassThrough
+  test_filter_component.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_PLUGIN=pcl_ros::PassThrough
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)
+ament_add_pytest_test(test_pcl_ros::ProjectInliers
+  test_filter_component.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_PLUGIN=pcl_ros::ProjectInliers
+      PARAMETERS={'model_type':0}
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)
+
+# test executables
 ament_add_pytest_test(test_filter_extract_indices_node
   test_filter_executable.py
   ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
       FILTER_EXECUTABLE=filter_extract_indices_node
   APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
 )
+ament_add_pytest_test(test_filter_passthrough_node
+  test_filter_executable.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_EXECUTABLE=filter_passthrough_node
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)
+ament_add_pytest_test(test_filter_project_inliers_node
+  test_filter_executable.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_EXECUTABLE=filter_project_inliers_node
+      PARAMETERS={'model_type':0}
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)

pcl_ros/tests/filters/test_filter_component.py

@@ -27,6 +27,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
+import ast
 import os
 import unittest
 
@@ -46,14 +47,8 @@ from sensor_msgs.msg import PointCloud2
 def generate_test_description():
     dummy_plugin = os.getenv('DUMMY_PLUGIN')
     filter_plugin = os.getenv('FILTER_PLUGIN')
-    use_indices = os.getenv('USE_INDICES')
+    parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {}
-    approximate_sync = os.getenv('APPROXIMATE_SYNC')
 
-    parameters = {}
-    if use_indices:
-        parameters['use_indices'] = (use_indices == 'true')
-    if approximate_sync:
-        parameters['approximate_sync'] = (approximate_sync == 'true')
     print(parameters)
 
     return launch.LaunchDescription([

pcl_ros/tests/filters/test_filter_executable.py

@@ -1,3 +1,33 @@
+#
+# Copyright (c) 2022  Carnegie Mellon University
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of Carnegie Mellon University nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+import ast
 import os
 import unittest
 
@@ -17,14 +47,11 @@ from sensor_msgs.msg import PointCloud2
 def generate_test_description():
     dummy_plugin = os.getenv('DUMMY_PLUGIN')
     filter_executable = os.getenv('FILTER_EXECUTABLE')
-    use_indices = os.getenv('USE_INDICES')
+    parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {}
-    approximate_sync = os.getenv('APPROXIMATE_SYNC')
 
     ros_arguments = ["-r", "input:=point_cloud2"]
-    if use_indices:
+    for key in parameters.keys():
-        ros_arguments.extend(["-p", "use_indices:={}".format(use_indices)])
+        ros_arguments.extend(["-p", "{}:={}".format(key, parameters[key])])
-    if approximate_sync:
-        ros_arguments.extend(["-p", "approximate_sync:={}".format(approximate_sync)])
 
     return launch.LaunchDescription([