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/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,
-    PointCloud2 & output)
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    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.
-    * \param nh ROS node handle
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
+  /** \brief Parameter callback
+    * \param params parameter values to set
     */
-  bool
-  child_init(ros::NodeHandle & nh, bool & has_service);
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure service callback.
-    * \param config the dynamic reconfigure FilterConfig object
-    * \param level the dynamic reconfigure level
-    */
-  void
-  config_callback(pcl_ros::FilterConfig & config, uint32_t level);
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
 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()
-  : model_() {}
+  explicit ProjectInliers(const rclcpp::NodeOptions & options);
 
 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. */
-  virtual void
-  onInit();
-
-  /** \brief NodeletLazy connection routine. */
-  void subscribe();
-  void unsubscribe();
+  void subscribe() override;
+  void unsubscribe() override;
 
   /** \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);