Migrate the ROS1 pcl::VoxelGrid filter to ROS2 pcl_ros::VoxelGrid (#398)

* Add the VoxelGrid filter (squash commit)

* Revert the change that brings in separate leaf sizes

pcl_ros/include/pcl_ros/filters/voxel_grid.hpp

@@ -40,11 +40,9 @@
 
 // PCL includes
 #include <pcl/filters/voxel_grid.h>
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
 
-// Dynamic reconfigure
-#include "pcl_ros/VoxelGridConfig.hpp"
-
 namespace pcl_ros
 {
 /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
@@ -53,38 +51,42 @@ namespace pcl_ros
 class VoxelGrid : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig>> srv_;
-
-  /** \brief The PCL filter implementation used. */
-  pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
-
   /** \brief Call the actual filter.
     * \param input the input point cloud dataset
     * \param indices the input set of indices to use from \a input
     * \param output the resultant filtered dataset
     */
-  virtual void
+  inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
-    PointCloud2 & output);
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override
+  {
+    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);
+    impl_.setIndices(indices);
+    pcl::PCLPointCloud2 pcl_output;
+    impl_.filter(pcl_output);
+    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 callback
-    * \param config the config object
-    * \param level the dynamic reconfigure level
-    */
-  void
-  config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level);
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+private:
+  /** \brief The PCL filter implementation used. */
+  pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit VoxelGrid(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros