Add 'ros2_ws/src/perception_pcl/' from commit '6642b7398a4488040c44d48a48308411bcde2228'

git-subtree-dir: ros2_ws/src/perception_pcl
git-subtree-mainline: 0c9504b3439e4fea47710a74d1e2ccde5ad6e19d
git-subtree-split: 6642b7398a4488040c44d48a48308411bcde2228

ros2_ws/src/perception_pcl/.gitignore

@@ -0,0 +1 @@
+*.pyc

ros2_ws/src/perception_pcl/.travis.yml

@@ -0,0 +1,27 @@
+language: generic
+
+services:
+  - docker
+
+cache:
+  directories:
+    - $HOME/.ccache
+
+env:
+  global:
+    - ROS_REPO=ros2
+    - CCACHE_DIR=$HOME/.ccache
+    # travis build will time out with no output unless we use verbose output
+    - VERBOSE_OUTPUT=true
+    - VERBOSE_TESTS=true
+  matrix:
+    - ROS_DISTRO=foxy OS_NAME=ubuntu OS_CODE_NAME=focal
+
+install:
+  - git clone --branch master --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci
+script:
+  - .industrial_ci/travis.sh
+
+branches:
+  only:
+    - /.*-devel$/

ros2_ws/src/perception_pcl/README.md

@@ -0,0 +1,7 @@
+# perception_pcl
+
+[![Build Status](https://travis-ci.org/ros-perception/perception_pcl.svg?branch=melodic-devel)](https://travis-ci.org/ros-perception/perception_pcl)
+
+PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred
+bridge for 3D applications involving n-D Point Clouds and 3D geometry
+processing in ROS.

ros2_ws/src/perception_pcl/pcl_conversions/CHANGELOG.rst

@@ -0,0 +1,68 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package pcl_conversions
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.6.2 (2018-05-20)
+------------------
+
+1.6.1 (2018-05-08)
+------------------
+* Add 1.6.0 section to CHANGELOG.rst
+* Use foreach + string regex to implement list(filter on old cmake
+* Downgrade the required cmake version for backward compatibility
+* update package.xml links to point to new repository
+* CMake 3.6.3 is sufficient
+* Fix a bug building on artful.
+* Fixup pcl_conversions test
+* Contributors: Chris Lalancette, Kentaro Wada, Mikael Arguedas, Paul Bovbel
+
+1.6.0 (2018-04-30)
+------------------
+
+* Fix build and update maintainers
+* Contributors: Paul Bovbel, Kentaro Wada
+
+0.2.1 (2015-06-08)
+------------------
+* Added a test for rounding errors in stamp conversion
+  for some values the test fails.
+* add pcl::PointCloud to Image msg converter for extracting the rgb component of a cloud
+* Contributors: Brice Rebsamen, Lucid One, Michael Ferguson, Paul Bovbel
+
+0.2.0 (2014-04-10)
+------------------
+* Added conversions for stamp types
+* update maintainer info, add eigen dependency
+* fix Eigen dependency
+* Make pcl_conversions run_depend on libpcl-all-dev
+* Contributors: Brice Rebsamen, Paul Bovbel, Scott K Logan, William Woodall
+
+0.1.5 (2013-08-27)
+------------------
+* Use new pcl rosdep keys (libpcl-all and libpcl-all-dev)
+
+0.1.4 (2013-07-13)
+------------------
+* Fixup dependencies and CMakeLists.txt:
+
+  * Added a versioned dependency on pcl, fixes `#1 <https://github.com/ros-perception/pcl_conversions/issues/1>`_
+  * Added a dependency on pcl_msgs, fixes `#2 <https://github.com/ros-perception/pcl_conversions/issues/2>`_
+  * Wrapped the test target in a CATKIN_ENABLE_TESTING check
+
+0.1.3 (2013-07-13)
+------------------
+* Add missing dependency on roscpp
+* Fixup tests and pcl usage in CMakeList.txt
+
+0.1.2 (2013-07-12)
+------------------
+* small fix for conversion functions
+
+0.1.1 (2013-07-10)
+------------------
+* Fix find_package bug with pcl
+
+0.1.0 (2013-07-09 21:49:26 -0700)
+---------------------------------
+- Initial release
+- This package is designed to allow users to more easily convert between pcl-1.7+ types and ROS message types

ros2_ws/src/perception_pcl/pcl_conversions/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(pcl_conversions)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(PCL REQUIRED QUIET COMPONENTS common io)
+find_package(pcl_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+set(dependencies
+  message_filters
+  pcl_msgs
+  rclcpp
+  sensor_msgs
+  std_msgs
+)
+
+include_directories(
+  include
+  ${PCL_COMMON_INCLUDE_DIRS}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+# Add gtest based cpp test target
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp)
+  ament_target_dependencies(${PROJECT_NAME}-test
+    ${dependencies}
+  )
+  target_link_libraries(${PROJECT_NAME}-test ${Boost_LIBRARIES} ${PCL_LIBRARIES})
+endif()
+
+# Export old-style CMake variables
+ament_export_include_directories("include/${PROJECT_NAME}")
+
+ament_export_dependencies(${dependencies} PCL)
+ament_package()

ros2_ws/src/perception_pcl/pcl_conversions/README.rst

@@ -0,0 +1,22 @@
+pcl_conversions
+===============
+
+This package provides conversions from PCL data types and ROS message types.
+
+
+Code & tickets
+--------------
+
+.. Build status: |Build Status|
+
+.. .. |Build Status| image:: https://secure.travis-ci.org/ros-perception/pcl_conversions.png
+   :target: http://travis-ci.org/ros-perception/pcl_conversions
+
++-----------------+------------------------------------------------------------+
+| pcl_conversions | http://ros.org/wiki/pcl_conversions                        |
++-----------------+------------------------------------------------------------+
+| Issues          | http://github.com/ros-perception/perception_pcl/issues    |
++-----------------+------------------------------------------------------------+
+.. | Documentation   | http://ros-perception.github.com/pcl_conversions/doc       |
+.. +-----------------+------------------------------------------------------------+
+

ros2_ws/src/perception_pcl/pcl_conversions/include/pcl_conversions/pcl_conversions.h

@@ -0,0 +1,942 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, Open Source Robotics Foundation, Inc.
+ * Copyright (c) 2010-2012, Willow Garage, Inc.
+ * 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 Open Source Robotics Foundation, Inc. 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.
+ */
+
+#ifndef PCL_CONVERSIONS_H__
+#define PCL_CONVERSIONS_H__
+
+#include <cstddef>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <message_filters/message_event.h>
+#include <message_filters/message_traits.h>
+
+#include <pcl/conversions.h>
+
+#include <pcl/PCLHeader.h>
+#include <std_msgs/msg/header.hpp>
+
+#include <pcl/PCLImage.h>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <pcl/PCLPointField.h>
+#include <sensor_msgs/msg/point_field.hpp>
+
+#include <pcl/PCLPointCloud2.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/PointIndices.h>
+#include <pcl_msgs/msg/point_indices.hpp>
+
+#include <pcl/ModelCoefficients.h>
+#include <pcl_msgs/msg/model_coefficients.hpp>
+
+#include <pcl/Vertices.h>
+#include <pcl_msgs/msg/vertices.hpp>
+
+#include <pcl/PolygonMesh.h>
+#include <pcl_msgs/msg/polygon_mesh.hpp>
+
+#include <pcl/io/pcd_io.h>
+
+#include <Eigen/StdVector>
+#include <Eigen/Geometry>
+
+namespace pcl_conversions {
+
+  /** PCLHeader <=> Header **/
+
+  inline
+  void fromPCL(const std::uint64_t &pcl_stamp, rclcpp::Time &stamp)
+  {
+    stamp = rclcpp::Time(
+      static_cast<rcl_time_point_value_t>(pcl_stamp * 1000ull));  // Convert from us to ns
+  }
+
+  inline
+  void toPCL(const rclcpp::Time &stamp, std::uint64_t &pcl_stamp)
+  {
+    pcl_stamp = static_cast<std::uint64_t>(stamp.nanoseconds()) / 1000ull;  // Convert from ns to us
+  }
+
+  inline
+  rclcpp::Time fromPCL(const std::uint64_t &pcl_stamp)
+  {
+    rclcpp::Time stamp;
+    fromPCL(pcl_stamp, stamp);
+    return stamp;
+  }
+
+  inline
+  std::uint64_t toPCL(const rclcpp::Time &stamp)
+  {
+    std::uint64_t pcl_stamp;
+    toPCL(stamp, pcl_stamp);
+    return pcl_stamp;
+  }
+
+  /** PCLHeader <=> Header **/
+
+  inline
+  void fromPCL(const pcl::PCLHeader &pcl_header, std_msgs::msg::Header &header)
+  {
+    header.stamp = fromPCL(pcl_header.stamp);
+    header.frame_id = pcl_header.frame_id;
+  }
+
+  inline
+  void toPCL(const std_msgs::msg::Header &header, pcl::PCLHeader &pcl_header)
+  {
+    toPCL(header.stamp, pcl_header.stamp);
+    // TODO(clalancette): Seq doesn't exist in the ROS2 header
+    // anymore.  wjwwood suggests that we might be able to get this
+    // information from the middleware in the future, but for now we
+    // just set it to 0.
+    pcl_header.seq = 0;
+    pcl_header.frame_id = header.frame_id;
+  }
+
+  inline
+  std_msgs::msg::Header fromPCL(const pcl::PCLHeader &pcl_header)
+  {
+    std_msgs::msg::Header header;
+    fromPCL(pcl_header, header);
+    return header;
+  }
+
+  inline
+  pcl::PCLHeader toPCL(const std_msgs::msg::Header &header)
+  {
+    pcl::PCLHeader pcl_header;
+    toPCL(header, pcl_header);
+    return pcl_header;
+  }
+
+  /** PCLImage <=> Image **/
+
+  inline
+  void copyPCLImageMetaData(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image)
+  {
+    fromPCL(pcl_image.header, image.header);
+    image.height = pcl_image.height;
+    image.width = pcl_image.width;
+    image.encoding = pcl_image.encoding;
+    image.is_bigendian = pcl_image.is_bigendian;
+    image.step = pcl_image.step;
+  }
+
+  inline
+  void fromPCL(const pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image)
+  {
+    copyPCLImageMetaData(pcl_image, image);
+    image.data = pcl_image.data;
+  }
+
+  inline
+  void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::msg::Image &image)
+  {
+    copyPCLImageMetaData(pcl_image, image);
+    image.data.swap(pcl_image.data);
+  }
+
+  inline
+  void copyImageMetaData(const sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image)
+  {
+    toPCL(image.header, pcl_image.header);
+    pcl_image.height = image.height;
+    pcl_image.width = image.width;
+    pcl_image.encoding = image.encoding;
+    pcl_image.is_bigendian = image.is_bigendian;
+    pcl_image.step = image.step;
+  }
+
+  inline
+  void toPCL(const sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image)
+  {
+    copyImageMetaData(image, pcl_image);
+    pcl_image.data = image.data;
+  }
+
+  inline
+  void moveToPCL(sensor_msgs::msg::Image &image, pcl::PCLImage &pcl_image)
+  {
+    copyImageMetaData(image, pcl_image);
+    pcl_image.data.swap(image.data);
+  }
+
+  /** PCLPointField <=> PointField **/
+
+  inline
+  void fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::msg::PointField &pf)
+  {
+    pf.name = pcl_pf.name;
+    pf.offset = pcl_pf.offset;
+    pf.datatype = pcl_pf.datatype;
+    pf.count = pcl_pf.count;
+  }
+
+  inline
+  void fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::msg::PointField> &pfs)
+  {
+    pfs.resize(pcl_pfs.size());
+    std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();
+    size_t i = 0;
+    for(; it != pcl_pfs.end(); ++it, ++i) {
+      fromPCL(*(it), pfs[i]);
+    }
+  }
+
+  inline
+  void toPCL(const sensor_msgs::msg::PointField &pf, pcl::PCLPointField &pcl_pf)
+  {
+    pcl_pf.name = pf.name;
+    pcl_pf.offset = pf.offset;
+    pcl_pf.datatype = pf.datatype;
+    pcl_pf.count = pf.count;
+  }
+
+  inline
+  void toPCL(const std::vector<sensor_msgs::msg::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs)
+  {
+    pcl_pfs.resize(pfs.size());
+    std::vector<sensor_msgs::msg::PointField>::const_iterator it = pfs.begin();
+    size_t i = 0;
+    for(; it != pfs.end(); ++it, ++i) {
+      toPCL(*(it), pcl_pfs[i]);
+    }
+  }
+
+  /** PCLPointCloud2 <=> PointCloud2 **/
+
+  inline
+  void copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2)
+  {
+    fromPCL(pcl_pc2.header, pc2.header);
+    pc2.height = pcl_pc2.height;
+    pc2.width = pcl_pc2.width;
+    fromPCL(pcl_pc2.fields, pc2.fields);
+    pc2.is_bigendian = pcl_pc2.is_bigendian;
+    pc2.point_step = pcl_pc2.point_step;
+    pc2.row_step = pcl_pc2.row_step;
+    pc2.is_dense = pcl_pc2.is_dense;
+  }
+
+  inline
+  void fromPCL(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2)
+  {
+    copyPCLPointCloud2MetaData(pcl_pc2, pc2);
+    pc2.data = pcl_pc2.data;
+  }
+
+  inline
+  void moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::msg::PointCloud2 &pc2)
+  {
+    copyPCLPointCloud2MetaData(pcl_pc2, pc2);
+    pc2.data.swap(pcl_pc2.data);
+  }
+
+  inline
+  void copyPointCloud2MetaData(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
+  {
+    toPCL(pc2.header, pcl_pc2.header);
+    pcl_pc2.height = pc2.height;
+    pcl_pc2.width = pc2.width;
+    toPCL(pc2.fields, pcl_pc2.fields);
+    pcl_pc2.is_bigendian = pc2.is_bigendian;
+    pcl_pc2.point_step = pc2.point_step;
+    pcl_pc2.row_step = pc2.row_step;
+    pcl_pc2.is_dense = pc2.is_dense;
+  }
+
+  inline
+  void toPCL(const sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
+  {
+    copyPointCloud2MetaData(pc2, pcl_pc2);
+    pcl_pc2.data = pc2.data;
+  }
+
+  inline
+  void moveToPCL(sensor_msgs::msg::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
+  {
+    copyPointCloud2MetaData(pc2, pcl_pc2);
+    pcl_pc2.data.swap(pc2.data);
+  }
+
+  /** pcl::PointIndices <=> pcl_msgs::PointIndices **/
+
+  inline
+  void fromPCL(const pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi)
+  {
+    fromPCL(pcl_pi.header, pi.header);
+    pi.indices = pcl_pi.indices;
+  }
+
+  inline
+  void moveFromPCL(pcl::PointIndices &pcl_pi, pcl_msgs::msg::PointIndices &pi)
+  {
+    fromPCL(pcl_pi.header, pi.header);
+    pi.indices.swap(pcl_pi.indices);
+  }
+
+  inline
+  void toPCL(const pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi)
+  {
+    toPCL(pi.header, pcl_pi.header);
+    pcl_pi.indices = pi.indices;
+  }
+
+  inline
+  void moveToPCL(pcl_msgs::msg::PointIndices &pi, pcl::PointIndices &pcl_pi)
+  {
+    toPCL(pi.header, pcl_pi.header);
+    pcl_pi.indices.swap(pi.indices);
+  }
+
+  /** pcl::ModelCoefficients <=> pcl_msgs::ModelCoefficients **/
+
+  inline
+  void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc)
+  {
+    fromPCL(pcl_mc.header, mc.header);
+    mc.values = pcl_mc.values;
+  }
+
+  inline
+  void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::msg::ModelCoefficients &mc)
+  {
+    fromPCL(pcl_mc.header, mc.header);
+    mc.values.swap(pcl_mc.values);
+  }
+
+  inline
+  void toPCL(const pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
+  {
+    toPCL(mc.header, pcl_mc.header);
+    pcl_mc.values = mc.values;
+  }
+
+  inline
+  void moveToPCL(pcl_msgs::msg::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
+  {
+    toPCL(mc.header, pcl_mc.header);
+    pcl_mc.values.swap(mc.values);
+  }
+
+  /** pcl::Vertices <=> pcl_msgs::Vertices **/
+
+  namespace internal
+  {
+    template <class T>
+    inline void move(std::vector<T> &a, std::vector<T> &b)
+    {
+      b.swap(a);
+    }
+
+    template <class T1, class T2>
+    inline void move(std::vector<T1> &a, std::vector<T2> &b)
+    {
+      b.assign(a.cbegin(), a.cend());
+    }
+  }
+
+  inline
+  void fromPCL(const pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert)
+  {
+    vert.vertices.assign(pcl_vert.vertices.cbegin(), pcl_vert.vertices.cend());
+  }
+
+  inline
+  void fromPCL(const std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::msg::Vertices> &verts)
+  {
+    verts.resize(pcl_verts.size());
+    std::vector<pcl::Vertices>::const_iterator it = pcl_verts.begin();
+    std::vector<pcl_msgs::msg::Vertices>::iterator jt = verts.begin();
+    for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
+      fromPCL(*(it), *(jt));
+    }
+  }
+
+  inline
+  void moveFromPCL(pcl::Vertices &pcl_vert, pcl_msgs::msg::Vertices &vert)
+  {
+    internal::move(pcl_vert.vertices, vert.vertices);
+  }
+
+  inline
+  void fromPCL(std::vector<pcl::Vertices> &pcl_verts, std::vector<pcl_msgs::msg::Vertices> &verts)
+  {
+    verts.resize(pcl_verts.size());
+    std::vector<pcl::Vertices>::iterator it = pcl_verts.begin();
+    std::vector<pcl_msgs::msg::Vertices>::iterator jt = verts.begin();
+    for (; it != pcl_verts.end() && jt != verts.end(); ++it, ++jt) {
+      moveFromPCL(*(it), *(jt));
+    }
+  }
+
+  inline
+  void toPCL(const pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert)
+  {
+    pcl_vert.vertices.assign(vert.vertices.cbegin(), vert.vertices.cend());
+  }
+
+  inline
+  void toPCL(const std::vector<pcl_msgs::msg::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
+  {
+    pcl_verts.resize(verts.size());
+    std::vector<pcl_msgs::msg::Vertices>::const_iterator it = verts.begin();
+    std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
+    for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
+      toPCL(*(it), *(jt));
+    }
+  }
+
+  inline
+  void moveToPCL(pcl_msgs::msg::Vertices &vert, pcl::Vertices &pcl_vert)
+  {
+    internal::move(vert.vertices, pcl_vert.vertices);
+  }
+
+  inline
+  void moveToPCL(std::vector<pcl_msgs::msg::Vertices> &verts, std::vector<pcl::Vertices> &pcl_verts)
+  {
+    pcl_verts.resize(verts.size());
+    std::vector<pcl_msgs::msg::Vertices>::iterator it = verts.begin();
+    std::vector<pcl::Vertices>::iterator jt = pcl_verts.begin();
+    for (; it != verts.end() && jt != pcl_verts.end(); ++it, ++jt) {
+      moveToPCL(*(it), *(jt));
+    }
+  }
+
+  /** pcl::PolygonMesh <=> pcl_msgs::PolygonMesh **/
+
+  inline
+  void fromPCL(const pcl::PolygonMesh &pcl_mesh, pcl_msgs::msg::PolygonMesh &mesh)
+  {
+    fromPCL(pcl_mesh.header, mesh.header);
+    fromPCL(pcl_mesh.cloud, mesh.cloud);
+    fromPCL(pcl_mesh.polygons, mesh.polygons);
+  }
+
+  inline
+  void moveFromPCL(pcl::PolygonMesh &pcl_mesh, pcl_msgs::msg::PolygonMesh &mesh)
+  {
+    fromPCL(pcl_mesh.header, mesh.header);
+    moveFromPCL(pcl_mesh.cloud, mesh.cloud);
+  }
+
+  inline
+  void toPCL(const pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
+  {
+    toPCL(mesh.header, pcl_mesh.header);
+    toPCL(mesh.cloud, pcl_mesh.cloud);
+    toPCL(mesh.polygons, pcl_mesh.polygons);
+  }
+
+  inline
+  void moveToPCL(pcl_msgs::msg::PolygonMesh &mesh, pcl::PolygonMesh &pcl_mesh)
+  {
+    toPCL(mesh.header, pcl_mesh.header);
+    moveToPCL(mesh.cloud, pcl_mesh.cloud);
+    moveToPCL(mesh.polygons, pcl_mesh.polygons);
+  }
+
+} // namespace pcl_conversions
+
+namespace pcl {
+
+  /** Overload pcl::getFieldIndex **/
+
+  inline int getFieldIndex(const sensor_msgs::msg::PointCloud2 &cloud, const std::string &field_name)
+  {
+    // Get the index we need
+    for (size_t d = 0; d < cloud.fields.size(); ++d) {
+      if (cloud.fields[d].name == field_name) {
+        return (static_cast<int>(d));
+      }
+    }
+    return (-1);
+  }
+
+  /** Overload pcl::getFieldsList **/
+
+  inline std::string getFieldsList(const sensor_msgs::msg::PointCloud2 &cloud)
+  {
+    std::string result;
+    for (size_t i = 0; i < cloud.fields.size () - 1; ++i) {
+      result += cloud.fields[i].name + " ";
+    }
+    result += cloud.fields[cloud.fields.size () - 1].name;
+    return (result);
+  }
+
+  /** Provide pcl::toROSMsg **/
+
+  inline
+  void toROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image)
+  {
+    pcl::PCLPointCloud2 pcl_cloud;
+    pcl_conversions::toPCL(cloud, pcl_cloud);
+    pcl::PCLImage pcl_image;
+    pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
+    pcl_conversions::moveFromPCL(pcl_image, image);
+  }
+
+  inline
+  void moveToROSMsg(sensor_msgs::msg::PointCloud2 &cloud, sensor_msgs::msg::Image &image)
+  {
+    pcl::PCLPointCloud2 pcl_cloud;
+    pcl_conversions::moveToPCL(cloud, pcl_cloud);
+    pcl::PCLImage pcl_image;
+    pcl::toPCLPointCloud2(pcl_cloud, pcl_image);
+    pcl_conversions::moveFromPCL(pcl_image, image);
+  }
+
+  template<typename T> void
+  toROSMsg (const pcl::PointCloud<T> &cloud, sensor_msgs::msg::Image& msg)
+  {
+    // Ease the user's burden on specifying width/height for unorganized datasets
+    if (cloud.width == 0 && cloud.height == 0)
+    {
+      throw std::runtime_error("Needs to be a dense like cloud!!");
+    }
+    else
+    {
+      if (cloud.points.size () != cloud.width * cloud.height)
+        throw std::runtime_error("The width and height do not match the cloud size!");
+      msg.height = cloud.height;
+      msg.width = cloud.width;
+    }
+
+    // sensor_msgs::image_encodings::BGR8;
+    msg.encoding = "bgr8";
+    msg.step = msg.width * sizeof (std::uint8_t) * 3;
+    msg.data.resize (msg.step * msg.height);
+    for (size_t y = 0; y < cloud.height; y++)
+    {
+      for (size_t x = 0; x < cloud.width; x++)
+      {
+        std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]);
+        memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t));
+      }
+    }
+  }
+
+  /** Provide to/fromROSMsg for sensor_msgs::msg::PointCloud2 <=> pcl::PointCloud<T> **/
+
+  template<typename T>
+  void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::msg::PointCloud2 &cloud)
+  {
+    pcl::PCLPointCloud2 pcl_pc2;
+    pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
+    pcl_conversions::moveFromPCL(pcl_pc2, cloud);
+  }
+
+  template<typename T>
+  void fromROSMsg(const sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
+  {
+    pcl::PCLPointCloud2 pcl_pc2;
+#if PCL_VERSION_COMPARE(>=, 1, 13, 1)
+    pcl_conversions::copyPointCloud2MetaData(cloud, pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data
+    pcl::MsgFieldMap field_map;
+    pcl::createMapping<T> (pcl_pc2.fields, field_map);
+    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud, field_map, &cloud.data[0]);
+#else
+    pcl_conversions::toPCL(cloud, pcl_pc2);
+    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
+#endif
+  }
+
+  template<typename T>
+  void moveFromROSMsg(sensor_msgs::msg::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
+  {
+    pcl::PCLPointCloud2 pcl_pc2;
+    pcl_conversions::moveToPCL(cloud, pcl_pc2);
+    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
+  }
+
+  /** Overload pcl::createMapping **/
+
+  template<typename PointT>
+  void createMapping(const std::vector<sensor_msgs::msg::PointField>& msg_fields, MsgFieldMap& field_map)
+  {
+    std::vector<pcl::PCLPointField> pcl_msg_fields;
+    pcl_conversions::toPCL(msg_fields, pcl_msg_fields);
+    return createMapping<PointT>(pcl_msg_fields, field_map);
+  }
+
+  namespace io {
+
+    /** Overload pcl::io::savePCDFile **/
+
+    inline int
+    savePCDFile(const std::string &file_name, const sensor_msgs::msg::PointCloud2 &cloud,
+                const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+                const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
+                const bool binary_mode = false)
+    {
+      pcl::PCLPointCloud2 pcl_cloud;
+      pcl_conversions::toPCL(cloud, pcl_cloud);
+      return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
+    }
+
+    inline int
+    destructiveSavePCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud,
+                           const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+                           const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (),
+                           const bool binary_mode = false)
+    {
+      pcl::PCLPointCloud2 pcl_cloud;
+      pcl_conversions::moveToPCL(cloud, pcl_cloud);
+      return pcl::io::savePCDFile(file_name, pcl_cloud, origin, orientation, binary_mode);
+    }
+
+    /** Overload pcl::io::loadPCDFile **/
+
+    inline int loadPCDFile(const std::string &file_name, sensor_msgs::msg::PointCloud2 &cloud)
+    {
+      pcl::PCLPointCloud2 pcl_cloud;
+      int ret = pcl::io::loadPCDFile(file_name, pcl_cloud);
+      pcl_conversions::moveFromPCL(pcl_cloud, cloud);
+      return ret;
+    }
+
+  } // namespace io
+
+  /** Overload asdf **/
+
+  inline
+  bool concatenatePointCloud (const sensor_msgs::msg::PointCloud2 &cloud1,
+                              const sensor_msgs::msg::PointCloud2 &cloud2,
+                              sensor_msgs::msg::PointCloud2 &cloud_out)
+  {
+    //if one input cloud has no points, but the other input does, just return the cloud with points
+    if (cloud1.width * cloud1.height == 0 && cloud2.width * cloud2.height > 0)
+    {
+      cloud_out = cloud2;
+      return (true);
+    }
+    else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
+    {
+      cloud_out = cloud1;
+      return (true);
+    }
+
+    bool strip = false;
+    for (size_t i = 0; i < cloud1.fields.size (); ++i)
+      if (cloud1.fields[i].name == "_")
+        strip = true;
+
+    for (size_t i = 0; i < cloud2.fields.size (); ++i)
+      if (cloud2.fields[i].name == "_")
+        strip = true;
+
+    if (!strip && cloud1.fields.size () != cloud2.fields.size ())
+    {
+      PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
+      return (false);
+    }
+
+    // Copy cloud1 into cloud_out
+    cloud_out = cloud1;
+    size_t nrpts = cloud_out.data.size ();
+    // Height = 1 => no more organized
+    cloud_out.width    = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
+    cloud_out.height   = 1;
+    if (!cloud1.is_dense || !cloud2.is_dense)
+      cloud_out.is_dense = false;
+    else
+      cloud_out.is_dense = true;
+
+    // We need to strip the extra padding fields
+    if (strip)
+    {
+      // Get the field sizes for the second cloud
+      std::vector<sensor_msgs::msg::PointField> fields2;
+      std::vector<size_t> fields2_sizes;
+      for (size_t j = 0; j < cloud2.fields.size (); ++j)
+      {
+        if (cloud2.fields[j].name == "_")
+          continue;
+
+        fields2_sizes.push_back(
+          cloud2.fields[j].count *
+          static_cast<size_t>(pcl::getFieldSize(cloud2.fields[j].datatype)));
+        fields2.push_back(cloud2.fields[j]);
+      }
+
+      cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);
+
+      // Copy the second cloud
+      for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
+      {
+        size_t i = 0;
+        for (size_t j = 0; j < fields2.size (); ++j)
+        {
+          if (cloud1.fields[i].name == "_")
+          {
+            ++i;
+            continue;
+          }
+
+          // We're fine with the special RGB vs RGBA use case
+          if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
+              (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
+              (cloud1.fields[i].name == fields2[j].name))
+          {
+            memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]),
+                    reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
+                    fields2_sizes[j]);
+            ++i;  // increment the field size i
+          }
+        }
+      }
+    }
+    else
+    {
+      for (size_t i = 0; i < cloud1.fields.size (); ++i)
+      {
+        // We're fine with the special RGB vs RGBA use case
+        if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
+            (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
+          continue;
+        // Otherwise we need to make sure the names are the same
+        if (cloud1.fields[i].name != cloud2.fields[i].name)
+        {
+          PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
+          return (false);
+        }
+      }
+      cloud_out.data.resize (nrpts + cloud2.data.size ());
+      memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
+    }
+    return (true);
+  }
+
+} // namespace pcl
+
+/* TODO when ROS2 type masquerading is implemented */ 
+/**
+namespace ros
+{
+  template<>
+  struct DefaultMessageCreator<pcl::PCLPointCloud2>
+  {
+    std::shared_ptr<pcl::PCLPointCloud2> operator() ()
+    {
+      std::shared_ptr<pcl::PCLPointCloud2> msg(new pcl::PCLPointCloud2());
+      return msg;
+    }
+  };
+  
+  namespace message_traits
+  {
+    template<>
+    struct MD5Sum<pcl::PCLPointCloud2>
+    {
+      static const char* value() { return MD5Sum<sensor_msgs::msg::PointCloud2>::value(); }
+      static const char* value(const pcl::PCLPointCloud2&) { return value(); }
+
+      static const uint64_t static_value1 = MD5Sum<sensor_msgs::msg::PointCloud2>::static_value1;
+      static const uint64_t static_value2 = MD5Sum<sensor_msgs::msg::PointCloud2>::static_value2;
+
+      // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
+      static_assert(static_value1 == 0x1158d486dd51d683ULL);
+      static_assert(static_value2 == 0xce2f1be655c3c181ULL);
+    };
+
+    template<>
+    struct DataType<pcl::PCLPointCloud2>
+    {
+      static const char* value() { return DataType<sensor_msgs::msg::PointCloud2>::value(); }
+      static const char* value(const pcl::PCLPointCloud2&) { return value(); }
+    };
+
+    template<>
+    struct Definition<pcl::PCLPointCloud2>
+    {
+      static const char* value() { return Definition<sensor_msgs::msg::PointCloud2>::value(); }
+      static const char* value(const pcl::PCLPointCloud2&) { return value(); }
+    };
+
+    template<> struct HasHeader<pcl::PCLPointCloud2> : std::true_type {};
+  } // namespace message_filters::message_traits
+
+  namespace serialization
+  {
+  **/
+    /*
+     * Provide a custom serialization for pcl::PCLPointCloud2
+     */
+    /**
+    template<>
+    struct Serializer<pcl::PCLPointCloud2>
+    {
+      template<typename Stream>
+      inline static void write(Stream& stream, const pcl::PCLPointCloud2& m)
+      {
+        std_msgs::msg::Header header;
+        pcl_conversions::fromPCL(m.header, header);
+        stream.next(header);
+        stream.next(m.height);
+        stream.next(m.width);
+        std::vector<sensor_msgs::msg::PointField> pfs;
+        pcl_conversions::fromPCL(m.fields, pfs);
+        stream.next(pfs);
+        stream.next(m.is_bigendian);
+        stream.next(m.point_step);
+        stream.next(m.row_step);
+        stream.next(m.data);
+        stream.next(m.is_dense);
+      }
+
+      template<typename Stream>
+      inline static void read(Stream& stream, pcl::PCLPointCloud2& m)
+      {
+        std_msgs::msg::Header header;
+        stream.next(header);
+        pcl_conversions::toPCL(header, m.header);
+        stream.next(m.height);
+        stream.next(m.width);
+        std::vector<sensor_msgs::msg::PointField> pfs;
+        stream.next(pfs);
+        pcl_conversions::toPCL(pfs, m.fields);
+        stream.next(m.is_bigendian);
+        stream.next(m.point_step);
+        stream.next(m.row_step);
+        stream.next(m.data);
+        stream.next(m.is_dense);
+      }
+
+      inline static uint32_t serializedLength(const pcl::PCLPointCloud2& m)
+      {
+        uint32_t length = 0;
+
+        std_msgs::msg::Header header;
+        pcl_conversions::fromPCL(m.header, header);
+        length += serializationLength(header);
+        length += 4; // height
+        length += 4; // width
+        std::vector<sensor_msgs::msg::PointField> pfs;
+        pcl_conversions::fromPCL(m.fields, pfs);
+        length += serializationLength(pfs); // fields
+        length += 1; // is_bigendian
+        length += 4; // point_step
+        length += 4; // row_step
+        length += 4; // data's size
+        length += m.data.size() * sizeof(std::uint8_t);
+        length += 1; // is_dense
+
+        return length;
+      }
+    };
+    **/
+    /*
+     * Provide a custom serialization for pcl::PCLPointField
+     */
+    /**
+    template<>
+    struct Serializer<pcl::PCLPointField>
+    {
+      template<typename Stream>
+      inline static void write(Stream& stream, const pcl::PCLPointField& m)
+      {
+        stream.next(m.name);
+        stream.next(m.offset);
+        stream.next(m.datatype);
+        stream.next(m.count);
+      }
+
+      template<typename Stream>
+      inline static void read(Stream& stream, pcl::PCLPointField& m)
+      {
+        stream.next(m.name);
+        stream.next(m.offset);
+        stream.next(m.datatype);
+        stream.next(m.count);
+      }
+
+      inline static uint32_t serializedLength(const pcl::PCLPointField& m)
+      {
+        uint32_t length = 0;
+
+        length += serializationLength(m.name);
+        length += serializationLength(m.offset);
+        length += serializationLength(m.datatype);
+        length += serializationLength(m.count);
+
+        return length;
+      }
+    };
+    **/
+    /*
+     * Provide a custom serialization for pcl::PCLHeader
+     */
+    /**
+    template<>
+    struct Serializer<pcl::PCLHeader>
+    {
+      template<typename Stream>
+      inline static void write(Stream& stream, const pcl::PCLHeader& m)
+      {
+        std_msgs::msg::Header header;
+        pcl_conversions::fromPCL(m, header);
+        stream.next(header);
+      }
+
+      template<typename Stream>
+      inline static void read(Stream& stream, pcl::PCLHeader& m)
+      {
+        std_msgs::msg::Header header;
+        stream.next(header);
+        pcl_conversions::toPCL(header, m);
+      }
+
+      inline static uint32_t serializedLength(const pcl::PCLHeader& m)
+      {
+        uint32_t length = 0;
+
+        std_msgs::msg::Header header;
+        pcl_conversions::fromPCL(m, header);
+        length += serializationLength(header);
+
+        return length;
+      }
+    };
+  } // namespace ros::serialization
+} // namespace ros
+**/
+
+#endif /* PCL_CONVERSIONS_H__ */

ros2_ws/src/perception_pcl/pcl_conversions/package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>pcl_conversions</name>
+  <version>2.4.1</version>
+  <description>Provides conversions from PCL data types and ROS message types</description>
+
+  <author email="william@osrfoundation.org">William Woodall</author>
+  <author email="paul@bovbel.com">Paul Bovbel</author>
+  <author email="bill@neautomation.com">Bill Morris</author>
+  <author email="ankl@kth.se">Andreas Klintberg</author>
+
+  <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
+  <maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+
+  <license>BSD</license>
+
+  <url>http://wiki.ros.org/pcl_conversions</url>
+  <url type="repository">https://github.com/ros-perception/perception_pcl</url>
+  <url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>libpcl-all-dev</build_depend>
+
+  <depend>eigen</depend>
+  <depend>message_filters</depend>
+  <depend>pcl_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>libpcl-common</exec_depend>
+  <exec_depend>libpcl-io</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros2_ws/src/perception_pcl/pcl_conversions/test/test_pcl_conversions.cpp

@@ -0,0 +1,158 @@
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "pcl_conversions/pcl_conversions.h"
+
+namespace {
+
+class PCLConversionTests : public ::testing::Test {
+protected:
+  virtual void SetUp() {
+    pcl_image.header.stamp = 3141592653;
+    pcl_image.header.frame_id = "pcl";
+    pcl_image.height = 1;
+    pcl_image.width = 2;
+    pcl_image.step = 1;
+    pcl_image.is_bigendian = true;
+    pcl_image.encoding = "bgr8";
+    pcl_image.data.resize(2);
+    pcl_image.data[0] = 0x42;
+    pcl_image.data[1] = 0x43;
+
+    pcl_pc2.header.stamp = 3141592653;
+    pcl_pc2.header.frame_id = "pcl";
+    pcl_pc2.height = 1;
+    pcl_pc2.width = 2;
+    pcl_pc2.point_step = 1;
+    pcl_pc2.row_step = 1;
+    pcl_pc2.is_bigendian = true;
+    pcl_pc2.is_dense = true;
+    pcl_pc2.fields.resize(2);
+    pcl_pc2.fields[0].name = "XYZ";
+    pcl_pc2.fields[0].datatype = pcl::PCLPointField::INT8;
+    pcl_pc2.fields[0].count = 3;
+    pcl_pc2.fields[0].offset = 0;
+    pcl_pc2.fields[1].name = "RGB";
+    pcl_pc2.fields[1].datatype = pcl::PCLPointField::INT8;
+    pcl_pc2.fields[1].count = 3;
+    pcl_pc2.fields[1].offset = 8 * 3;
+    pcl_pc2.data.resize(2);
+    pcl_pc2.data[0] = 0x42;
+    pcl_pc2.data[1] = 0x43;
+  }
+
+  pcl::PCLImage pcl_image;
+  sensor_msgs::msg::Image image;
+
+  pcl::PCLPointCloud2 pcl_pc2;
+  sensor_msgs::msg::PointCloud2 pc2;
+};
+
+template<class T>
+void test_image(T &image) {
+  EXPECT_EQ(std::string("pcl"), image.header.frame_id);
+  EXPECT_EQ(1U, image.height);
+  EXPECT_EQ(2U, image.width);
+  EXPECT_EQ(1U, image.step);
+  EXPECT_TRUE(image.is_bigendian);
+  EXPECT_EQ(std::string("bgr8"), image.encoding);
+  EXPECT_EQ(2U, image.data.size());
+  EXPECT_EQ(0x42, image.data[0]);
+  EXPECT_EQ(0x43, image.data[1]);
+}
+
+TEST_F(PCLConversionTests, imageConversion) {
+  pcl_conversions::fromPCL(pcl_image, image);
+  test_image(image);
+  pcl::PCLImage pcl_image2;
+  pcl_conversions::toPCL(image, pcl_image2);
+  test_image(pcl_image2);
+  EXPECT_EQ(pcl_image.header.stamp, pcl_image2.header.stamp);
+}
+
+template<class T>
+void test_pc(T &pc) {
+  EXPECT_EQ(std::string("pcl"), pc.header.frame_id);
+  EXPECT_EQ(1U, pc.height);
+  EXPECT_EQ(2U, pc.width);
+  EXPECT_EQ(1U, pc.point_step);
+  EXPECT_EQ(1U, pc.row_step);
+  EXPECT_TRUE(pc.is_bigendian);
+  EXPECT_TRUE(pc.is_dense);
+  EXPECT_EQ("XYZ", pc.fields[0].name);
+  EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[0].datatype);
+  EXPECT_EQ(3U, pc.fields[0].count);
+  EXPECT_EQ(0U, pc.fields[0].offset);
+  EXPECT_EQ("RGB", pc.fields[1].name);
+  EXPECT_EQ(pcl::PCLPointField::INT8, pc.fields[1].datatype);
+  EXPECT_EQ(3U, pc.fields[1].count);
+  EXPECT_EQ(8U * 3U, pc.fields[1].offset);
+  EXPECT_EQ(2U, pc.data.size());
+  EXPECT_EQ(0x42, pc.data[0]);
+  EXPECT_EQ(0x43, pc.data[1]);
+}
+
+TEST_F(PCLConversionTests, pointcloud2Conversion) {
+  pcl_conversions::fromPCL(pcl_pc2, pc2);
+  test_pc(pc2);
+  pcl::PCLPointCloud2 pcl_pc2_2;
+  pcl_conversions::toPCL(pc2, pcl_pc2_2);
+  test_pc(pcl_pc2_2);
+  EXPECT_EQ(pcl_pc2.header.stamp, pcl_pc2_2.header.stamp);
+}
+
+} // namespace
+
+
+struct StampTestData
+{
+  const rclcpp::Time stamp_;
+  rclcpp::Time stamp2_;
+
+  explicit StampTestData(const rclcpp::Time &stamp)
+    : stamp_(stamp)
+  {
+    std::uint64_t pcl_stamp;
+    pcl_conversions::toPCL(stamp_, pcl_stamp);
+    pcl_conversions::fromPCL(pcl_stamp, stamp2_);
+  }
+};
+
+TEST(PCLConversionStamp, Stamps)
+{
+  {
+    const StampTestData d(rclcpp::Time(1, 1000));
+    EXPECT_TRUE(d.stamp_==d.stamp2_);
+  }
+
+  {
+    const StampTestData d(rclcpp::Time(1, 999999000));
+    EXPECT_TRUE(d.stamp_==d.stamp2_);
+  }
+
+  {
+    const StampTestData d(rclcpp::Time(1, 999000000));
+    EXPECT_TRUE(d.stamp_==d.stamp2_);
+  }
+
+  {
+    const StampTestData d(rclcpp::Time(1423680574, 746000000));
+    EXPECT_TRUE(d.stamp_==d.stamp2_);
+  }
+
+  {
+    const StampTestData d(rclcpp::Time(1423680629, 901000000));
+    EXPECT_TRUE(d.stamp_==d.stamp2_);
+  }
+}
+
+int main(int argc, char **argv) {
+  try {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+  } catch (std::exception &e) {
+    std::cerr << "Unhandled Exception: " << e.what() << std::endl;
+  }
+  return 1;
+}

ros2_ws/src/perception_pcl/pcl_ros/CHANGELOG.rst

@@ -0,0 +1,328 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package pcl_ros
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.6.2 (2018-05-20)
+------------------
+* Fix exported includes in Ubuntu Artful
+* Increase limits on CropBox filter parameters
+* Contributors: James Ward, Jiri Horner
+
+1.6.1 (2018-05-08)
+------------------
+* Add 1.6.0 section to CHANGELOG.rst
+* Fix the use of Eigen3 in cmake
+* Contributors: Kentaro Wada
+
+1.6.0 (2018-04-30)
+------------------
+
+* Fix build and update maintainers
+* Add message_filters to find_package
+* Remove unnecessary dependency on genmsg
+* Contributors: Paul Bovbel, Kentaro Wada
+
+1.5.4 (2018-03-31)
+------------------
+* update to use non deprecated pluginlib macro
+* Fix config path of sample_voxel_grid.launch
+* remove hack now that upstream pcl has been rebuilt
+* Looser hzerror in test for extract_clusters to make it pass on Travis
+* Add sample & test for surface/convex_hull
+* Add sample & test for segmentation/extract_clusters.cpp
+* Add sample & test for io/concatenate_data.cpp
+* Add sample & test for features/normal_3d.cpp
+* Organize samples of pcl_ros/features
+* Add test arg to avoid duplicated testing
+* LazyNodelet for features/*
+* LazyNodelet for filters/ProjectInliers
+* Refactor io/PCDReader and io/PCDWriter as child of PCLNodelet
+* LazyNodelet for io/PointCloudConcatenateFieldsSynchronizer
+* LazyNodelet for io/PointCloudConcatenateDataSynchronizer
+* LazyNodelet for segmentation/SegmentDifferences
+* LazyNodelet for segmentation/SACSegmentationFromNormals
+* LazyNodelet for segmentation/SACSegmentation
+* LazyNodelet for segmentation/ExtractPolygonalPrismData
+* LazyNodelet for segmentation/EuclideanClusterExtraction
+* LazyNodelet for surface/MovingLeastSquares
+* LazyNodelet for surface/ConvexHull2D
+* Add missing COMPONENTS of PCL
+* Inherit NodeletLazy for pipeline with less cpu load
+* Set leaf_size 0.02
+* Install samples
+* Add sample and test for pcl/StatisticalOutlierRemoval
+  Conflicts:
+  pcl_ros/CMakeLists.txt
+* Add sample and test for pcl/VoxelGrid
+  Conflicts:
+  pcl_ros/CMakeLists.txt
+* no need to remove duplicates
+* spourious line change
+* remove now unnecessary build_depend on qtbase5
+* exclude PCL IO libraries exporting Qt flag
+* find only PCL components used instead of all PCL
+* Remove dependency on vtk/libproj-dev (`#145 <https://github.com/ros-perception/perception_pcl/issues/145>`_)
+  * Remove dependency on vtk/libproj-dev
+  These dependencies were introduced in `#124 <https://github.com/ros-perception/perception_pcl/issues/124>`_ to temporarily fix
+  missing / wrong dependencies in upstream vtk. This hack is no longer
+  necessary, since fixed vtk packages have been uploaded to
+  packages.ros.org (see `#124 <https://github.com/ros-perception/perception_pcl/issues/124>`_ and `ros-infrastructure/reprepro-updater#32 <https://github.com/ros-infrastructure/reprepro-updater/issues/32>`_).
+  * Remove vtk hack from CMakeLists.txt
+* Contributors: Kentaro Wada, Mikael Arguedas
+
+1.5.3 (2017-05-03)
+------------------
+* Add dependency on qtbase5-dev for find_package(Qt5Widgets)
+  See https://github.com/ros-perception/perception_pcl/pull/117#issuecomment-298158272 for detail.
+* Contributors: Kentaro Wada
+
+1.5.2 (2017-04-29)
+------------------
+* Find Qt5Widgets to fix -lQt5::Widgets error
+* Contributors: Kentaro Wada
+
+1.5.1 (2017-04-26)
+------------------
+* Add my name as a maintainer
+* Contributors: Kentaro Wada
+
+1.5.0 (2017-04-25)
+------------------
+* Fix lib name duplication error in ubunt:zesty
+* Detect automatically the version of PCL in cmake
+* Install xml files declaring nodelets
+* Fix syntax of nodelet manifest file by splitting files for each library.
+* Contributors: Kentaro Wada
+
+1.4.0 (2016-04-22)
+------------------
+* Fixup libproj-dev rosdep
+* Add build depend on libproj, since it's not provided by vtk right now
+* manually remove dependency on vtkproj from PCL_LIBRARIES
+* Remove python-vtk for kinetic-devel, see issue `#44 <https://github.com/ros-perception/perception_pcl/issues/44>`_
+* Contributors: Jackie Kay, Paul Bovbel
+
+1.3.0 (2015-06-22)
+------------------
+* cleanup broken library links
+  All removed library names are included in ${PCL_LIBRARIES}.
+  However, the plain library names broke catkin's overlay mechanism:
+  Where ${PCL_LIBRARIES} could point to a local installation of the PCL,
+  e.g. pcd_ros_segmentation might still link to the system-wide installed version
+  of pcl_segmentation.
+* Fixed test for jade-devel. Progress on `#92 <https://github.com/ros-perception/perception_pcl/issues/92>`_
+* commented out test_tf_message_filter_pcl
+  Until `ros/geometry#80 <https://github.com/ros/geometry/issues/80>`_ has been merged the test will fail.
+* fixed indentation and author
+* Adds a test for tf message filters with pcl pointclouds
+* specialized HasHeader, TimeStamp, FrameId
+  - HasHeader now returns false
+  - TimeStamp and FrameId specialed for pcl::PointCloud<T> for any point type T
+  These changes allow to use pcl::PointCloud with tf::MessageFilter
+* Sync pcl_nodelets.xml from hydro to indigo
+  Fixes to pass catkin lint -W1
+* Fixes `#87 <https://github.com/ros-perception/perception_pcl/issues/87>`_ for Indigo
+* Fixes `#85 <https://github.com/ros-perception/perception_pcl/issues/85>`_ for Indigo
+* Fixes `#77 <https://github.com/ros-perception/perception_pcl/issues/77>`_ and `#80 <https://github.com/ros-perception/perception_pcl/issues/80>`_ for indigo
+* Added option to save pointclouds in binary and binary compressed format
+* Contributors: Brice Rebsamen, Lucid One, Mitchell Wills, v4hn
+
+1.2.6 (2015-02-04)
+------------------
+
+1.2.5 (2015-01-20)
+------------------
+
+1.2.4 (2015-01-15)
+------------------
+
+1.2.3 (2015-01-10)
+------------------
+* Update common.py
+  Extended filter limits up to ±100000.0 in order to support intensity channel filtering.
+* Contributors: Dani Carbonell
+
+1.2.2 (2014-10-25)
+------------------
+* Adding target_frame
+  [Ability to specify frame in bag_to_pcd ](https://github.com/ros-perception/perception_pcl/issues/55)
+* Update pcl_nodelets.xml
+  Included missing closing library tag.  This was causing the pcl/Filter nodelets below the missing nodelet tag to not be exported correctly.
+* Contributors: Matt Derry, Paul Bovbel, Ruffin
+
+1.2.1 (2014-09-13)
+------------------
+* clean up merge
+* merge pull request `#60 <https://github.com/ros-perception/perception_pcl/issues/60>`_
+* Contributors: Paul Bovbel
+
+1.2.0 (2014-04-09)
+------------------
+* Updated maintainership
+* Fix TF2 support for bag_to_pcd `#46 <https://github.com/ros-perception/perception_pcl/issues/46>`_
+* Use cmake_modules to find eigen on indigo `#45 <https://github.com/ros-perception/perception_pcl/issues/45>`_
+
+1.1.7 (2013-09-20)
+------------------
+* adding more uncaught config dependencies
+* adding FeatureConfig dependency too
+
+1.1.6 (2013-09-20)
+------------------
+* add excplicit dependency on gencfg target
+
+1.1.5 (2013-08-27)
+------------------
+* Updated package.xml's to use new libpcl-all rosdep rules
+* package.xml: tuned whitespaces
+  This commit removes trailing whitespaces and makes the line with the license information in the package.xml bitwise match exactly the common license information line in most ROS packages.
+  The trailing whitespaces were detected when providing a bitbake recipe in the meta-ros project (github.com/bmwcarit/meta-ros). In the recipe, the hash of the license line is declared and is used to check for changes in the license. For this recipe, it was not matching the common one.
+  A related already merged commit is https://github.com/ros/std_msgs/pull/3 and a related pending commit is https://github.com/ros-perception/pcl_msgs/pull/1.
+
+1.1.4 (2013-07-23)
+------------------
+* Fix a serialization error with point_cloud headers
+* Initialize shared pointers before use in part of the pcl_conversions
+  Should address runtime errors reported in `#29 <https://github.com/ros-perception/perception_pcl/issues/29>`_
+* Changed the default bounds on filters to -1000, 1000 from -5, 5 in common.py
+
+1.1.2 (2013-07-19)
+------------------
+* Fixed missing package exports on pcl_conversions and others
+* Make find_package on Eigen and PCL REQUIRED
+
+1.1.1 (2013-07-10)
+------------------
+* Add missing EIGEN define which caused failures on the farm
+
+1.1.0 (2013-07-09)
+------------------
+* Add missing include in one of the installed headers
+* Refactors to use pcl-1.7
+* Use the PointIndices from pcl_msgs
+* Experimental changes to point_cloud.h
+* Fixes from converting from pcl-1.7, incomplete
+* Depend on pcl_conversions and pcl_msgs
+* bag_to_pcd: check return code of transformPointCloud()
+  This fixes a bug where bag_to_pcd segfaults because of an ignored
+  tf::ExtrapolationException.
+* Changed #include type to lib
+* Changed some #include types to lib
+* removed a whitespace
+
+1.0.34 (2013-05-21)
+-------------------
+* fixing catkin python imports
+
+1.0.33 (2013-05-20)
+-------------------
+* Fixing catkin python imports
+
+1.0.32 (2013-05-17)
+-------------------
+* Merge pull request `#11 <https://github.com/ros-perception/perception_pcl/issues/11>`_ from k-okada/groovy-devel
+  revert removed directories
+* fix to compileable
+* copy features/segmentation/surface from fuerte-devel
+
+1.0.31 (2013-04-22 11:58)
+-------------------------
+* No changes
+
+1.0.30 (2013-04-22 11:47)
+-------------------------
+* deprecating bin install targets
+
+1.0.29 (2013-03-04)
+-------------------
+* Fixes `#7 <https://github.com/ros-perception/perception_pcl/issues/7>`_
+* now also works without specifying publishing interval like described in the wiki.
+
+1.0.28 (2013-02-05 12:29)
+-------------------------
+* reenabling deprecated install targets - comment added
+
+1.0.27 (2013-02-05 12:10)
+-------------------------
+* Update pcl_ros/package.xml
+* Fixing target install directory for pcl tools
+* update pluginlib macro
+
+1.0.26 (2013-01-17)
+-------------------
+* fixing catkin export
+
+1.0.25 (2013-01-01)
+-------------------
+* fixes `#1 <https://github.com/ros-perception/perception_pcl/issues/1>`_
+
+1.0.24 (2012-12-21)
+-------------------
+* remove obsolete roslib import
+
+1.0.23 (2012-12-19 16:52)
+-------------------------
+* clean up shared parameters
+
+1.0.22 (2012-12-19 15:22)
+-------------------------
+* fix dyn reconf files
+
+1.0.21 (2012-12-18 17:42)
+-------------------------
+* fixing catkin_package debs
+
+1.0.20 (2012-12-18 14:21)
+-------------------------
+* adding catkin_project dependencies
+
+1.0.19 (2012-12-17 21:47)
+-------------------------
+* adding nodelet_topic_tools dependency
+
+1.0.18 (2012-12-17 21:17)
+-------------------------
+* adding pluginlib dependency
+* adding nodelet dependencies
+* CMake install fixes
+* migrating nodelets and tools from fuerte release to pcl_ros
+* Updated for new <buildtool_depend>catkin<...> catkin rule
+
+1.0.17 (2012-10-26 09:28)
+-------------------------
+* remove useless tags
+
+1.0.16 (2012-10-26 08:53)
+-------------------------
+* no need to depend on a meta-package
+
+1.0.15 (2012-10-24)
+-------------------
+* do not generrate messages automatically
+
+1.0.14 (2012-10-23)
+-------------------
+* bring back the PCL msgs
+
+1.0.13 (2012-10-11 17:46)
+-------------------------
+* install library to the right place
+
+1.0.12 (2012-10-11 17:25)
+-------------------------
+
+1.0.11 (2012-10-10)
+-------------------
+* fix a few dependencies
+
+1.0.10 (2012-10-04)
+-------------------
+* comply to the new catkin API
+* fixed pcl_ros manifest
+* added pcl exports in manifest.xml
+* fixed rosdeb pcl in pcl_ros/manifest.xml
+* removing common_rosdeps from manifest.xml
+* perception_pcl restructuring in groovy branch
+* restructuring perception_pcl in groovy branch
+* catkinized version of perception_pcl for groovy
+* added PCL 1.6 stack for groovy

ros2_ws/src/perception_pcl/pcl_ros/CMakeLists.txt

@@ -0,0 +1,242 @@
+cmake_minimum_required(VERSION 3.5)
+project(pcl_ros)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -fPIC)
+endif()
+
+## Find system dependencies
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED QUIET COMPONENTS common features filters io segmentation surface)
+
+## Find ROS package dependencies
+find_package(ament_cmake REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+set(dependencies
+  pcl_conversions
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  EIGEN3
+  PCL
+)
+
+## Declare the pcl_ros_tf library
+add_library(pcl_ros_tf src/transforms.cpp)
+target_include_directories(pcl_ros_tf PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+ament_target_dependencies(pcl_ros_tf
+  ${dependencies}
+)
+
+### Nodelets
+#
+### Declare the pcl_ros_io library
+#add_library(pcl_ros_io
+#  src/pcl_ros/io/bag_io.cpp
+#  src/pcl_ros/io/concatenate_data.cpp
+#  src/pcl_ros/io/concatenate_fields.cpp
+#  src/pcl_ros/io/io.cpp
+#  src/pcl_ros/io/pcd_io.cpp
+#)
+#target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#class_loader_hide_library_symbols(pcl_ros_io)
+#
+### Declare the pcl_ros_features library
+#add_library(pcl_ros_features
+#  src/pcl_ros/features/feature.cpp
+#  # Compilation is much faster if we include all the following CPP files in feature.cpp
+#  src/pcl_ros/features/boundary.cpp
+#  src/pcl_ros/features/fpfh.cpp
+#  src/pcl_ros/features/fpfh_omp.cpp
+#  src/pcl_ros/features/shot.cpp
+#  src/pcl_ros/features/shot_omp.cpp
+#  src/pcl_ros/features/moment_invariants.cpp
+#  src/pcl_ros/features/normal_3d.cpp
+#  src/pcl_ros/features/normal_3d_omp.cpp
+#  src/pcl_ros/features/pfh.cpp
+#  src/pcl_ros/features/principal_curvatures.cpp
+#  src/pcl_ros/features/vfh.cpp
+#)
+#target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg)
+#class_loader_hide_library_symbols(pcl_ros_features)
+#
+#
+### Declare the pcl_ros_filters library
+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/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
+  src/pcl_ros/filters/crop_box.cpp
+)
+target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
+ament_target_dependencies(pcl_ros_filters ${dependencies})
+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
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::RadiusOutlierRemoval"
+  EXECUTABLE filter_radius_outlier_removal_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::StatisticalOutlierRemoval"
+  EXECUTABLE filter_statistical_outlier_removal_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::CropBox"
+  EXECUTABLE filter_crop_box_node
+)
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::VoxelGrid"
+  EXECUTABLE filter_voxel_grid_node
+)
+class_loader_hide_library_symbols(pcl_ros_filters)
+#
+### Declare the pcl_ros_segmentation library
+#add_library (pcl_ros_segmentation
+#  src/pcl_ros/segmentation/extract_clusters.cpp
+#  src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
+#  src/pcl_ros/segmentation/sac_segmentation.cpp
+#  src/pcl_ros/segmentation/segment_differences.cpp
+#  src/pcl_ros/segmentation/segmentation.cpp
+#)
+#target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg)
+#class_loader_hide_library_symbols(pcl_ros_segmentation)
+#
+### Declare the pcl_ros_surface library
+#add_library (pcl_ros_surface
+#  src/pcl_ros/surface/surface.cpp
+#  # Compilation is much faster if we include all the following CPP files in surface.cpp
+#  src/pcl_ros/surface/convex_hull.cpp
+#  src/pcl_ros/surface/moving_least_squares.cpp
+#)
+#target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg)
+#class_loader_hide_library_symbols(pcl_ros_surface)
+#
+### Tools
+#
+add_library(pcd_to_pointcloud_lib SHARED tools/pcd_to_pointcloud.cpp)
+target_link_libraries(pcd_to_pointcloud_lib
+  ${PCL_LIBRARIES})
+target_include_directories(pcd_to_pointcloud_lib PUBLIC
+  ${PCL_INCLUDE_DIRS})
+ament_target_dependencies(pcd_to_pointcloud_lib
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  pcl_conversions)
+rclcpp_components_register_node(pcd_to_pointcloud_lib
+  PLUGIN "pcl_ros::PCDPublisher"
+  EXECUTABLE pcd_to_pointcloud)
+#
+#add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
+#target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#
+#add_executable(bag_to_pcd tools/bag_to_pcd.cpp)
+#target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#
+#add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp)
+#target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#
+#add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp)
+#target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+#
+### Downloads
+#
+#catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd
+#  DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data
+#  MD5 546b5b4822fb1de21b0cf83d41ad6683
+#)
+#add_custom_target(download ALL DEPENDS download_extra_data)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(tests/filters)
+  #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp)
+  #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+  #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false)
+  #add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false)
+  #add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false)
+  #add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false)
+  #add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false)
+endif()
+
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  TARGETS
+    pcl_ros_tf
+    pcd_to_pointcloud_lib
+#    pcl_ros_io
+#    pcl_ros_features
+    pcl_ros_filters
+#    pcl_ros_surface
+#    pcl_ros_segmentation
+#    pointcloud_to_pcd
+#    bag_to_pcd
+#    convert_pcd_to_image
+#    convert_pointcloud_to_image
+  EXPORT export_pcl_ros
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(DIRECTORY plugins samples
+  DESTINATION share/${PROJECT_NAME})
+
+# Export old-style CMake variables
+ament_export_include_directories("include/${PROJECT_NAME}")
+ament_export_libraries(pcl_ros_tf)
+
+# Export modern CMake targets
+ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET)
+
+ament_export_dependencies(${dependencies})
+
+ament_package()

ros2_ws/src/perception_pcl/pcl_ros/cfg/CropBox.cfg

@@ -0,0 +1,25 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import os
+import sys
+sys.path.insert(0, os.path.dirname(__file__))
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator ()
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("min_x", double_t, 0, "X coordinate of the minimum point of the box.", -1, -1000, 1000)
+gen.add ("max_x", double_t, 0, "X coordinate of the maximum point of the box.",  1, -1000, 1000)
+gen.add ("min_y", double_t, 0, "Y coordinate of the minimum point of the box.", -1, -1000, 1000)
+gen.add ("max_y", double_t, 0, "Y coordinate of the maximum point of the box.",  1, -1000, 1000)
+gen.add ("min_z", double_t, 0, "Z coordinate of the minimum point of the box.", -1, -1000, 1000)
+gen.add ("max_z", double_t, 0, "Z coordinate of the maximum point of the box.",  1, -1000, 1000)
+gen.add ("keep_organized", bool_t, 0, "Set whether the filtered points should be kept and set to NaN, or removed from the PointCloud, thus potentially breaking its organized structure.", False)
+gen.add ("negative", bool_t, 0, "If True the box will be empty Else the remaining points will be the ones in the box", False)
+gen.add ("input_frame", str_t, 0, "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.", "")
+gen.add ("output_frame", str_t, 0, "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different.", "")
+
+exit (gen.generate (PACKAGE, "pcl_ros", "CropBox"))

ros2_ws/src/perception_pcl/pcl_ros/cfg/EuclideanClusterExtraction.cfg

@@ -0,0 +1,17 @@
+#! /usr/bin/env python
+
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# enabling/disabling the unit limits
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("cluster_tolerance", double_t, 0, "The spatial tolerance as a measure in the L2 Euclidean space", 0.05, 0.0, 2.0)
+gen.add ("cluster_min_size", int_t, 0, "The minimum number of points that a cluster must contain in order to be accepted", 1, 0, 1000)
+gen.add ("cluster_max_size", int_t, 0, "The maximum number of points that a cluster must contain in order to be accepted", 2147483647, 0, 2147483647)
+gen.add ("max_clusters", int_t, 0, "The maximum number of clusters to extract.", 2147483647, 1, 2147483647)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "EuclideanClusterExtraction"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractIndices.cfg

@@ -0,0 +1,12 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *;
+
+gen = ParameterGenerator ()
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("negative", bool_t, 0, "Extract indices or the negative (all-indices)", False)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "ExtractIndices"))

ros2_ws/src/perception_pcl/pcl_ros/cfg/ExtractPolygonalPrismData.cfg

@@ -0,0 +1,13 @@
+#! /usr/bin/env python
+
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("height_min", double_t, 0, "The minimum allowed distance to the plane model value a point will be considered from", 0.0, -10.0, 10.0)
+gen.add ("height_max", double_t, 0, "The maximum allowed distance to the plane model value a point will be considered from", 0.5, -10.0, 10.0)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "ExtractPolygonalPrismData"))

ros2_ws/src/perception_pcl/pcl_ros/cfg/Feature.cfg

@@ -0,0 +1,17 @@
+#! /usr/bin/env python
+
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
+gen.add ("radius_search", double_t,  0, "Sphere radius for nearest neighbor search",  0.0, 0.0, 0.5)
+#gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
+#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
+
+exit (gen.generate (PACKAGE, "pcl_ros", "Feature"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/Filter.cfg

@@ -0,0 +1,17 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import os
+import sys
+sys.path.insert(0, os.path.dirname(__file__))
+from common import add_common_parameters
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator ()
+add_common_parameters (gen)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "Filter"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/MLS.cfg

@@ -0,0 +1,20 @@
+#! /usr/bin/env python
+
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+enum = gen.enum ([ gen.const("ANN", int_t, 0, "ANN"), gen.const("FLANN", int_t, 1, "FLANN"), gen.const("organized", int_t, 2, "Dense/organized data locator") ], "Set the spatial locator")
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("spatial_locator", int_t, 0, "Set the spatial locator", 0, 0, 2, enum)
+gen.add ("search_radius", double_t,  0, "Sphere radius for nearest neighbor search",  0.02, 0.0, 0.5)
+#gen.add ("k_search", int_t, 0, "Number of k-nearest neighbors to search for", 10, 0, 1000)
+gen.add ("use_polynomial_fit", bool_t, 0, "Whether to use polynomial fit", True)
+gen.add ("polynomial_order", int_t, 0, "Set the spatial locator", 2, 0, 5)
+gen.add ("gaussian_parameter", double_t,  0, "How 'flat' should the neighbor weighting gaussian be (the smaller, the more local the fit)",  0.02, 0.0, 0.5)
+#gen.add ("spatial_locator", str_t, 0, "Set the spatial locator", "ANN")
+
+exit (gen.generate (PACKAGE, "pcl_ros", "MLS"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/RadiusOutlierRemoval.cfg

@@ -0,0 +1,15 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *;
+
+gen = ParameterGenerator ()
+
+# enabling/disabling the unit limits
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("radius_search", double_t, 0, "Radius of the sphere that will determine which points are neighbors.", 0.1, 0.0, 10.0)
+gen.add ("min_neighbors", int_t, 0, "The number of neighbors that need to be present in order to be classified as an inlier.", 5, 0, 1000)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "RadiusOutlierRemoval"))

ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation.cfg

@@ -0,0 +1,13 @@
+#! /usr/bin/env python
+
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+import SACSegmentation_common as common
+
+gen = ParameterGenerator ()
+common.add_common_parameters (gen)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentation"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentationFromNormals.cfg

@@ -0,0 +1,18 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+import roslib.packages
+import SACSegmentation_common as common
+
+gen = ParameterGenerator()
+
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("normal_distance_weight", double_t, 0, "The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point normals and the plane normal.", 0.1, 0, 1.0)
+common.add_common_parameters (gen)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "SACSegmentationFromNormals"))

ros2_ws/src/perception_pcl/pcl_ros/cfg/SACSegmentation_common.py

@@ -0,0 +1,47 @@
+#! /usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import bool_t
+from dynamic_reconfigure.parameter_generator_catkin import double_t
+from dynamic_reconfigure.parameter_generator_catkin import int_t
+from dynamic_reconfigure.parameter_generator_catkin import str_t
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+
+def add_common_parameters(gen):
+    # add(self, name, paramtype, level, description, default = None, min = None,
+    # max = None, edit_method = '')
+    gen.add('max_iterations', int_t, 0,
+            'The maximum number of iterations the algorithm will run for',
+            50, 0, 100000)
+    gen.add('probability', double_t, 0,
+            'The desired probability of choosing at least one sample free from outliers',
+            0.99, 0.5, 0.99)
+    gen.add('distance_threshold', double_t, 0,
+            'The distance to model threshold',
+            0.02, 0, 1.0)
+    gen.add('optimize_coefficients', bool_t, 0,
+            'Model coefficient refinement',
+            True)
+    gen.add('radius_min', double_t, 0,
+            'The minimum allowed model radius (where applicable)',
+            0.0, 0, 1.0)
+    gen.add('radius_max', double_t, 0,
+            'The maximum allowed model radius (where applicable)',
+            0.05, 0, 1.0)
+    gen.add('eps_angle', double_t, 0,
+            ('The maximum allowed difference between the model normal '
+             'and the given axis in radians.'),
+            0.17, 0.0, 1.5707)
+    gen.add('min_inliers', int_t, 0,
+            'The minimum number of inliers a model must have in order to be considered valid.',
+            0, 0, 100000)
+    gen.add('input_frame', str_t, 0,
+            ('The input TF frame the data should be transformed into, '
+             'if input.header.frame_id is different.'),
+            '')
+    gen.add('output_frame', str_t, 0,
+            ('The output TF frame the data should be transformed into, '
+             'if input.header.frame_id is different.'),
+            '')

ros2_ws/src/perception_pcl/pcl_ros/cfg/SegmentDifferences.cfg

@@ -0,0 +1,16 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE='pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+
+# enabling/disabling the unit limits
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("distance_threshold", double_t, 0, "The distance tolerance as a measure in the L2 Euclidean space between corresponding points.", 0.0, 0.0, 2.0)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "SegmentDifferences"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/StatisticalOutlierRemoval.cfg

@@ -0,0 +1,17 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+from dynamic_reconfigure.parameter_generator_catkin import *;
+
+gen = ParameterGenerator ()
+
+# enabling/disabling the unit limits
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("mean_k", int_t, 0, "The number of points (k) to use for mean distance estimation", 2, 2, 100)
+gen.add ("stddev", double_t, 0, "The standard deviation multiplier threshold. All points outside the mean +- sigma * std_mul will be considered outliers.", 0.0, 0.0, 5.0)
+gen.add ("negative", bool_t, 0, "Set whether the inliers should be returned (true) or the outliers (false)", False)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "StatisticalOutlierRemoval"))
+

ros2_ws/src/perception_pcl/pcl_ros/cfg/VoxelGrid.cfg

@@ -0,0 +1,18 @@
+#! /usr/bin/env python
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+import os
+import sys
+sys.path.insert(0, os.path.dirname(__file__))
+from common import add_common_parameters
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator ()
+# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""):
+gen.add ("leaf_size", double_t, 0, "The size of a leaf (on x,y,z) used for downsampling.", 0.01, 0, 1.0)
+add_common_parameters (gen)
+
+exit (gen.generate (PACKAGE, "pcl_ros", "VoxelGrid"))

ros2_ws/src/perception_pcl/pcl_ros/cfg/common.py

@@ -0,0 +1,36 @@
+#! /usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import bool_t
+from dynamic_reconfigure.parameter_generator_catkin import double_t
+from dynamic_reconfigure.parameter_generator_catkin import str_t
+
+# set up parameters that we care about
+PACKAGE = 'pcl_ros'
+
+
+def add_common_parameters(gen):
+    # def add (self, name, paramtype, level, description, default = None, min = None,
+    # max = None, edit_method = ''):
+    gen.add('filter_field_name', str_t, 0, 'The field name used for filtering', 'z')
+    gen.add('filter_limit_min', double_t, 0,
+            'The minimum allowed field value a point will be considered from',
+            0.0, -100000.0, 100000.0)
+    gen.add('filter_limit_max', double_t, 0,
+            'The maximum allowed field value a point will be considered from',
+            1.0, -100000.0, 100000.0)
+    gen.add('filter_limit_negative', bool_t, 0,
+            ('Set to true if we want to return the data outside '
+             '[filter_limit_min; filter_limit_max].'),
+            False)
+    gen.add('keep_organized', bool_t, 0,
+            ('Set whether the filtered points should be kept and set to NaN, '
+             'or removed from the PointCloud, thus potentially breaking its organized structure.'),
+            False)
+    gen.add('input_frame', str_t, 0,
+            ('The input TF frame the data should be transformed into before processing, '
+             'if input.header.frame_id is different.'),
+            '')
+    gen.add('output_frame', str_t, 0,
+            ('The output TF frame the data should be transformed into after processing, '
+             'if input.header.frame_id is different.'),
+            '')

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/boundary.hpp

@@ -0,0 +1,87 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: boundary.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__BOUNDARY_HPP_
+#define PCL_ROS__FEATURES__BOUNDARY_HPP_
+
+#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
+
+#include <pcl/features/boundary.h>
+#include "pcl_ros/features/feature.hpp"
+
+namespace pcl_ros
+{
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle
+  * criterion. The code makes use of the estimated surface normals at each point in the input dataset.
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
+  * \author Radu Bogdan Rusu
+  */
+class BoundaryEstimation : public FeatureFromNormals
+{
+private:
+  pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> impl_;
+
+  typedef pcl::PointCloud<pcl::Boundary> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__BOUNDARY_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/feature.hpp

@@ -0,0 +1,267 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: feature.h 35422 2011-01-24 20:04:44Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__FEATURE_HPP_
+#define PCL_ROS__FEATURES__FEATURE_HPP_
+
+// PCL includes
+#include <pcl/features/feature.h>
+#include <pcl_msgs/PointIndices.h>
+
+#include <message_filters/pass_through.h>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+
+// PCL conversions
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "pcl_ros/pcl_nodelet.hpp"
+#include "pcl_ros/FeatureConfig.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+///////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b Feature represents the base feature class. Some generic 3D operations that
+  * are applicable to all features are defined here as static methods.
+  * \author Radu Bogdan Rusu
+  */
+class Feature : public PCLNodelet
+{
+public:
+  typedef pcl::KdTree<pcl::PointXYZ> KdTree;
+  typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
+
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn;
+  typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
+  typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
+
+  typedef pcl::IndicesPtr IndicesPtr;
+  typedef pcl::IndicesConstPtr IndicesConstPtr;
+
+  /** \brief Empty constructor. */
+  Feature()
+  : /*input_(), indices_(), surface_(), */ tree_(), k_(0), search_radius_(0),
+    use_surface_(false), spatial_locator_type_(-1)
+  {}
+
+protected:
+  /** \brief The input point cloud dataset. */
+  // PointCloudInConstPtr input_;
+
+  /** \brief A pointer to the vector of point indices to use. */
+  // IndicesConstPtr indices_;
+
+  /** \brief An input point cloud describing the surface that is to be used
+    * for nearest neighbors estimation.
+    */
+  // PointCloudInConstPtr surface_;
+
+  /** \brief A pointer to the spatial search object. */
+  KdTreePtr tree_;
+
+  /** \brief The number of K nearest neighbors to use for each point. */
+  int k_;
+
+  /** \brief The nearest neighbors search radius for each point. */
+  double search_radius_;
+
+  // ROS nodelet attributes
+  /** \brief The surface PointCloud subscriber filter. */
+  message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+  /** \brief Set to true if the nodelet needs to listen for incoming point
+   * clouds representing the search surface.
+   */
+  bool use_surface_;
+
+  /** \brief Parameter for the spatial locator tree. By convention, the values represent:
+    * 0: ANN (Approximate Nearest Neigbor library) kd-tree
+    * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
+    * 2: Organized spatial dataset index
+    */
+  int spatial_locator_type_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<FeatureConfig>> srv_;
+
+  /** \brief Child initialization routine. Internal method. */
+  virtual bool childInit(ros::NodeHandle & nh) = 0;
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
+
+  /** \brief Compute the feature and publish it. Internal method. */
+  virtual void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices) = 0;
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(FeatureConfig & config, uint32_t level);
+
+  /** \brief Null passthrough filter, used for pushing empty elements in the
+    * synchronizer */
+  message_filters::PassThrough<PointIndices> nf_pi_;
+  message_filters::PassThrough<PointCloudIn> nf_pc_;
+
+  /** \brief Input point cloud callback.
+    * Because we want to use the same synchronizer object, we push back
+    * empty elements with the same timestamp.
+    */
+  inline void
+  input_callback(const PointCloudInConstPtr & input)
+  {
+    PointIndices indices;
+    indices.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
+    PointCloudIn cloud;
+    cloud.header.stamp = input->header.stamp;
+    nf_pc_.add(ros_ptr(cloud.makeShared()));
+    nf_pi_.add(boost::make_shared<PointIndices>(indices));
+  }
+
+private:
+  /** \brief Synchronized input, surface, and point indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
+    PointCloudIn, PointIndices>>> sync_input_surface_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+    PointCloudIn, PointIndices>>> sync_input_surface_indices_e_;
+
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+  /** \brief NodeletLazy connection routine. */
+  virtual void subscribe();
+  virtual void unsubscribe();
+
+  /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
+    * \param cloud the pointer to the input point cloud
+    * \param cloud_surface the pointer to the surface point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_surface_indices_callback(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudInConstPtr & cloud_surface,
+    const PointIndicesConstPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////
+class FeatureFromNormals : public Feature
+{
+public:
+  typedef sensor_msgs::PointCloud2 PointCloud2;
+
+  typedef pcl::PointCloud<pcl::Normal> PointCloudN;
+  typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
+  typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
+
+  FeatureFromNormals()
+  : normals_() {}
+
+protected:
+  /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
+  PointCloudNConstPtr normals_;
+
+  /** \brief Child initialization routine. Internal method. */
+  virtual bool childInit(ros::NodeHandle & nh) = 0;
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  virtual void emptyPublish(const PointCloudInConstPtr & cloud) = 0;
+
+  /** \brief Compute the feature and publish it. */
+  virtual void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices) = 0;
+
+private:
+  /** \brief The normals PointCloud subscriber filter. */
+  message_filters::Subscriber<PointCloudN> sub_normals_filter_;
+
+  /** \brief Synchronized input, normals, surface, and point indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
+    PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+    PointCloudN, PointCloudIn, PointIndices>>> sync_input_normals_surface_indices_e_;
+
+  /** \brief Internal method. */
+  void computePublish(
+    const PointCloudInConstPtr &,
+    const PointCloudInConstPtr &,
+    const IndicesPtr &) {}                        // This should never be called
+
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+  /** \brief NodeletLazy connection routine. */
+  virtual void subscribe();
+  virtual void unsubscribe();
+
+  /** \brief Input point cloud callback. Used when \a use_indices and \a use_surface are set.
+    * \param cloud the pointer to the input point cloud
+    * \param cloud_normals the pointer to the input point cloud normals
+    * \param cloud_surface the pointer to the surface point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_normals_surface_indices_callback(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & cloud_normals,
+    const PointCloudInConstPtr & cloud_surface,
+    const PointIndicesConstPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__FEATURE_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh.hpp

@@ -0,0 +1,99 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: fpfh.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__FPFH_HPP_
+#define PCL_ROS__FEATURES__FPFH_HPP_
+
+#include <pcl/features/fpfh.h>
+#include "pcl_ros/features/pfh.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b FPFHEstimation estimates the <b>Fast Point Feature Histogram (FPFH)</b> descriptor for a given point cloud
+  * dataset containing points and normals.
+  *
+  * @note If you use this code in any academic work, please cite:
+  *
+  * <ul>
+  * <li> R.B. Rusu, N. Blodow, M. Beetz.
+  *      Fast Point Feature Histograms (FPFH) for 3D Registration.
+  *      In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
+  *      Kobe, Japan, May 12-17 2009.
+  * </li>
+  * <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
+  *      Fast Geometric Point Labeling using Conditional Random Fields.
+  *      In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
+  *      St. Louis, MO, USA, October 11-15 2009.
+  * </li>
+  * </ul>
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+  * \author Radu Bogdan Rusu
+  */
+class FPFHEstimation : public FeatureFromNormals
+{
+private:
+  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
+
+  typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__FPFH_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/fpfh_omp.hpp

@@ -0,0 +1,96 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: fpfh_omp.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__FPFH_OMP_HPP_
+#define PCL_ROS__FEATURES__FPFH_OMP_HPP_
+
+#include <pcl/features/fpfh_omp.h>
+#include "pcl_ros/features/fpfh.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud
+  * dataset containing points and normals, in parallel, using the OpenMP standard.
+  *
+  * @note If you use this code in any academic work, please cite:
+  *
+  * <ul>
+  * <li> R.B. Rusu, N. Blodow, M. Beetz.
+  *      Fast Point Feature Histograms (FPFH) for 3D Registration.
+  *      In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),
+  *      Kobe, Japan, May 12-17 2009.
+  * </li>
+  * <li> R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz.
+  *      Fast Geometric Point Labeling using Conditional Random Fields.
+  *      In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
+  *      St. Louis, MO, USA, October 11-15 2009.
+  * </li>
+  * </ul>
+  * \author Radu Bogdan Rusu
+  */
+class FPFHEstimationOMP : public FeatureFromNormals
+{
+private:
+  pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> impl_;
+
+  typedef pcl::PointCloud<pcl::FPFHSignature33> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__FPFH_OMP_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/moment_invariants.hpp

@@ -0,0 +1,82 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: moment_invariants.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
+#define PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_
+
+#include <pcl/features/moment_invariants.h>
+#include "pcl_ros/features/feature.hpp"
+
+namespace pcl_ros
+{
+/** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
+  * \author Radu Bogdan Rusu
+  */
+class MomentInvariantsEstimation : public Feature
+{
+private:
+  pcl::MomentInvariantsEstimation<pcl::PointXYZ, pcl::MomentInvariants> impl_;
+
+  typedef pcl::PointCloud<pcl::MomentInvariants> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__MOMENT_INVARIANTS_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d.hpp

@@ -0,0 +1,86 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: normal_3d.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__NORMAL_3D_HPP_
+#define PCL_ROS__FEATURES__NORMAL_3D_HPP_
+
+#include <pcl/features/normal_3d.h>
+#include "pcl_ros/features/feature.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b NormalEstimation estimates local surface properties at each 3D point, such as surface normals and
+  * curvatures.
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a NormalEstimationOpenMP and \a NormalEstimationTBB for parallel implementations.
+  * \author Radu Bogdan Rusu
+  */
+class NormalEstimation : public Feature
+{
+private:
+  /** \brief PCL implementation object. */
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> impl_;
+
+  typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type.
+    * \param cloud the input point cloud to copy the header from.
+    */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__NORMAL_3D_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_omp.hpp

@@ -0,0 +1,80 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: normal_3d_omp.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
+#define PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_
+
+#include <pcl/features/normal_3d_omp.h>
+#include "pcl_ros/features/normal_3d.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and
+  * curvatures, in parallel, using the OpenMP standard.
+  * \author Radu Bogdan Rusu
+  */
+class NormalEstimationOMP : public Feature
+{
+private:
+  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> impl_;
+
+  typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__NORMAL_3D_OMP_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/normal_3d_tbb.hpp

@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: normal_3d_tbb.h 35661 2011-02-01 06:04:14Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
+#define PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_
+
+// #include "pcl_ros/pcl_ros_config.hpp"
+// #if defined(HAVE_TBB)
+
+#include <pcl/features/normal_3d_tbb.h>
+#include "pcl_ros/features/normal_3d.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and
+  * curvatures, in parallel, using Intel's Threading Building Blocks library.
+  * \author Radu Bogdan Rusu
+  */
+class NormalEstimationTBB : public Feature
+{
+private:
+  pcl::NormalEstimationTBB<pcl::PointXYZ, pcl::Normal> impl_;
+
+  typedef pcl::PointCloud<pcl::Normal> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloud>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+// #endif  // HAVE_TBB
+
+#endif  // PCL_ROS__FEATURES__NORMAL_3D_TBB_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/pfh.hpp

@@ -0,0 +1,99 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: pfh.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__PFH_HPP_
+#define PCL_ROS__FEATURES__PFH_HPP_
+
+#include <pcl/features/pfh.h>
+#include "pcl_ros/features/feature.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset
+  * containing points and normals.
+  *
+  * @note If you use this code in any academic work, please cite:
+  *
+  * <ul>
+  * <li> R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz.
+  *      Aligning Point Cloud Views using Persistent Feature Histograms.
+  *      In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),
+  *      Nice, France, September 22-26 2008.
+  * </li>
+  * <li> R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz.
+  *      Learning Informative Point Classes for the Acquisition of Object Model Maps.
+  *      In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV),
+  *      Hanoi, Vietnam, December 17-20 2008.
+  * </li>
+  * </ul>
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+  * \author Radu Bogdan Rusu
+  */
+class PFHEstimation : public FeatureFromNormals
+{
+private:
+  pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> impl_;
+
+  typedef pcl::PointCloud<pcl::PFHSignature125> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__PFH_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/principal_curvatures.hpp

@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: principal_curvatures.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
+#define PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_
+
+#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET true
+#include <pcl/features/principal_curvatures.h>
+#include "pcl_ros/features/feature.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of
+  * principal surface curvatures for a given point cloud dataset containing points and normals.
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a NormalEstimationOpenMP and \a NormalEstimationTBB for examples on how to extend this to parallel implementations.
+  * \author Radu Bogdan Rusu, Jared Glover
+  */
+class PrincipalCurvaturesEstimation : public FeatureFromNormals
+{
+private:
+  pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> impl_;
+
+  typedef pcl::PointCloud<pcl::PrincipalCurvatures> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__PRINCIPAL_CURVATURES_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot.hpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, Ryohei Ueda, JSK Lab
+ *  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 JSK Lab. 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.
+ *
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__SHOT_HPP_
+#define PCL_ROS__FEATURES__SHOT_HPP_
+
+#include <pcl/features/shot.h>
+#include "pcl_ros/features/pfh.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b SHOTEstimation estimates SHOT descriptor.
+  *
+  */
+class SHOTEstimation : public FeatureFromNormals
+{
+private:
+  pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
+
+  typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__SHOT_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/shot_omp.hpp

@@ -0,0 +1,78 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, Ryohei Ueda, JSK Lab
+ *  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 JSK Lab. 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.
+ *
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__SHOT_OMP_HPP_
+#define PCL_ROS__FEATURES__SHOT_OMP_HPP_
+
+#include <pcl/features/shot_omp.h>
+#include "pcl_ros/features/shot.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b SHOTEstimation estimates SHOT descriptor using OpenMP.
+  */
+class SHOTEstimationOMP : public FeatureFromNormals
+{
+private:
+  pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> impl_;
+
+  typedef pcl::PointCloud<pcl::SHOT352> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__SHOT_OMP_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/features/vfh.hpp

@@ -0,0 +1,84 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: vfh.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FEATURES__VFH_HPP_
+#define PCL_ROS__FEATURES__VFH_HPP_
+
+#include <pcl/features/vfh.h>
+#include "pcl_ros/features/fpfh.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
+  * dataset containing points and normals.
+  *
+  * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
+  * \a FPFHEstimationOpenMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram).
+  * \author Radu Bogdan Rusu
+  */
+class VFHEstimation : public FeatureFromNormals
+{
+private:
+  pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> impl_;
+
+  typedef pcl::PointCloud<pcl::VFHSignature308> PointCloudOut;
+
+  /** \brief Child initialization routine. Internal method. */
+  inline bool
+  childInit(ros::NodeHandle & nh)
+  {
+    // Create the output publisher
+    pub_output_ = advertise<PointCloudOut>(nh, "output", max_queue_size_);
+    return true;
+  }
+
+  /** \brief Publish an empty point cloud of the feature output type. */
+  void emptyPublish(const PointCloudInConstPtr & cloud);
+
+  /** \brief Compute the feature and publish it. */
+  void computePublish(
+    const PointCloudInConstPtr & cloud,
+    const PointCloudNConstPtr & normals,
+    const PointCloudInConstPtr & surface,
+    const IndicesPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FEATURES__VFH_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/crop_box.hpp

@@ -0,0 +1,87 @@
+/*
+ *
+ *  Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ *  $Id: cropbox.cpp
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__CROP_BOX_HPP_
+#define PCL_ROS__FILTERS__CROP_BOX_HPP_
+
+// PCL includes
+#include <pcl/filters/crop_box.h>
+#include <vector>
+#include "pcl_ros/filters/filter.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box.
+  *
+  * \author Radu Bogdan Rusu
+  * \author Justin Rosen
+  * \author Marti Morta Garriga
+  */
+class CropBox : public Filter
+{
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+private:
+  /** \brief The PCL filter implementation used. */
+  pcl::CropBox<pcl::PCLPointCloud2> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit CropBox(const rclcpp::NodeOptions & options);
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__CROP_BOX_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/extract_indices.hpp

@@ -0,0 +1,84 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: extract_indices.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
+#define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_
+
+// PCL includes
+#include <pcl/filters/extract_indices.h>
+#include <vector>
+#include "pcl_ros/filters/filter.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
+  * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
+  * \author Radu Bogdan Rusu
+  */
+class ExtractIndices : public Filter
+{
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+private:
+  /** \brief The PCL filter implementation used. */
+  pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit ExtractIndices(const rclcpp::NodeOptions & options);
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/filter.hpp

@@ -0,0 +1,163 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__FILTER_HPP_
+#define PCL_ROS__FILTERS__FILTER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include "pcl_ros/pcl_node.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+/** \brief @b Filter represents the base filter class. Some generic 3D operations that are
+  * applicable to all filters are defined here as static methods.
+  * \author Radu Bogdan Rusu
+  */
+class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
+{
+public:
+  typedef sensor_msgs::msg::PointCloud2 PointCloud2;
+
+  typedef pcl::IndicesPtr IndicesPtr;
+  typedef pcl::IndicesConstPtr IndicesConstPtr;
+
+  /** \brief Filter constructor
+    * \param node_name node name
+    * \param options node options
+    */
+  Filter(std::string node_name, const rclcpp::NodeOptions & options);
+
+protected:
+  /** \brief declare and subscribe to param callback for input_frame and output_frame params */
+  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_;
+
+  message_filters::Subscriber<PointCloud2> sub_input_filter_;
+
+  /** \brief The desired user filter field name. */
+  std::string filter_field_name_;
+
+  /** \brief The minimum allowed filter value a point will be considered from. */
+  double filter_limit_min_;
+
+  /** \brief The maximum allowed filter value a point will be considered from. */
+  double filter_limit_max_;
+
+  /** \brief Set to true if we want to return the data outside
+    * (\a filter_limit_min_;\a filter_limit_max_). Default: false.
+    */
+  bool filter_limit_negative_;
+
+  /** \brief The input TF frame the data should be transformed into,
+    * if input.header.frame_id is different.
+    */
+  std::string tf_input_frame_;
+
+  /** \brief The original data input TF frame. */
+  std::string tf_input_orig_frame_;
+
+  /** \brief The output TF frame the data should be transformed into,
+    * if input.header.frame_id is different.
+    */
+  std::string tf_output_frame_;
+
+  /** \brief Internal mutex. */
+  std::mutex mutex_;
+
+  /** \brief Virtual abstract filter method. To be implemented by every child.
+    * \param input the input point cloud dataset.
+    * \param indices a pointer to the vector of point indices to use.
+    * \param output the resultant filtered PointCloud2
+    */
+  virtual void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) = 0;
+
+  /** \brief Lazy transport subscribe routine. */
+  virtual void
+  subscribe();
+
+  /** \brief Lazy transport unsubscribe routine. */
+  virtual void
+  unsubscribe();
+
+  /** \brief Call the child filter () method, optionally transform the result, and publish it.
+    * \param input the input point cloud dataset.
+    * \param indices a pointer to the vector of point indices to use.
+    */
+  void
+  computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices);
+
+private:
+  /** \brief Pointer to parameters callback handle. */
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+  /** \brief Synchronized input, and indices.*/
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
+    PointIndices>>> sync_input_indices_e_;
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+    PointIndices>>> sync_input_indices_a_;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  /** \brief PointCloud2 + Indices data callback. */
+  void
+  input_indices_callback(
+    const PointCloud2::ConstSharedPtr & cloud,
+    const PointIndices::ConstSharedPtr & indices);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__FILTER_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/passthrough.hpp

@@ -0,0 +1,84 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: passthrough.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__PASSTHROUGH_HPP_
+#define PCL_ROS__FILTERS__PASSTHROUGH_HPP_
+
+// PCL includes
+#include <pcl/filters/passthrough.h>
+#include <vector>
+#include "pcl_ros/filters/filter.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given
+  * constraints.
+  * \author Radu Bogdan Rusu
+  */
+class PassThrough : public Filter
+{
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+private:
+  /** \brief The PCL filter implementation used. */
+  pcl::PassThrough<pcl::PCLPointCloud2> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit PassThrough(const rclcpp::NodeOptions & options);
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__PASSTHROUGH_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/project_inliers.hpp

@@ -0,0 +1,103 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: project_inliers.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#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"
+
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+/** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a
+  * separate PointCloud.
+  * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
+  * \author Radu Bogdan Rusu
+  */
+class ProjectInliers : public Filter
+{
+public:
+  explicit ProjectInliers(const rclcpp::NodeOptions & options);
+
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+private:
+  /** \brief A pointer to the vector of model coefficients. */
+  ModelCoefficientsConstPtr model_;
+
+  /** \brief The message filter subscriber for model coefficients. */
+  message_filters::Subscriber<ModelCoefficients> sub_model_;
+
+  /** \brief Synchronized input, indices, and model coefficients.*/
+  std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
+    PointIndices, ModelCoefficients>>> sync_input_indices_model_e_;
+  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_;
+
+  void subscribe() override;
+  void unsubscribe() override;
+
+  /** \brief PointCloud2 + Indices + Model data callback. */
+  void
+  input_indices_model_callback(
+    const PointCloud2::ConstSharedPtr & cloud,
+    const PointIndicesConstPtr & indices,
+    const ModelCoefficientsConstPtr & model);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp

@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: radius_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
+#define PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_
+
+// PCL includes
+#include <pcl/filters/radius_outlier_removal.h>
+#include <vector>
+#include "pcl_ros/filters/filter.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
+  * search radius is smaller than a given K.
+  * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
+  * \author Radu Bogdan Rusu
+  */
+class RadiusOutlierRemoval : public Filter
+{
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+private:
+  /** \brief The PCL filter implementation used. */
+  pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options);
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__RADIUS_OUTLIER_REMOVAL_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp

@@ -0,0 +1,91 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: statistical_outlier_removal.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
+#define PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_
+
+// PCL includes
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <vector>
+#include "pcl_ros/filters/filter.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
+  * information check:
+  * <ul>
+  * <li> R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz.
+  *      Towards 3D Point Cloud Based Object Maps for Household Environments
+  *      Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008.
+  * </ul>
+  *
+  * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
+  * \author Radu Bogdan Rusu
+  */
+class StatisticalOutlierRemoval : public Filter
+{
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+
+private:
+  /** \brief The PCL filter implementation used. */
+  pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options);
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__FILTERS__STATISTICAL_OUTLIER_REMOVAL_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp

@@ -0,0 +1,83 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: voxel_grid.h 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_
+#define PCL_ROS__FILTERS__VOXEL_GRID_HPP_
+
+// PCL includes
+#include <pcl/filters/voxel_grid.h>
+#include <vector>
+#include "pcl_ros/filters/filter.hpp"
+
+namespace pcl_ros
+{
+/** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
+  * \author Radu Bogdan Rusu
+  */
+class VoxelGrid : public Filter
+{
+protected:
+  /** \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
+    */
+  inline void
+  filter(
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+    PointCloud2 & output) override;
+
+  /** \brief Parameter callback
+    * \param params parameter values to set
+    */
+  rcl_interfaces::msg::SetParametersResult
+  config_callback(const std::vector<rclcpp::Parameter> & params);
+
+  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
+
+#endif  // PCL_ROS__FILTERS__VOXEL_GRID_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/impl/transforms.hpp

@@ -0,0 +1,266 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+#ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_
+#define PCL_ROS__IMPL__TRANSFORMS_HPP_
+
+#include <pcl/common/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2/convert.h>
+#include <tf2/exceptions.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <string>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/time.hpp>
+
+#include "pcl_ros/transforms.hpp"
+
+using pcl_conversions::fromPCL;
+using pcl_conversions::toPCL;
+
+namespace pcl_ros
+{
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+void
+transformPointCloudWithNormals(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform)
+{
+  // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
+  // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
+  // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
+  // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
+  // the conversion of the point cloud anyway. Idem for the origin.
+  tf2::Quaternion q = transform.getRotation();
+  Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z());  // internally stored as (x,y,z,w)
+  tf2::Vector3 v = transform.getOrigin();
+  Eigen::Vector3f origin(v.x(), v.y(), v.z());
+  //    Eigen::Translation3f translation(v);
+  // Assemble an Eigen Transform
+  // Eigen::Transform3f t;
+  // t = translation * rotation;
+  transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+void
+transformPointCloudWithNormals(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out, const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Transform tf;
+  tf2::convert(transform.transform, tf);
+  transformPointCloudWithNormals(cloud_in, cloud_out, tf);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+void
+transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform)
+{
+  // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering
+  // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but
+  // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined.
+  // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to
+  // the conversion of the point cloud anyway. Idem for the origin.
+  tf2::Quaternion q = transform.getRotation();
+  Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z());  // internally stored as (x,y,z,w)
+  tf2::Vector3 v = transform.getOrigin();
+  Eigen::Vector3f origin(v.x(), v.y(), v.z());
+  //    Eigen::Translation3f translation(v);
+  // Assemble an Eigen Transform
+  // Eigen::Transform3f t;
+  // t = translation * rotation;
+  transformPointCloud(cloud_in, cloud_out, origin, rotation);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+void
+transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out, const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Transform tf;
+  tf2::convert(transform.transform, tf);
+  transformPointCloud(cloud_in, cloud_out, tf);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  if (cloud_in.header.frame_id == target_frame) {
+    cloud_out = cloud_in;
+    return true;
+  }
+
+  geometry_msgs::msg::TransformStamped transform;
+  try {
+    transform =
+      tf_buffer.lookupTransform(
+      target_frame, cloud_in.header.frame_id,
+      fromPCL(cloud_in.header.stamp));
+  } catch (tf2::LookupException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  } catch (tf2::ExtrapolationException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  }
+
+  transformPointCloudWithNormals(cloud_in, cloud_out, transform);
+  cloud_out.header.frame_id = target_frame;
+  return true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  if (cloud_in.header.frame_id == target_frame) {
+    cloud_out = cloud_in;
+    return true;
+  }
+
+  geometry_msgs::msg::TransformStamped transform;
+  try {
+    transform =
+      tf_buffer.lookupTransform(
+      target_frame, cloud_in.header.frame_id,
+      fromPCL(cloud_in.header.stamp));
+  } catch (tf2::LookupException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  } catch (tf2::ExtrapolationException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  }
+
+  transformPointCloud(cloud_in, cloud_out, transform);
+  cloud_out.header.frame_id = target_frame;
+  return true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
+  const rclcpp::Time & target_time,
+  const pcl::PointCloud<PointT> & cloud_in,
+  const std::string & fixed_frame,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  try {
+    transform = tf_buffer.lookupTransform(
+      target_frame, target_time, cloud_in.header.frame_id,
+      fromPCL(cloud_in.header.stamp), fixed_frame);
+  } catch (tf2::LookupException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  } catch (tf2::ExtrapolationException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  }
+
+  transformPointCloud(cloud_in, cloud_out, transform);
+  cloud_out.header.frame_id = target_frame;
+  std_msgs::msg::Header header;
+  header.stamp = target_time;
+  cloud_out.header = toPCL(header);
+  return true;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
+  const rclcpp::Time & target_time,
+  const pcl::PointCloud<PointT> & cloud_in,
+  const std::string & fixed_frame,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  try {
+    transform = tf_buffer.lookupTransform(
+      target_frame, target_time, cloud_in.header.frame_id,
+      fromPCL(cloud_in.header.stamp), fixed_frame);
+  } catch (tf2::LookupException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  } catch (tf2::ExtrapolationException & e) {
+    RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
+    return false;
+  }
+
+  transformPointCloudWithNormals(cloud_in, cloud_out, transform);
+  cloud_out.header.frame_id = target_frame;
+  std_msgs::msg::Header header;
+  header.stamp = target_time;
+  cloud_out.header = toPCL(header);
+  return true;
+}
+}  // namespace pcl_ros
+#endif  // PCL_ROS__IMPL__TRANSFORMS_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/bag_io.hpp

@@ -0,0 +1,131 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: bag_io.h 35471 2011-01-25 06:50:00Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__IO__BAG_IO_HPP_
+#define PCL_ROS__IO__BAG_IO_HPP_
+
+#include <sensor_msgs/PointCloud2.h>
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <string>
+#include <pcl_ros/pcl_nodelet.hpp>
+
+namespace pcl_ros
+{
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief BAG PointCloud file format reader.
+  * \author Radu Bogdan Rusu
+  */
+class BAGReader : public nodelet::Nodelet
+{
+public:
+  typedef sensor_msgs::PointCloud2 PointCloud;
+  typedef PointCloud::Ptr PointCloudPtr;
+  typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+  /** \brief Empty constructor. */
+  BAGReader()
+  : publish_rate_(0), output_() /*, cloud_received_ (false)*/ {}
+
+  /** \brief Set the publishing rate in seconds.
+    * \param publish_rate the publishing rate in seconds
+    */
+  inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
+
+  /** \brief Get the publishing rate in seconds. */
+  inline double getPublishRate() {return publish_rate_;}
+
+  /** \brief Get the next point cloud dataset in the BAG file.
+    * \return the next point cloud dataset as read from the file
+    */
+  inline PointCloudConstPtr
+  getNextCloud()
+  {
+    if (it_ != view_.end()) {
+      output_ = it_->instantiate<sensor_msgs::PointCloud2>();
+      ++it_;
+    }
+    return output_;
+  }
+
+  /** \brief Open a BAG file for reading and select a specified topic
+    * \param file_name the BAG file to open
+    * \param topic_name the topic that we want to read data from
+    */
+  bool open(const std::string & file_name, const std::string & topic_name);
+
+  /** \brief Close an open BAG file. */
+  inline void
+  close()
+  {
+    bag_.close();
+  }
+
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+private:
+  /** \brief The publishing interval in seconds. Set to 0 to publish once (default). */
+  double publish_rate_;
+
+  /** \brief The BAG object. */
+  rosbag::Bag bag_;
+
+  /** \brief The BAG view object. */
+  rosbag::View view_;
+
+  /** \brief The BAG view iterator object. */
+  rosbag::View::iterator it_;
+
+  /** \brief The name of the topic that contains the PointCloud data. */
+  std::string topic_name_;
+
+  /** \brief The name of the BAG file that contains the PointCloud data. */
+  std::string file_name_;
+
+  /** \brief The output point cloud dataset containing the points loaded from the file. */
+  PointCloudPtr output_;
+
+  /** \brief Signals that a new PointCloud2 message has been read from the file. */
+  // bool cloud_received_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__IO__BAG_IO_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_data.hpp

@@ -0,0 +1,146 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: concatenate_data.h 35231 2011-01-14 05:33:20Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__IO__CONCATENATE_DATA_HPP_
+#define PCL_ROS__IO__CONCATENATE_DATA_HPP_
+
+// ROS includes
+#include <tf/transform_listener.h>
+#include <nodelet_topic_tools/nodelet_lazy.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/pass_through.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <string>
+#include <vector>
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data
+  * synchronizer: it listens for a set of input PointCloud messages on the same topic,
+  * checks their timestamps, and concatenates their fields together into a single
+  * PointCloud output message.
+  * \author Radu Bogdan Rusu
+  */
+class PointCloudConcatenateDataSynchronizer : public nodelet_topic_tools::NodeletLazy
+{
+public:
+  typedef sensor_msgs::PointCloud2 PointCloud2;
+  typedef PointCloud2::Ptr PointCloud2Ptr;
+  typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
+
+  /** \brief Empty constructor. */
+  PointCloudConcatenateDataSynchronizer()
+  : maximum_queue_size_(3) {}
+
+  /** \brief Empty constructor.
+    * \param queue_size the maximum queue size
+    */
+  explicit PointCloudConcatenateDataSynchronizer(int queue_size)
+  : maximum_queue_size_(queue_size), approximate_sync_(false) {}
+
+  /** \brief Empty destructor. */
+  virtual ~PointCloudConcatenateDataSynchronizer() {}
+
+  void onInit();
+  void subscribe();
+  void unsubscribe();
+
+private:
+  /** \brief The output PointCloud publisher. */
+  ros::Publisher pub_output_;
+
+  /** \brief The maximum number of messages that we can store in the queue. */
+  int maximum_queue_size_;
+
+  /** \brief True if we use an approximate time synchronizer
+    * versus an exact one (false by default).
+    */
+  bool approximate_sync_;
+
+  /** \brief A vector of message filters. */
+  std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2>>> filters_;
+
+  /** \brief Output TF frame the concatenated points should be transformed to. */
+  std::string output_frame_;
+
+  /** \brief Input point cloud topics. */
+  XmlRpc::XmlRpcValue input_topics_;
+
+  /** \brief TF listener object. */
+  tf::TransformListener tf_;
+
+  /** \brief Null passthrough filter, used for pushing empty elements in the
+    * synchronizer */
+  message_filters::PassThrough<PointCloud2> nf_;
+
+  /** \brief Synchronizer.
+    * \note This will most likely be rewritten soon using the DynamicTimeSynchronizer.
+    */
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
+    PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2,
+    PointCloud2>>> ts_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2,
+    PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2>>> ts_e_;
+
+  /** \brief Input point cloud callback.
+    * Because we want to use the same synchronizer object, we push back
+    * empty elements with the same timestamp.
+    */
+  inline void
+  input_callback(const PointCloud2ConstPtr & input)
+  {
+    PointCloud2 cloud;
+    cloud.header.stamp = input->header.stamp;
+    nf_.add(boost::make_shared<PointCloud2>(cloud));
+  }
+
+  /** \brief Input callback for 8 synchronized topics. */
+  void input(
+    const PointCloud2::ConstPtr & in1, const PointCloud2::ConstPtr & in2,
+    const PointCloud2::ConstPtr & in3, const PointCloud2::ConstPtr & in4,
+    const PointCloud2::ConstPtr & in5, const PointCloud2::ConstPtr & in6,
+    const PointCloud2::ConstPtr & in7, const PointCloud2::ConstPtr & in8);
+
+  void combineClouds(const PointCloud2 & in1, const PointCloud2 & in2, PointCloud2 & out);
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__IO__CONCATENATE_DATA_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/concatenate_fields.hpp

@@ -0,0 +1,106 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: concatenate_fields.h 35052 2011-01-03 21:04:57Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
+#define PCL_ROS__IO__CONCATENATE_FIELDS_HPP_
+
+// ROS includes
+#include <nodelet_topic_tools/nodelet_lazy.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <map>
+#include <vector>
+
+namespace pcl_ros
+{
+/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of
+  * input PointCloud messages on the same topic, checks their timestamps, and concatenates their fields together into
+  * a single PointCloud output message.
+  * \author Radu Bogdan Rusu
+  */
+class PointCloudConcatenateFieldsSynchronizer : public nodelet_topic_tools::NodeletLazy
+{
+public:
+  typedef sensor_msgs::PointCloud2 PointCloud;
+  typedef PointCloud::Ptr PointCloudPtr;
+  typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+  /** \brief Empty constructor. */
+  PointCloudConcatenateFieldsSynchronizer()
+  : maximum_queue_size_(3), maximum_seconds_(0) {}
+
+  /** \brief Empty constructor.
+    * \param queue_size the maximum queue size
+    */
+  explicit PointCloudConcatenateFieldsSynchronizer(int queue_size)
+  : maximum_queue_size_(queue_size), maximum_seconds_(0) {}
+
+  /** \brief Empty destructor. */
+  virtual ~PointCloudConcatenateFieldsSynchronizer() {}
+
+  void onInit();
+  void subscribe();
+  void unsubscribe();
+  void input_callback(const PointCloudConstPtr & cloud);
+
+private:
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+  /** \brief The output PointCloud publisher. */
+  ros::Publisher pub_output_;
+
+  /** \brief The number of input messages that we expect on the input topic. */
+  int input_messages_;
+
+  /** \brief The maximum number of messages that we can store in the queue. */
+  int maximum_queue_size_;
+
+  /** \brief The maximum number of seconds to wait until we drop the synchronization. */
+  double maximum_seconds_;
+
+  /** \brief A queue for messages. */
+  std::map<ros::Time, std::vector<PointCloudConstPtr>> queue_;
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__IO__CONCATENATE_FIELDS_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/io/pcd_io.hpp

@@ -0,0 +1,137 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: pcd_io.h 35054 2011-01-03 21:16:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__IO__PCD_IO_HPP_
+#define PCL_ROS__IO__PCD_IO_HPP_
+
+#include <pcl/io/pcd_io.h>
+#include <string>
+#include "pcl_ros/pcl_nodelet.hpp"
+
+namespace pcl_ros
+{
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Point Cloud Data (PCD) file format reader.
+  * \author Radu Bogdan Rusu
+  */
+class PCDReader : public PCLNodelet
+{
+public:
+  typedef sensor_msgs::PointCloud2 PointCloud2;
+  typedef PointCloud2::Ptr PointCloud2Ptr;
+  typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
+
+  /** \brief Empty constructor. */
+  PCDReader()
+  : publish_rate_(0), tf_frame_("/base_link") {}
+
+  virtual void onInit();
+
+  /** \brief Set the publishing rate in seconds.
+    * \param publish_rate the publishing rate in seconds
+    */
+  inline void setPublishRate(double publish_rate) {publish_rate_ = publish_rate;}
+
+  /** \brief Get the publishing rate in seconds. */
+  inline double getPublishRate() {return publish_rate_;}
+
+  /** \brief Set the TF frame the PointCloud will be published in.
+    * \param tf_frame the TF frame the PointCloud will be published in
+    */
+  inline void setTFframe(std::string tf_frame) {tf_frame_ = tf_frame;}
+
+  /** \brief Get the TF frame the PointCloud is published in. */
+  inline std::string getTFframe() {return tf_frame_;}
+
+protected:
+  /** \brief The publishing interval in seconds. Set to 0 to only publish once (default). */
+  double publish_rate_;
+
+  /** \brief The TF frame the data should be published in ("/base_link" by default). */
+  std::string tf_frame_;
+
+  /** \brief The name of the file that contains the PointCloud data. */
+  std::string file_name_;
+
+  /** \brief The output point cloud dataset containing the points loaded from the file. */
+  PointCloud2Ptr output_;
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::PCDReader impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Point Cloud Data (PCD) file format writer.
+  * \author Radu Bogdan Rusu
+  */
+class PCDWriter : public PCLNodelet
+{
+public:
+  PCDWriter()
+  : file_name_(""), binary_mode_(true) {}
+
+  typedef sensor_msgs::PointCloud2 PointCloud2;
+  typedef PointCloud2::Ptr PointCloud2Ptr;
+  typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
+
+  virtual void onInit();
+  void input_callback(const PointCloud2ConstPtr & cloud);
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+protected:
+  /** \brief The name of the file that contains the PointCloud data. */
+  std::string file_name_;
+
+  /** \brief Set to true if the output files should be saved in binary mode (true). */
+  bool binary_mode_;
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::PCDWriter impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__IO__PCD_IO_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/pcl_node.hpp

@@ -0,0 +1,296 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $
+ *
+ */
+
+/**
+
+\author Radu Bogdan Rusu
+
+**/
+
+#ifndef PCL_ROS__PCL_NODE_HPP_
+#define PCL_ROS__PCL_NODE_HPP_
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+// #include <nodelet_topic_tools/nodelet_lazy.h>  // TODO(daisukes): lazy subscription
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_msgs/msg/point_indices.hpp>
+#include <pcl_msgs/msg/model_coefficients.hpp>
+
+// #include "pcl_ros/point_cloud.hpp"
+
+using pcl_conversions::fromPCL;
+
+namespace pcl_ros
+{
+////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from
+ *  this class. */
+template<typename T, typename PublisherT = rclcpp::Publisher<T>>
+class PCLNode : public rclcpp::Node
+{
+public:
+  typedef sensor_msgs::msg::PointCloud2 PointCloud2;
+
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+  typedef PointCloud::Ptr PointCloudPtr;
+  typedef PointCloud::ConstPtr PointCloudConstPtr;
+
+  typedef pcl_msgs::msg::PointIndices PointIndices;
+  typedef PointIndices::SharedPtr PointIndicesPtr;
+  typedef PointIndices::ConstSharedPtr PointIndicesConstPtr;
+
+  typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients;
+  typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr;
+  typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr;
+
+  typedef pcl::IndicesPtr IndicesPtr;
+  typedef pcl::IndicesConstPtr IndicesConstPtr;
+
+  /** \brief Empty constructor. */
+  PCLNode(std::string node_name, const rclcpp::NodeOptions & options)
+  : rclcpp::Node(node_name, options),
+    use_indices_(false), transient_local_indices_(false),
+    max_queue_size_(3), approximate_sync_(false),
+    tf_buffer_(this->get_clock()),
+    tf_listener_(tf_buffer_)
+  {
+    {
+      rcl_interfaces::msg::ParameterDescriptor desc;
+      desc.name = "max_queue_size";
+      desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+      desc.description = "QoS History depth";
+      desc.read_only = true;
+      max_queue_size_ = declare_parameter(desc.name, max_queue_size_, desc);
+    }
+
+    {
+      rcl_interfaces::msg::ParameterDescriptor desc;
+      desc.name = "use_indices";
+      desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+      desc.description = "Only process a subset of the point cloud from an indices topic";
+      desc.read_only = true;
+      use_indices_ = declare_parameter(desc.name, use_indices_, desc);
+    }
+
+    {
+      rcl_interfaces::msg::ParameterDescriptor desc;
+      desc.name = "transient_local_indices";
+      desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+      desc.description = "Does indices topic use transient local documentation";
+      desc.read_only = true;
+      transient_local_indices_ = declare_parameter(desc.name, transient_local_indices_, desc);
+    }
+
+    {
+      rcl_interfaces::msg::ParameterDescriptor desc;
+      desc.name = "approximate_sync";
+      desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+      desc.description =
+        "Match indices and point cloud messages if time stamps are approximatly the same.";
+      desc.read_only = true;
+      approximate_sync_ = declare_parameter(desc.name, approximate_sync_, desc);
+    }
+
+    RCLCPP_DEBUG(
+      this->get_logger(), "PCL Node successfully created with the following parameters:\n"
+      " - approximate_sync          : %s\n"
+      " - use_indices               : %s\n"
+      " - transient_local_indices_  : %s\n"
+      " - max_queue_size            : %d",
+      (approximate_sync_) ? "true" : "false",
+      (use_indices_) ? "true" : "false",
+      (transient_local_indices_) ? "true" : "false",
+      max_queue_size_);
+  }
+
+protected:
+  /** \brief Set to true if point indices are used.
+   *
+   * When receiving a point cloud, if use_indices_ is false, the entire
+   * point cloud is processed for the given operation. If use_indices_ is
+   * true, then the ~indices topic is read to get the vector of point
+   * indices specifying the subset of the point cloud that will be used for
+   * the operation. In the case where use_indices_ is true, the ~input and
+   * ~indices topics must be synchronised in time, either exact or within a
+   * specified jitter. See also @ref transient_local_indices_ and approximate_sync.
+   **/
+  bool use_indices_;
+  /** \brief Set to true if the indices topic has transient_local durability.
+   *
+   * If use_indices_ is true, the ~input and ~indices topics generally must
+   * be synchronised in time. By setting this flag to true, the most recent
+   * value from ~indices can be used instead of requiring a synchronised
+   * message.
+   **/
+  bool transient_local_indices_;
+
+  /** \brief The message filter subscriber for PointCloud2. */
+  message_filters::Subscriber<PointCloud2> sub_input_filter_;
+
+  /** \brief The message filter subscriber for PointIndices. */
+  message_filters::Subscriber<PointIndices> sub_indices_filter_;
+
+  /** \brief The output PointCloud publisher. Allow each individual class that inherits from this
+    *  to declare the Publisher with their type.
+    */
+  std::shared_ptr<PublisherT> pub_output_;
+
+  /** \brief The maximum queue size (default: 3). */
+  int max_queue_size_;
+
+  /** \brief True if we use an approximate time synchronizer
+    * versus an exact one (false by default).
+    **/
+  bool approximate_sync_;
+
+  /** \brief TF listener object. */
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
+    * \param cloud the point cloud to test
+    * \param topic_name an optional topic name (only used for printing, defaults to "input")
+    */
+  inline bool
+  isValid(const PointCloud2::ConstSharedPtr & cloud, const std::string & topic_name = "input")
+  {
+    if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
+      RCLCPP_WARN(
+        this->get_logger(),
+        "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) "
+        "with stamp %d.%09d, and frame %s on topic %s received!",
+        cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
+        cloud->header.stamp.sec, cloud->header.stamp.nanosec,
+        cloud->header.frame_id.c_str(), topic_name.c_str());
+
+      return false;
+    }
+    return true;
+  }
+
+  /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
+    * \param cloud the point cloud to test
+    * \param topic_name an optional topic name (only used for printing, defaults to "input")
+    */
+  inline bool
+  isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input")
+  {
+    if (cloud->width * cloud->height != cloud->points.size()) {
+      RCLCPP_WARN(
+        this->get_logger(),
+        "Invalid PointCloud (points = %zu, width = %d, height = %d) "
+        "with stamp %d.%09d, and frame %s on topic %s received!",
+        cloud->points.size(), cloud->width, cloud->height,
+        fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec,
+        cloud->header.frame_id.c_str(), topic_name.c_str());
+
+      return false;
+    }
+    return true;
+  }
+
+  /** \brief Test whether a given PointIndices message is "valid" (i.e., has values).
+    * \param indices the point indices message to test
+    * \param topic_name an optional topic name (only used for printing, defaults to "indices")
+    */
+  inline bool
+  isValid(
+    const PointIndicesConstPtr & /*indices*/,
+    const std::string & /*topic_name*/ = "indices")
+  {
+    /*if (indices->indices.empty ())
+    {
+      RCLCPP_WARN(
+        this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, "
+        "and frame %s on topic %s received!",
+        indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
+        indices->header.frame_id.c_str(), topic_name.c_str());
+      //return (true);  // looks like it should be false
+      return false;
+    }*/
+    return true;
+  }
+
+  /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values).
+    * \param model the model coefficients to test
+    * \param topic_name an optional topic name (only used for printing, defaults to "model")
+    */
+  inline bool
+  isValid(
+    const ModelCoefficientsConstPtr & /*model*/,
+    const std::string & /*topic_name*/ = "model")
+  {
+    /*if (model->values.empty ())
+    {
+      RCLCPP_WARN(
+        this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, "
+        "and frame %s on topic %s received!",
+        model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec,
+        model->header.frame_id.c_str(), topic_name.c_str());
+      return (false);
+    }*/
+    return true;
+  }
+
+  /** \brief Lazy transport subscribe/unsubscribe routine.
+    * It is optional for backward compatibility.
+    **/
+  virtual void subscribe() {}
+  virtual void unsubscribe() {}
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__PCL_NODE_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/point_cloud.hpp

@@ -0,0 +1,467 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ */
+
+#ifndef PCL_ROS__POINT_CLOUD_HPP__
+#define PCL_ROS__POINT_CLOUD_HPP__
+
+// test if testing machinery can be implemented
+#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr)
+#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1
+#else
+#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0
+#endif
+
+#include <ros/ros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/pcl_config.h>  // for PCL_VERSION_COMPARE
+#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
+#include <pcl/type_traits.h>
+#else
+#include <pcl/point_traits.h>
+#endif  // PCL_VERSION_COMPARE(>=, 1, 11, 0)
+#include <pcl/for_each_type.h>
+#include <pcl/conversions.h>
+#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
+#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
+#include <pcl/memory.h>
+#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
+#include <pcl/make_shared.h>
+#endif
+#endif
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <string>
+#include <utility>
+#include <vector>
+#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
+#include <type_traits>
+#include <memory>
+#endif
+#include <boost/foreach.hpp>  // for BOOST_FOREACH
+#include <boost/mpl/size.hpp>
+#include <boost/ref.hpp>
+
+namespace pcl
+{
+namespace detail
+{
+template<typename Stream, typename PointT>
+struct FieldStreamer
+{
+  explicit FieldStreamer(Stream & stream)
+  : stream_(stream) {}
+
+  template<typename U>
+  void operator()()
+  {
+    const char * name = pcl::traits::name<PointT, U>::value;
+    std::uint32_t name_length = strlen(name);
+    stream_.next(name_length);
+    if (name_length > 0) {
+      memcpy(stream_.advance(name_length), name, name_length);
+    }
+
+    std::uint32_t offset = pcl::traits::offset<PointT, U>::value;
+    stream_.next(offset);
+
+    std::uint8_t datatype = pcl::traits::datatype<PointT, U>::value;
+    stream_.next(datatype);
+
+    std::uint32_t count = pcl::traits::datatype<PointT, U>::size;
+    stream_.next(count);
+  }
+
+  Stream & stream_;
+};
+
+template<typename PointT>
+struct FieldsLength
+{
+  FieldsLength()
+  : length(0) {}
+
+  template<typename U>
+  void operator()()
+  {
+    std::uint32_t name_length = strlen(pcl::traits::name<PointT, U>::value);
+    length += name_length + 13;
+  }
+
+  std::uint32_t length;
+};
+}  // namespace detail
+}  // namespace pcl
+
+namespace ros
+{
+// In ROS 1.3.1+, we can specialize the functor used to create PointCloud<T> objects
+// on the subscriber side. This allows us to generate the mapping between message
+// data and object fields only once and reuse it.
+#if ROS_VERSION_MINIMUM(1, 3, 1)
+template<typename T>
+struct DefaultMessageCreator<pcl::PointCloud<T>>
+{
+  boost::shared_ptr<pcl::MsgFieldMap> mapping_;
+
+  DefaultMessageCreator()
+  : mapping_(boost::make_shared<pcl::MsgFieldMap>() )
+  {
+  }
+
+  boost::shared_ptr<pcl::PointCloud<T>> operator()()
+  {
+    boost::shared_ptr<pcl::PointCloud<T>> msg(new pcl::PointCloud<T>());
+    pcl::detail::getMapping(*msg) = mapping_;
+    return msg;
+  }
+};
+#endif
+
+namespace message_traits
+{
+template<typename T>
+struct MD5Sum<pcl::PointCloud<T>>
+{
+  static const char * value() {return MD5Sum<sensor_msgs::PointCloud2>::value();}
+  static const char * value(const pcl::PointCloud<T> &) {return value();}
+
+  static const uint64_t static_value1 = MD5Sum<sensor_msgs::PointCloud2>::static_value1;
+  static const uint64_t static_value2 = MD5Sum<sensor_msgs::PointCloud2>::static_value2;
+
+  // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here.
+  ROS_STATIC_ASSERT(static_value1 == 0x1158d486dd51d683ULL);
+  ROS_STATIC_ASSERT(static_value2 == 0xce2f1be655c3c181ULL);
+};
+
+template<typename T>
+struct DataType<pcl::PointCloud<T>>
+{
+  static const char * value() {return DataType<sensor_msgs::PointCloud2>::value();}
+  static const char * value(const pcl::PointCloud<T> &) {return value();}
+};
+
+template<typename T>
+struct Definition<pcl::PointCloud<T>>
+{
+  static const char * value() {return Definition<sensor_msgs::PointCloud2>::value();}
+  static const char * value(const pcl::PointCloud<T> &) {return value();}
+};
+
+// pcl point clouds message don't have a ROS compatible header
+// the specialized meta functions below (TimeStamp and FrameId)
+// can be used to get the header data.
+template<typename T>
+struct HasHeader<pcl::PointCloud<T>>: FalseType {};
+
+template<typename T>
+struct TimeStamp<pcl::PointCloud<T>>
+{
+  // This specialization could be dangerous, but it's the best I can do.
+  // If this TimeStamp struct is destroyed before they are done with the
+  // pointer returned by the first functions may go out of scope, but there
+  // isn't a lot I can do about that. This is a good reason to refuse to
+  // returning pointers like this...
+  static ros::Time * pointer(typename pcl::PointCloud<T> & m)
+  {
+    header_.reset(new std_msgs::Header());
+    pcl_conversions::fromPCL(m.header, *(header_));
+    return &(header_->stamp);
+  }
+  static ros::Time const * pointer(const typename pcl::PointCloud<T> & m)
+  {
+    header_const_.reset(new std_msgs::Header());
+    pcl_conversions::fromPCL(m.header, *(header_const_));
+    return &(header_const_->stamp);
+  }
+  static ros::Time value(const typename pcl::PointCloud<T> & m)
+  {
+    return pcl_conversions::fromPCL(m.header).stamp;
+  }
+
+private:
+  static boost::shared_ptr<std_msgs::Header> header_;
+  static boost::shared_ptr<std_msgs::Header> header_const_;
+};
+
+template<typename T>
+struct FrameId<pcl::PointCloud<T>>
+{
+  static std::string * pointer(pcl::PointCloud<T> & m) {return &m.header.frame_id;}
+  static std::string const * pointer(const pcl::PointCloud<T> & m) {return &m.header.frame_id;}
+  static std::string value(const pcl::PointCloud<T> & m) {return m.header.frame_id;}
+};
+
+}  // namespace message_traits
+
+namespace serialization
+{
+template<typename T>
+struct Serializer<pcl::PointCloud<T>>
+{
+  template<typename Stream>
+  inline static void write(Stream & stream, const pcl::PointCloud<T> & m)
+  {
+    stream.next(m.header);
+
+    // Ease the user's burden on specifying width/height for unorganized datasets
+    uint32_t height = m.height, width = m.width;
+    if (height == 0 && width == 0) {
+      width = m.points.size();
+      height = 1;
+    }
+    stream.next(height);
+    stream.next(width);
+
+    // Stream out point field metadata
+    typedef typename pcl::traits::fieldList<T>::type FieldList;
+    uint32_t fields_size = boost::mpl::size<FieldList>::value;
+    stream.next(fields_size);
+    pcl::for_each_type<FieldList>(pcl::detail::FieldStreamer<Stream, T>(stream));
+
+    // Assume little-endian...
+    uint8_t is_bigendian = false;
+    stream.next(is_bigendian);
+
+    // Write out point data as binary blob
+    uint32_t point_step = sizeof(T);
+    stream.next(point_step);
+    uint32_t row_step = point_step * width;
+    stream.next(row_step);
+    uint32_t data_size = row_step * height;
+    stream.next(data_size);
+    memcpy(stream.advance(data_size), m.points.data(), data_size);
+
+    uint8_t is_dense = m.is_dense;
+    stream.next(is_dense);
+  }
+
+  template<typename Stream>
+  inline static void read(Stream & stream, pcl::PointCloud<T> & m)
+  {
+    std_msgs::Header header;
+    stream.next(header);
+    pcl_conversions::toPCL(header, m.header);
+    stream.next(m.height);
+    stream.next(m.width);
+
+    /// @todo Check that fields haven't changed!
+    std::vector<sensor_msgs::PointField> fields;
+    stream.next(fields);
+
+    // Construct field mapping if deserializing for the first time
+    boost::shared_ptr<pcl::MsgFieldMap> & mapping_ptr = pcl::detail::getMapping(m);
+    if (!mapping_ptr) {
+      // This normally should get allocated by DefaultMessageCreator, but just in case
+      mapping_ptr = boost::make_shared<pcl::MsgFieldMap>();
+    }
+    pcl::MsgFieldMap & mapping = *mapping_ptr;
+    if (mapping.empty()) {
+      pcl::createMapping<T>(fields, mapping);
+    }
+
+    uint8_t is_bigendian;
+    stream.next(is_bigendian);     // ignoring...
+    uint32_t point_step, row_step;
+    stream.next(point_step);
+    stream.next(row_step);
+
+    // Copy point data
+    uint32_t data_size;
+    stream.next(data_size);
+    assert(data_size == m.height * m.width * point_step);
+    m.points.resize(m.height * m.width);
+    uint8_t * m_data = reinterpret_cast<uint8_t *>(m.points.data());
+    // If the data layouts match, can copy a whole row in one memcpy
+    if (mapping.size() == 1 &&
+      mapping[0].serialized_offset == 0 &&
+      mapping[0].struct_offset == 0 &&
+      point_step == sizeof(T))
+    {
+      uint32_t m_row_step = sizeof(T) * m.width;
+      // And if the row steps match, can copy whole point cloud in one memcpy
+      if (m_row_step == row_step) {
+        memcpy(m_data, stream.advance(data_size), data_size);
+      } else {
+        for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) {
+          memcpy(m_data, stream.advance(row_step), m_row_step);
+        }
+      }
+    } else {
+      // If not, do a lot of memcpys to copy over the fields
+      for (uint32_t row = 0; row < m.height; ++row) {
+        const uint8_t * stream_data = stream.advance(row_step);
+        for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) {
+          BOOST_FOREACH(const pcl::detail::FieldMapping & fm, mapping) {
+            memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size);
+          }
+          m_data += sizeof(T);
+        }
+      }
+    }
+
+    uint8_t is_dense;
+    stream.next(is_dense);
+    m.is_dense = is_dense;
+  }
+
+  inline static uint32_t serializedLength(const pcl::PointCloud<T> & m)
+  {
+    uint32_t length = 0;
+
+    length += serializationLength(m.header);
+    length += 8;     // height/width
+
+    pcl::detail::FieldsLength<T> fl;
+    typedef typename pcl::traits::fieldList<T>::type FieldList;
+    pcl::for_each_type<FieldList>(boost::ref(fl));
+    length += 4;     // size of 'fields'
+    length += fl.length;
+
+    length += 1;     // is_bigendian
+    length += 4;     // point_step
+    length += 4;     // row_step
+    length += 4;     // size of 'data'
+    length += m.points.size() * sizeof(T);     // data
+    length += 1;     // is_dense
+
+    return length;
+  }
+};
+}  // namespace serialization
+
+/// @todo Printer specialization in message_operations
+
+}  // namespace ros
+
+namespace pcl
+{
+namespace detail
+{
+#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+template<class T>
+constexpr static bool pcl_uses_boost = std::is_same<boost::shared_ptr<T>,
+    pcl::shared_ptr<T>>::value;
+#else
+template<class T>
+constexpr static bool pcl_uses_boost = true;
+#endif
+
+template<class SharedPointer>
+struct Holder
+{
+  SharedPointer p;
+
+  explicit Holder(const SharedPointer & p)
+  : p(p) {}
+  Holder(const Holder & other)
+  : p(other.p) {}
+  Holder(Holder && other)
+  : p(std::move(other.p)) {}
+
+  void operator()(...) {p.reset();}
+};
+
+template<class T>
+inline std::shared_ptr<T> to_std_ptr(const boost::shared_ptr<T> & p)
+{
+  typedef Holder<std::shared_ptr<T>> H;
+  if (H * h = boost::get_deleter<H>(p)) {
+    return h->p;
+  } else {
+    return std::shared_ptr<T>(p.get(), Holder<boost::shared_ptr<T>>(p));
+  }
+}
+
+template<class T>
+inline boost::shared_ptr<T> to_boost_ptr(const std::shared_ptr<T> & p)
+{
+  typedef Holder<boost::shared_ptr<T>> H;
+  if (H * h = std::get_deleter<H>(p)) {
+    return h->p;
+  } else {
+    return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
+  }
+}
+#endif
+}  // namespace detail
+
+// add functions to convert to smart pointer used by ROS
+template<class T>
+inline boost::shared_ptr<T> ros_ptr(const boost::shared_ptr<T> & p)
+{
+  return p;
+}
+
+#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED
+template<class T>
+inline boost::shared_ptr<T> ros_ptr(const std::shared_ptr<T> & p)
+{
+  return detail::to_boost_ptr(p);
+}
+
+// add functions to convert to smart pointer used by PCL, based on PCL's own pointer
+template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
+inline std::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
+{
+  return p;
+}
+
+template<class T, class = typename std::enable_if<!detail::pcl_uses_boost<T>>::type>
+inline std::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
+{
+  return detail::to_std_ptr(p);
+}
+
+template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
+inline boost::shared_ptr<T> pcl_ptr(const std::shared_ptr<T> & p)
+{
+  return detail::to_boost_ptr(p);
+}
+
+template<class T, class = typename std::enable_if<detail::pcl_uses_boost<T>>::type>
+inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
+{
+  return p;
+}
+#else
+template<class T>
+inline boost::shared_ptr<T> pcl_ptr(const boost::shared_ptr<T> & p)
+{
+  return p;
+}
+#endif
+}  // namespace pcl
+
+#endif  // PCL_ROS__POINT_CLOUD_HPP__

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/publisher.hpp

@@ -0,0 +1,151 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: publisher.h 33238 2010-03-11 00:46:58Z rusu $
+ *
+ */
+/**
+
+\author Patrick Mihelich
+
+@b Publisher represents a ROS publisher for the templated PointCloud implementation.
+
+**/
+
+#ifndef PCL_ROS__PUBLISHER_HPP_
+#define PCL_ROS__PUBLISHER_HPP_
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl/conversions.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <string>
+
+namespace pcl_ros
+{
+class BasePublisher
+{
+public:
+  void
+  advertise(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
+  {
+    pub_ = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
+  }
+
+  std::string
+  getTopic()
+  {
+    return pub_.getTopic();
+  }
+
+  uint32_t
+  getNumSubscribers() const
+  {
+    return pub_.getNumSubscribers();
+  }
+
+  void
+  shutdown()
+  {
+    pub_.shutdown();
+  }
+
+  operator void *() const
+  {
+    return (pub_) ? reinterpret_cast<void *>(1) : reinterpret_cast<void *>(0);
+  }
+
+protected:
+  ros::Publisher pub_;
+};
+
+template<typename PointT>
+class Publisher : public BasePublisher
+{
+public:
+  Publisher() {}
+
+  Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
+  {
+    advertise(nh, topic, queue_size);
+  }
+
+  ~Publisher() {}
+
+  inline void
+  publish(const boost::shared_ptr<const pcl::PointCloud<PointT>> & point_cloud) const
+  {
+    publish(*point_cloud);
+  }
+
+  inline void
+  publish(const pcl::PointCloud<PointT> & point_cloud) const
+  {
+    // Fill point cloud binary data
+    sensor_msgs::PointCloud2::Ptr msg_ptr(new sensor_msgs::PointCloud2);
+    pcl::toROSMsg(point_cloud, *msg_ptr);
+    pub_.publish(msg_ptr);
+  }
+};
+
+template<>
+class Publisher<sensor_msgs::PointCloud2>: public BasePublisher
+{
+public:
+  Publisher() {}
+
+  Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size)
+  {
+    advertise(nh, topic, queue_size);
+  }
+
+  ~Publisher() {}
+
+  void
+  publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const
+  {
+    pub_.publish(point_cloud);
+    // pub_.publish (*point_cloud);
+  }
+
+  void
+  publish(const sensor_msgs::PointCloud2 & point_cloud) const
+  {
+    pub_.publish(boost::make_shared<const sensor_msgs::PointCloud2>(point_cloud));
+    // pub_.publish (point_cloud);
+  }
+};
+}  // namespace pcl_ros
+#endif  // PCL_ROS__PUBLISHER_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp

@@ -0,0 +1,113 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: extract_clusters.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
+#define PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_
+
+#include <dynamic_reconfigure/server.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <limits>
+#include "pcl_ros/pcl_nodelet.hpp"
+#include "pcl_ros/EuclideanClusterExtractionConfig.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
+  * \author Radu Bogdan Rusu
+  */
+class EuclideanClusterExtraction : public PCLNodelet
+{
+public:
+  /** \brief Empty constructor. */
+  EuclideanClusterExtraction()
+  : publish_indices_(false), max_clusters_(std::numeric_limits<int>::max()) {}
+
+protected:
+  // ROS nodelet attributes
+  /** \brief Publish indices or convert to PointCloud clusters. Default: false */
+  bool publish_indices_;
+
+  /** \brief Maximum number of clusters to publish. */
+  int max_clusters_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>> srv_;
+
+  /** \brief Nodelet initialization routine. */
+  void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  void subscribe();
+  void unsubscribe();
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(EuclideanClusterExtractionConfig & config, uint32_t level);
+
+  /** \brief Input point cloud callback.
+    * \param cloud the pointer to the input point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointIndicesConstPtr & indices);
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::EuclideanClusterExtraction<pcl::PointXYZ> impl_;
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+  /** \brief Synchronized input, and indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointIndices>>> sync_input_indices_a_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp

@@ -0,0 +1,130 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: extract_polygonal_prism_data.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
+#define PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_
+
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/pass_through.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp"
+#include "pcl_ros/pcl_nodelet.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+/** \brief @b ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with
+  * a given height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying
+  * inside it.
+  *
+  * An example of its usage is to extract the data lying within a set of 3D boundaries (e.g., objects supported by a plane).
+  * \author Radu Bogdan Rusu
+  */
+class ExtractPolygonalPrismData : public PCLNodelet
+{
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+protected:
+  /** \brief The output PointIndices publisher. */
+  ros::Publisher pub_output_;
+
+  /** \brief The message filter subscriber for PointCloud2. */
+  message_filters::Subscriber<PointCloud> sub_hull_filter_;
+
+  /** \brief Synchronized input, planar hull, and indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud,
+    PointIndices>>> sync_input_hull_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointCloud, PointIndices>>> sync_input_hull_indices_a_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>> srv_;
+
+  /** \brief Null passthrough filter, used for pushing empty elements in the
+    * synchronizer */
+  message_filters::PassThrough<PointIndices> nf_;
+
+  /** \brief Input point cloud callback.
+    * Because we want to use the same synchronizer object, we push back
+    * empty elements with the same timestamp.
+    */
+  inline void
+  input_callback(const PointCloudConstPtr & input)
+  {
+    PointIndices cloud;
+    cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
+    nf_.add(boost::make_shared<PointIndices>(cloud));
+  }
+
+  /** \brief Nodelet initialization routine. */
+  void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  void subscribe();
+  void unsubscribe();
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(ExtractPolygonalPrismDataConfig & config, uint32_t level);
+
+  /** \brief Input point cloud callback. Used when \a use_indices is set.
+    * \param cloud the pointer to the input point cloud
+    * \param hull the pointer to the planar hull point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_hull_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointCloudConstPtr & hull,
+    const PointIndicesConstPtr & indices);
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp

@@ -0,0 +1,301 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: sac_segmentation.h 35564 2011-01-27 07:32:12Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
+#define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_
+
+#include <message_filters/pass_through.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <dynamic_reconfigure/server.h>
+#include <string>
+#include "pcl_ros/pcl_nodelet.hpp"
+#include "pcl_ros/SACSegmentationConfig.hpp"
+#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus
+  * methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose
+  * SAC-based segmentation.
+  * \author Radu Bogdan Rusu
+  */
+class SACSegmentation : public PCLNodelet
+{
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+public:
+  /** \brief Constructor. */
+  SACSegmentation()
+  : min_inliers_(0) {}
+
+  /** \brief Set the input TF frame the data should be transformed into before processing,
+    * if input.header.frame_id is different.
+    * \param tf_frame the TF frame the input PointCloud should be transformed into before processing
+    */
+  inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
+
+  /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
+  inline std::string getInputTFframe() {return tf_input_frame_;}
+
+  /** \brief Set the output TF frame the data should be transformed into after processing.
+    * \param tf_frame the TF frame the PointCloud should be transformed into after processing
+    */
+  inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
+
+  /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
+  inline std::string getOutputTFframe() {return tf_output_frame_;}
+
+protected:
+  // The minimum number of inliers a model must have in order to be considered valid.
+  int min_inliers_;
+
+  // ROS nodelet attributes
+  /** \brief The output PointIndices publisher. */
+  ros::Publisher pub_indices_;
+
+  /** \brief The output ModelCoefficients publisher. */
+  ros::Publisher pub_model_;
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationConfig>> srv_;
+
+  /** \brief The input TF frame the data should be transformed into,
+    * if input.header.frame_id is different.
+    */
+  std::string tf_input_frame_;
+
+  /** \brief The original data input TF frame. */
+  std::string tf_input_orig_frame_;
+
+  /** \brief The output TF frame the data should be transformed into,
+    * if input.header.frame_id is different.
+    */
+  std::string tf_output_frame_;
+
+  /** \brief Null passthrough filter, used for pushing empty elements in the
+    * synchronizer */
+  message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;
+
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  virtual void subscribe();
+  virtual void unsubscribe();
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(SACSegmentationConfig & config, uint32_t level);
+
+  /** \brief Input point cloud callback. Used when \a use_indices is set.
+    * \param cloud the pointer to the input point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointIndicesConstPtr & indices);
+
+  /** \brief Pointer to a set of indices stored internally.
+   * (used when \a latched_indices_ is set).
+   */
+  PointIndices indices_;
+
+  /** \brief Indices callback. Used when \a latched_indices_ is set.
+    * \param indices the pointer to the input point cloud indices
+    */
+  inline void
+  indices_callback(const PointIndicesConstPtr & indices)
+  {
+    indices_ = *indices;
+  }
+
+  /** \brief Input callback. Used when \a latched_indices_ is set.
+    * \param input the pointer to the input point cloud
+    */
+  inline void
+  input_callback(const PointCloudConstPtr & input)
+  {
+    indices_.header = fromPCL(input->header);
+    PointIndicesConstPtr indices;
+    indices.reset(new PointIndices(indices_));
+    nf_pi_.add(indices);
+  }
+
+private:
+  /** \brief Internal mutex. */
+  boost::mutex mutex_;
+
+  /** \brief The PCL implementation used. */
+  pcl::SACSegmentation<pcl::PointXYZ> impl_;
+
+  /** \brief Synchronized input, and indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointIndices>>> sync_input_indices_a_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for
+  * Sample Consensus methods and models that require the use of surface normals for estimation.
+  */
+class SACSegmentationFromNormals : public SACSegmentation
+{
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+  typedef pcl::PointCloud<pcl::Normal> PointCloudN;
+  typedef boost::shared_ptr<PointCloudN> PointCloudNPtr;
+  typedef boost::shared_ptr<const PointCloudN> PointCloudNConstPtr;
+
+public:
+  /** \brief Set the input TF frame the data should be transformed into before processing,
+    * if input.header.frame_id is different.
+    * \param tf_frame the TF frame the input PointCloud should be transformed into before processing
+    */
+  inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;}
+
+  /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */
+  inline std::string getInputTFframe() {return tf_input_frame_;}
+
+  /** \brief Set the output TF frame the data should be transformed into after processing.
+    * \param tf_frame the TF frame the PointCloud should be transformed into after processing
+    */
+  inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;}
+
+  /** \brief Get the TF frame the PointCloud should be transformed into after processing. */
+  inline std::string getOutputTFframe() {return tf_output_frame_;}
+
+protected:
+  // ROS nodelet attributes
+  /** \brief The normals PointCloud subscriber filter. */
+  message_filters::Subscriber<PointCloudN> sub_normals_filter_;
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_axis_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<SACSegmentationFromNormalsConfig>> srv_;
+
+  /** \brief Input point cloud callback.
+    * Because we want to use the same synchronizer object, we push back
+    * empty elements with the same timestamp.
+    */
+  inline void
+  input_callback(const PointCloudConstPtr & cloud)
+  {
+    PointIndices indices;
+    indices.header.stamp = fromPCL(cloud->header).stamp;
+    nf_.add(boost::make_shared<PointIndices>(indices));
+  }
+
+  /** \brief Null passthrough filter, used for pushing empty elements in the
+    * synchronizer */
+  message_filters::PassThrough<PointIndices> nf_;
+
+  /** \brief The input TF frame the data should be transformed into,
+   * if input.header.frame_id is different.
+   */
+  std::string tf_input_frame_;
+  /** \brief The original data input TF frame. */
+  std::string tf_input_orig_frame_;
+  /** \brief The output TF frame the data should be transformed into,
+    * if input.header.frame_id is different.
+    */
+  std::string tf_output_frame_;
+
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  virtual void subscribe();
+  virtual void unsubscribe();
+
+  /** \brief Model callback
+    * \param model the sample consensus model found
+    */
+  void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model);
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level);
+
+  /** \brief Input point cloud callback.
+    * \param cloud the pointer to the input point cloud
+    * \param cloud_normals the pointer to the input point cloud normals
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_normals_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointCloudNConstPtr & cloud_normals,
+    const PointIndicesConstPtr & indices);
+
+private:
+  /** \brief Internal mutex. */
+  boost::mutex mutex_;
+
+  /** \brief The PCL implementation used. */
+  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> impl_;
+
+  /** \brief Synchronized input, normals, and indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointCloudN, PointIndices>>> sync_input_normals_indices_a_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloudN,
+    PointIndices>>> sync_input_normals_indices_e_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp

@@ -0,0 +1,111 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: segment_differences.h 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
+#define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_
+
+#include <pcl/segmentation/segment_differences.h>
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/SegmentDifferencesConfig.hpp"
+#include "pcl_ros/pcl_nodelet.hpp"
+
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
+  * difference between them for a maximum given distance threshold.
+  * \author Radu Bogdan Rusu
+  */
+class SegmentDifferences : public PCLNodelet
+{
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+public:
+  /** \brief Empty constructor. */
+  SegmentDifferences() {}
+
+protected:
+  /** \brief The message filter subscriber for PointCloud2. */
+  message_filters::Subscriber<PointCloud> sub_target_filter_;
+
+  /** \brief Synchronized input, and planar hull.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointCloud>>> sync_input_target_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointCloud>>> sync_input_target_a_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<SegmentDifferencesConfig>> srv_;
+
+  /** \brief Nodelet initialization routine. */
+  void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  void subscribe();
+  void unsubscribe();
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(SegmentDifferencesConfig & config, uint32_t level);
+
+  /** \brief Input point cloud callback.
+    * \param cloud the pointer to the input point cloud
+    * \param cloud_target the pointcloud that we want to segment \a cloud from
+    */
+  void input_target_callback(
+    const PointCloudConstPtr & cloud,
+    const PointCloudConstPtr & cloud_target);
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::SegmentDifferences<pcl::PointXYZ> impl_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/convex_hull.hpp

@@ -0,0 +1,95 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: convex_hull.h 36116 2011-02-22 00:05:23Z rusu $
+ *
+ */
+
+#ifndef PCL_ROS__SURFACE__CONVEX_HULL_HPP_
+#define PCL_ROS__SURFACE__CONVEX_HULL_HPP_
+
+#include <pcl/surface/convex_hull.h>
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/pcl_nodelet.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+/** \brief @b ConvexHull2D represents a 2D ConvexHull implementation.
+  * \author Radu Bogdan Rusu
+  */
+class ConvexHull2D : public PCLNodelet
+{
+  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+private:
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  virtual void subscribe();
+  virtual void unsubscribe();
+
+  /** \brief Input point cloud callback.
+    * \param cloud the pointer to the input point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_indices_callback(
+    const PointCloudConstPtr & cloud,
+    const PointIndicesConstPtr & indices);
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::ConvexHull<pcl::PointXYZ> impl_;
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+  /** \brief Publisher for PolygonStamped. */
+  ros::Publisher pub_plane_;
+
+  /** \brief Synchronized input, and indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
+    PointIndices>>> sync_input_indices_a_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__SURFACE__CONVEX_HULL_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/surface/moving_least_squares.hpp

@@ -0,0 +1,152 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: moving_least_squares.h 36097 2011-02-20 14:18:58Z marton $
+ *
+ */
+
+#ifndef PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
+#define PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_
+
+#include <pcl/surface/mls.h>
+#include <dynamic_reconfigure/server.h>
+#include "pcl_ros/pcl_nodelet.hpp"
+#include "pcl_ros/MLSConfig.hpp"
+
+namespace pcl_ros
+{
+namespace sync_policies = message_filters::sync_policies;
+
+/** \brief @b MovingLeastSquares represents a nodelet using the MovingLeastSquares implementation.
+  * The type of the output is the same as the input, it only smooths the XYZ coordinates according
+  * to the parameters.
+  * Normals are estimated at each point as well and published on a separate topic.
+  * \author Radu Bogdan Rusu, Zoltan-Csaba Marton
+  */
+class MovingLeastSquares : public PCLNodelet
+{
+  typedef pcl::PointXYZ PointIn;
+  typedef pcl::PointNormal NormalOut;
+
+  typedef pcl::PointCloud<PointIn> PointCloudIn;
+  typedef boost::shared_ptr<PointCloudIn> PointCloudInPtr;
+  typedef boost::shared_ptr<const PointCloudIn> PointCloudInConstPtr;
+  typedef pcl::PointCloud<NormalOut> NormalCloudOut;
+
+  typedef pcl::KdTree<PointIn> KdTree;
+  typedef pcl::KdTree<PointIn>::Ptr KdTreePtr;
+
+protected:
+  /** \brief An input point cloud describing the surface that is to be used for
+    * nearest neighbors estimation.
+    */
+  PointCloudInConstPtr surface_;
+
+  /** \brief A pointer to the spatial search object. */
+  KdTreePtr tree_;
+
+  /** \brief The nearest neighbors search radius for each point. */
+  double search_radius_;
+
+  /** \brief The number of K nearest neighbors to use for each point. */
+  // int k_;
+
+  /** \brief Whether to use a polynomial fit. */
+  bool use_polynomial_fit_;
+
+  /** \brief The order of the polynomial to be fit. */
+  int polynomial_order_;
+
+  /** \brief How 'flat' should the neighbor weighting gaussian be
+    * the smaller, the more local the fit).
+    */
+  double gaussian_parameter_;
+
+  // ROS nodelet attributes
+  /** \brief The surface PointCloud subscriber filter. */
+  message_filters::Subscriber<PointCloudIn> sub_surface_filter_;
+
+  /** \brief Parameter for the spatial locator tree. By convention, the values represent:
+    * 0: ANN (Approximate Nearest Neigbor library) kd-tree
+    * 1: FLANN (Fast Library for Approximate Nearest Neighbors) kd-tree
+    * 2: Organized spatial dataset index
+    */
+  int spatial_locator_type_;
+
+  /** \brief Pointer to a dynamic reconfigure service. */
+  boost::shared_ptr<dynamic_reconfigure::Server<MLSConfig>> srv_;
+
+  /** \brief Dynamic reconfigure callback
+    * \param config the config object
+    * \param level the dynamic reconfigure level
+    */
+  void config_callback(MLSConfig & config, uint32_t level);
+
+  /** \brief Nodelet initialization routine. */
+  virtual void onInit();
+
+  /** \brief LazyNodelet connection routine. */
+  virtual void subscribe();
+  virtual void unsubscribe();
+
+private:
+  /** \brief Input point cloud callback.
+    * \param cloud the pointer to the input point cloud
+    * \param indices the pointer to the input point cloud indices
+    */
+  void input_indices_callback(
+    const PointCloudInConstPtr & cloud,
+    const PointIndicesConstPtr & indices);
+
+private:
+  /** \brief The PCL implementation used. */
+  pcl::MovingLeastSquares<PointIn, NormalOut> impl_;
+
+  /** \brief The input PointCloud subscriber. */
+  ros::Subscriber sub_input_;
+
+  /** \brief The output PointCloud (containing the normals) publisher. */
+  ros::Publisher pub_normals_;
+
+  /** \brief Synchronized input, and indices.*/
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+    PointIndices>>> sync_input_indices_e_;
+  boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
+    PointIndices>>> sync_input_indices_a_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__SURFACE__MOVING_LEAST_SQUARES_HPP_

ros2_ws/src/perception_pcl/pcl_ros/include/pcl_ros/transforms.hpp

@@ -0,0 +1,233 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+#ifndef PCL_ROS__TRANSFORMS_HPP_
+#define PCL_ROS__TRANSFORMS_HPP_
+
+#include <pcl/point_cloud.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <Eigen/Core>
+#include <string>
+#include <rclcpp/time.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+namespace pcl_ros
+{
+/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
+  * \param cloud_in the input point cloud
+  * \param cloud_out the input point cloud
+  * \param transform a rigid transformation from tf
+  * \note calls the Eigen version
+  */
+template<typename PointT>
+void
+transformPointCloudWithNormals(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2::Transform & transform);
+
+/** \brief Transform a point cloud and rotate its normals using an Eigen transform.
+  * \param cloud_in the input point cloud
+  * \param cloud_out the input point cloud
+  * \param transform a rigid transformation from tf
+  * \note calls the Eigen version
+  */
+template<typename PointT>
+void
+transformPointCloudWithNormals(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const geometry_msgs::msg::TransformStamped & transform);
+
+/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
+  * \param target_frame the target TF frame the point cloud should be transformed to
+  * \param cloud_in the input point cloud
+  * \param cloud_out the input point cloud
+  * \param tf_buffer a TF buffer object
+  */
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer);
+
+/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
+  * \param target_frame the target TF frame the point cloud should be transformed to
+  * \param target_time the target timestamp
+  * \param cloud_in the input point cloud
+  * \param fixed_frame fixed TF frame
+  * \param cloud_out the input point cloud
+  * \param tf_buffer a TF buffer object
+  */
+template<typename PointT>
+bool
+transformPointCloudWithNormals(
+  const std::string & target_frame,
+  const rclcpp::Time & target_time,
+  const pcl::PointCloud<PointT> & cloud_in,
+  const std::string & fixed_frame,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer);
+
+/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
+  * \param cloud_in the input point cloud
+  * \param cloud_out the input point cloud
+  * \param transform a rigid transformation from tf
+  * \note calls the Eigen version
+  */
+template<typename PointT>
+void
+transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2::Transform & transform);
+
+/** \brief Apply a rigid transform defined by a 3D offset and a quaternion
+  * \param cloud_in the input point cloud
+  * \param cloud_out the input point cloud
+  * \param transform a rigid transformation from tf
+  * \note calls the Eigen version
+  */
+template<typename PointT>
+void
+transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const geometry_msgs::msg::TransformStamped & transform);
+
+/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
+  * \param target_frame the target TF frame the point cloud should be transformed to
+  * \param cloud_in the input point cloud
+  * \param cloud_out the input point cloud
+  * \param tf_buffer a TF buffer object
+  */
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
+  const pcl::PointCloud<PointT> & cloud_in,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer);
+
+/** \brief Transforms a point cloud in a given target TF frame using a TransformListener
+  * \param target_frame the target TF frame the point cloud should be transformed to
+  * \param target_time the target timestamp
+  * \param cloud_in the input point cloud
+  * \param fixed_frame fixed TF frame
+  * \param cloud_out the input point cloud
+  * \param tf_buffer a TF buffer object
+  */
+template<typename PointT>
+bool
+transformPointCloud(
+  const std::string & target_frame,
+  const rclcpp::Time & target_time,
+  const pcl::PointCloud<PointT> & cloud_in,
+  const std::string & fixed_frame,
+  pcl::PointCloud<PointT> & cloud_out,
+  const tf2_ros::Buffer & tf_buffer);
+
+/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
+  * \param target_frame the target TF frame
+  * \param in the input PointCloud2 dataset
+  * \param out the resultant transformed PointCloud2 dataset
+  * \param tf_buffer a TF buffer object
+  */
+bool
+transformPointCloud(
+  const std::string & target_frame,
+  const sensor_msgs::msg::PointCloud2 & in,
+  sensor_msgs::msg::PointCloud2 & out,
+  const tf2_ros::Buffer & tf_buffer);
+
+/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
+  * \param target_frame the target TF frame
+  * \param net_transform the TF transformer object
+  * \param in the input PointCloud2 dataset
+  * \param out the resultant transformed PointCloud2 dataset
+  */
+void
+transformPointCloud(
+  const std::string & target_frame,
+  const tf2::Transform & net_transform,
+  const sensor_msgs::msg::PointCloud2 & in,
+  sensor_msgs::msg::PointCloud2 & out);
+
+/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
+  * \param target_frame the target TF frame
+  * \param net_transform the TF transformer object
+  * \param in the input PointCloud2 dataset
+  * \param out the resultant transformed PointCloud2 dataset
+  */
+void
+transformPointCloud(
+  const std::string & target_frame,
+  const geometry_msgs::msg::TransformStamped & net_transform,
+  const sensor_msgs::msg::PointCloud2 & in,
+  sensor_msgs::msg::PointCloud2 & out);
+
+/** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
+  * \param transform the transformation to use on the points
+  * \param in the input PointCloud2 dataset
+  * \param out the resultant transformed PointCloud2 dataset
+  */
+void
+transformPointCloud(
+  const Eigen::Matrix4f & transform,
+  const sensor_msgs::msg::PointCloud2 & in,
+  sensor_msgs::msg::PointCloud2 & out);
+
+/** \brief Obtain the transformation matrix from TF into an Eigen form
+  * \param bt the TF transformation
+  * \param out_mat the Eigen transformation
+  */
+void
+transformAsMatrix(const tf2::Transform & bt, Eigen::Matrix4f & out_mat);
+
+/** \brief Obtain the transformation matrix from TF into an Eigen form
+  * \param bt the TF transformation
+  * \param out_mat the Eigen transformation
+  */
+void
+transformAsMatrix(const geometry_msgs::msg::TransformStamped & bt, Eigen::Matrix4f & out_mat);
+}  // namespace pcl_ros
+
+#endif  // PCL_ROS__TRANSFORMS_HPP_

ros2_ws/src/perception_pcl/pcl_ros/package.xml

@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pcl_ros</name>
+  <version>2.4.1</version>
+  <description>
+
+  PCL (Point Cloud Library) ROS interface stack. PCL-ROS is the preferred
+  bridge for 3D applications involving n-D Point Clouds and 3D geometry
+  processing in ROS.
+
+  </description>
+
+  <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/perception_pcl</url>
+  <url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
+  <url type="repository">https://github.com/ros-perception/perception_pcl</url>
+
+  <author>Open Perception</author>
+  <author email="julius@kammerl.de">Julius Kammerl</author>
+  <author email="william@osrfoundation.org">William Woodall</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>libpcl-all-dev</build_depend>
+
+  <depend>eigen</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <exec_depend>libpcl-common</exec_depend>
+  <exec_depend>libpcl-features</exec_depend>
+  <exec_depend>libpcl-filters</exec_depend>
+  <exec_depend>libpcl-io</exec_depend>
+  <exec_depend>libpcl-segmentation</exec_depend>
+  <exec_depend>libpcl-surface</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>sensor_msgs</test_depend>
+
+    <!--
+    <export>
+    <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_features.xml"/>
+    <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_filters.xml"/>
+    <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_io.xml"/>
+    <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_segmentation.xml"/>
+    <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_surface.xml"/>
+    </export>
+    -->
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_features.xml

@@ -0,0 +1,85 @@
+<!-- PCL Features library component -->
+<library path="lib/libpcl_ros_features">
+  <class name="pcl/BoundaryEstimation" type="BoundaryEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle criterion. The
+      code makes use of the estimated surface normals at each point in the input data set.
+    </description>
+  </class>
+
+  <class name="pcl/FPFHEstimation" type="FPFHEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset
+      containing points and normals.
+    </description>
+  </class>
+
+  <class name="pcl/FPFHEstimationOMP" type="FPFHEstimationOMP" base_class_type="nodelet::Nodelet">
+    <description>
+      FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud dataset
+      containing points and normals, in parallel, using the OpenMP standard.
+    </description>
+  </class>
+
+  <class name="pcl/SHOTEstimation" type="SHOTEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      SHOTEstimation estimates SHOT descriptor for a given point cloud dataset
+      containing points and normals.
+    </description>
+  </class>
+
+  <class name="pcl/SHOTEstimationOMP" type="SHOTEstimationOMP" base_class_type="nodelet::Nodelet">
+    <description>
+      SHOTEstimationOMP estimates SHOT descriptor for a given point cloud dataset
+      containing points and normals, in parallel, using the OpenMP standard.
+    </description>
+  </class>
+  
+  <class name="pcl/MomentInvariantsEstimation" type="MomentInvariantsEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point.
+    </description>
+  </class>
+
+  <class name="pcl/NormalEstimationOMP" type="NormalEstimationOMP" base_class_type="nodelet::Nodelet">
+    <description>
+      NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and curvatures,
+      in parallel, using the OpenMP standard.
+    </description>
+  </class>
+
+  <class name="pcl/NormalEstimationTBB" type="NormalEstimationTBB" base_class_type="nodelet::Nodelet">
+    <description>
+      NormalEstimationTBB estimates local surface properties at each 3D point, such as surface normals and curvatures, in
+      parallel, using Intel's Threading Building Blocks library.
+    </description>
+  </class>
+
+  <class name="pcl/NormalEstimation" type="NormalEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      NormalEstimation estimates local surface properties at each 3D point, such as surface normals and curvatures.
+    </description>
+  </class>
+
+  <class name="pcl/PFHEstimation" type="PFHEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset containing
+      points and normals.
+    </description>
+  </class>
+
+  <class name="pcl/PrincipalCurvaturesEstimation" type="PrincipalCurvaturesEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of principal surface 
+      curvatures for a given point cloud dataset containing points and normals.
+    </description>
+  </class>
+
+  <class name="pcl/VFHEstimation" type="VFHEstimation" base_class_type="nodelet::Nodelet">
+    <description>
+      VFHEstimation estimates the Viewpoint Feature Histogram (VFH) global descriptor for a given point cloud cluster dataset
+      containing points and normals.
+    </description>
+  </class>
+
+</library>

ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_filters.xml

@@ -0,0 +1,43 @@
+<!-- PCL Filters library component -->
+<library path="lib/libpcl_ros_filters">
+  <class name="pcl/PassThrough" type="PassThrough" base_class_type="nodelet::Nodelet">
+    <description>
+      PassThrough is a filter that uses the basic Filter class mechanisms for passing data around.
+    </description>
+  </class>
+
+  <class name="pcl/VoxelGrid" type="VoxelGrid" base_class_type="nodelet::Nodelet">
+    <description>
+      VoxelGrid assembles a local 3D grid over a given PointCloud, and uses that to downsample the data.
+    </description>
+  </class>
+
+  <class name="pcl/ProjectInliers" type="ProjectInliers" base_class_type="nodelet::Nodelet">
+    <description>
+      ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a separate PointCloud.
+    </description>
+  </class>
+
+  <class name="pcl/ExtractIndices" type="ExtractIndices" base_class_type="nodelet::Nodelet">
+    <description>
+      ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud.
+    </description>
+  </class>
+
+  <class name="pcl/StatisticalOutlierRemoval" type="StatisticalOutlierRemoval" base_class_type="nodelet::Nodelet">
+    <description>
+      StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data.
+    </description>
+  </class>
+  <class name="pcl/RadiusOutlierRemoval" type="RadiusOutlierRemoval" base_class_type="nodelet::Nodelet">
+    <description>
+      RadiusOutlierRemoval uses point neighborhood statistics to filter outlier data.
+    </description>
+  </class>
+
+  <class name="pcl/CropBox" type="CropBox" base_class_type="nodelet::Nodelet">
+    <description>
+      CropBox is a filter that allows the user to filter all the data inside of a given box.
+    </description>
+  </class>
+</library>

ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_io.xml

@@ -0,0 +1,49 @@
+<!-- PCL IO library component -->
+<library path="lib/libpcl_ros_io">
+  <class name="pcl/NodeletMUX" type="NodeletMUX" base_class_type="nodelet::Nodelet">
+    <description>
+      NodeletMUX represent a mux nodelet for PointCloud topics: it takes N (up
+      to 8) input topics, and publishes all of them on one output topic.
+    </description>
+  </class>
+
+  <class name="pcl/NodeletDEMUX" type="NodeletDEMUX" base_class_type="nodelet::Nodelet">
+    <description>
+      NodeletDEMUX represent a demux nodelet for PointCloud topics: it
+      publishes 1 input topic to N output topics.
+    </description>
+  </class>
+
+  <class name="pcl/PCDReader" type="PCDReader" base_class_type="nodelet::Nodelet">
+    <description>
+      PCDReader reads in a PCD (Point Cloud Data) v.5 file from disk and converts it to a PointCloud message.
+    </description>
+  </class>
+
+  <class name="pcl/BAGReader" type="BAGReader" base_class_type="nodelet::Nodelet">
+    <description>
+      BAGReader reads in sensor_msgs/PointCloud2 messages from BAG files.
+    </description>
+  </class>
+
+  <class name="pcl/PCDWriter" type="PCDWriter" base_class_type="nodelet::Nodelet">
+    <description>
+      PCDWriter writes a PointCloud message to disk in a PCD (Point Cloud Data) v.5 file format.
+    </description>
+  </class>
+
+  <class name="pcl/PointCloudConcatenateFieldsSynchronizer" type="PointCloudConcatenateFieldsSynchronizer" base_class_type="nodelet::Nodelet">
+    <description>
+      PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of input PointCloud messages on the
+      same topic, checks their timestamps, and concatenates their fields together into a single PointCloud output message.
+    </description>
+  </class>
+  <class name="pcl/PointCloudConcatenateDataSynchronizer" type="PointCloudConcatenateDataSynchronizer" base_class_type="nodelet::Nodelet">
+    <description>
+      PointCloudConcatenateDataSynchronizer is a special form of data
+      synchronizer: it listens for a set of input PointCloud messages on
+      different topics, and concatenates them together into a single PointCloud
+      output message.  
+    </description>
+  </class>
+</library>

ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_segmentation.xml

@@ -0,0 +1,37 @@
+<!-- PCL Segmentation library component -->
+<library path="lib/libpcl_ros_segmentation">
+  <class name="pcl/ExtractPolygonalPrismData" type="ExtractPolygonalPrismData" base_class_type="nodelet::Nodelet">
+    <description>
+      ExtractPolygonalPrismData uses a set of point indices that represent a planar model, and together with a given
+      height, generates a 3D polygonal prism. The polygonal prism is then used to segment all points lying inside it.
+    </description>
+  </class>
+
+   <class name="pcl/EuclideanClusterExtraction" type="EuclideanClusterExtraction" base_class_type="nodelet::Nodelet">
+     <description>
+       EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
+    </description>
+  </class>
+
+  <class name="pcl/SACSegmentationFromNormals" type="SACSegmentationFromNormals" base_class_type="nodelet::Nodelet">
+    <description>
+      SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that
+      it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
+    </description>
+  </class>
+
+  <class name="pcl/SACSegmentation" type="SACSegmentation" base_class_type="nodelet::Nodelet">
+    <description>
+      SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models, in the sense that
+      it just creates a Nodelet wrapper for generic-purpose SAC-based segmentation.
+    </description>
+  </class>
+
+  <class name="pcl/SegmentDifferences" type="SegmentDifferences" base_class_type="nodelet::Nodelet">
+     <description>
+       SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the
+       difference between them for a maximum given distance threshold.
+    </description>
+  </class>
+
+</library>

ros2_ws/src/perception_pcl/pcl_ros/plugins/nodelet/libpcl_ros_surface.xml

@@ -0,0 +1,13 @@
+<!-- PCL Surface reconstruction library component -->
+<library path="lib/libpcl_ros_surface">
+  <class name="pcl/MovingLeastSquares" type="MovingLeastSquares" base_class_type="nodelet::Nodelet">
+    <description>
+      MovingLeastSquares is an implementation of the MLS algorithm for data reconstruction through bivariate polynomial fitting.
+    </description>
+  </class>
+  <class name="pcl/ConvexHull2D" type="ConvexHull2D" base_class_type="nodelet::Nodelet">
+    <description>
+      ConvexHull2D represents a 2D ConvexHull implementation.
+    </description>
+  </class>
+</library>

ros2_ws/src/perception_pcl/pcl_ros/samples/data/.gitignore

@@ -0,0 +1,2 @@
+*
+!.gitignore

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/features/sample_normal_3d.launch

@@ -0,0 +1,44 @@
+<launch>
+
+  <arg name="gui" default="true" />
+
+  <!-- use voxel_grid for small cpu load in filtering -->
+  <include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
+    <arg name="gui" value="false" />
+    <arg name="test" value="false" />
+    <arg name="leaf_size" value="0.02" />
+  </include>
+
+  <node name="normal_estimation"
+        pkg="nodelet" type="nodelet"
+        args="standalone pcl/NormalEstimation">
+    <remap from="~input" to="voxel_grid/output" />
+    <rosparam>
+      radius_search: 0
+      k_search: 10
+      # 0, => ANN, 1 => FLANN, 2 => Organized
+      spatial_locator: 1
+    </rosparam>
+  </node>
+
+  <test test-name="test_normal_estimation"
+        name="test_normal_estimation"
+        pkg="rostest" type="hztest">
+    <rosparam>
+      topic: /normal_estimation/output
+      hz: 10
+      hzerror: 8
+      test_duration: 5.0
+    </rosparam>
+  </test>
+
+  <!-- TODO(wkentaro): Add sample visualization
+  <group if="$(arg gui)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/normal_estimation.rviz">
+    </node>
+  </group>
+  -->
+
+</launch>

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz

@@ -0,0 +1,169 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Filtered1
+      Splitter Ratio: 0.5
+    Tree Height: 565
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Raw
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 0
+      Max Intensity: 246
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Raw
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /voxel_grid/output
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 0
+      Max Intensity: 246
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Filtered
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 5
+      Size (m): 0.01
+      Style: Points
+      Topic: /statistical_outlier_removal/output
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.08107
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.047887
+        Y: -0.164255
+        Z: -0.385388
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.4548
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.73358
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: -10
+  Y: 14

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/config/voxel_grid.rviz

@@ -0,0 +1,169 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Filtered1
+      Splitter Ratio: 0.5
+    Tree Height: 565
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Raw
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 0
+      Max Intensity: 246
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Raw
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 0
+      Max Intensity: 246
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Filtered
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 5
+      Size (m): 0.01
+      Style: Points
+      Topic: /voxel_grid/output
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.08107
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.047887
+        Y: -0.164255
+        Z: -0.385388
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.4548
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.73358
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: -10
+  Y: 14

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_statistical_outlier_removal.launch

@@ -0,0 +1,40 @@
+<launch>
+
+  <arg name="gui" default="true" />
+
+  <!-- use voxel_grid for small cpu load in filtering -->
+  <include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
+    <arg name="gui" value="false" />
+    <arg name="test" value="false" />
+    <arg name="leaf_size" value="0.02" />
+  </include>
+
+  <node name="statistical_outlier_removal"
+        pkg="nodelet" type="nodelet"
+        args="standalone pcl/StatisticalOutlierRemoval">
+    <remap from="~input" to="voxel_grid/output" />
+    <rosparam>
+      mean_k: 10
+      stddev: 1.0
+    </rosparam>
+  </node>
+
+  <test test-name="test_statistical_outlier_removal"
+        name="test_statistical_outlier_removal"
+        pkg="rostest" type="hztest">
+    <rosparam>
+      topic: /statistical_outlier_removal/output
+      hz: 20
+      hzerror: 15
+      test_duration: 5.0
+    </rosparam>
+  </test>
+
+  <group if="$(arg gui)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/statistical_outlier_removal.rviz">
+    </node>
+  </group>
+
+</launch>

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/filters/sample_voxel_grid.launch

@@ -0,0 +1,44 @@
+<launch>
+
+  <arg name="gui" default="true" />
+  <arg name="test" default="true" />
+
+  <arg name="leaf_size" default="0.05" />
+
+  <node name="pcd_to_pointcloud"
+        pkg="pcl_ros" type="pcd_to_pointcloud"
+        args="$(find pcl_ros)/samples/data/table_scene_lms400.pcd 0.033">
+    <remap from="cloud_pcd" to="points" />
+  </node>
+
+  <node name="voxel_grid"
+        pkg="nodelet" type="nodelet"
+        args="standalone pcl/VoxelGrid">
+    <remap from="~input" to="points" />
+    <rosparam subst_value="true">
+      filter_field_name: ''
+      leaf_size: $(arg leaf_size)
+    </rosparam>
+  </node>
+
+  <group if="$(arg test)">
+    <test test-name="test_voxel_grid"
+          name="test_voxel_grid"
+          pkg="rostest" type="hztest">
+      <rosparam>
+        topic: /voxel_grid/output
+        hz: 20
+        hzerror: 15
+        test_duration: 5.0
+      </rosparam>
+    </test>
+  </group>
+
+  <group if="$(arg gui)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find pcl_ros)/samples/pcl_ros/filters/config/voxel_grid.rviz">
+    </node>
+  </group>
+
+</launch>

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/io/sample_concatenate_data.launch

@@ -0,0 +1,43 @@
+<launch>
+
+  <arg name="gui" default="true" />
+
+  <!-- use voxel_grid for small cpu load in filtering -->
+  <include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
+    <arg name="gui" value="false" />
+    <arg name="test" value="false" />
+    <arg name="leaf_size" value="0.02" />
+  </include>
+
+  <node name="concatenate_data"
+        pkg="nodelet" type="nodelet"
+        args="standalone pcl/PointCloudConcatenateDataSynchronizer">
+    <rosparam>
+      output_frame: /base_link
+      input_topics:
+        - /voxel_grid/output
+        - /voxel_grid/output
+    </rosparam>
+  </node>
+
+  <test test-name="test_concatenate_data"
+        name="test_concatenate_data"
+        pkg="rostest" type="hztest">
+    <rosparam>
+      topic: /concatenate_data/output
+      hz: 10
+      hzerror: 8
+      test_duration: 5.0
+    </rosparam>
+  </test>
+
+  <!-- TODO(wkentaro): Add sample visualization
+  <group if="$(arg gui)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find pcl_ros)/samples/pcl_ros/io/config/concatenate_data.rviz">
+    </node>
+  </group>
+  -->
+
+</launch>

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/segmentation/sample_extract_clusters.launch

@@ -0,0 +1,42 @@
+<launch>
+
+  <arg name="gui" default="true" />
+
+  <!-- use voxel_grid for small cpu load in filtering -->
+  <include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
+    <arg name="gui" value="false" />
+    <arg name="test" value="false" />
+    <arg name="leaf_size" value="0.02" />
+  </include>
+
+  <node name="extract_clusters"
+        pkg="nodelet" type="nodelet"
+        args="standalone pcl/EuclideanClusterExtraction">
+    <remap from="~input" to="voxel_grid/output" />
+    <rosparam>
+      cluster_tolerance: 0.03
+      spatial_locator: 1  # FLANN
+    </rosparam>
+  </node>
+
+  <test test-name="test_extract_clusters"
+    name="test_extract_clusters"
+    pkg="rostest" type="hztest">
+    <rosparam>
+      topic: /extract_clusters/output
+      hz: 3000
+      hzerror: 2400
+      test_duration: 5.0
+    </rosparam>
+  </test>
+
+  <!-- TODO(wkentaro): Add sample visualization
+  <group if="$(arg gui)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find pcl_ros)/samples/pcl_ros/segmentation/config/extract_clusters.rviz">
+    </node>
+  </group>
+  -->
+
+</launch>

ros2_ws/src/perception_pcl/pcl_ros/samples/pcl_ros/surface/sample_convex_hull.launch

@@ -0,0 +1,40 @@
+<launch>
+
+  <arg name="gui" default="true" />
+
+  <!-- use voxel_grid for small cpu load in filtering -->
+  <include file="$(find pcl_ros)/samples/pcl_ros/filters/sample_voxel_grid.launch">
+    <arg name="gui" value="false" />
+    <arg name="test" value="false" />
+    <arg name="leaf_size" value="0.02" />
+  </include>
+
+  <node name="convex_hull"
+        pkg="nodelet" type="nodelet"
+        args="standalone pcl/ConvexHull2D">
+    <remap from="~input" to="voxel_grid/output" />
+    <rosparam>
+    </rosparam>
+  </node>
+
+  <test test-name="test_convex_hull"
+    name="test_convex_hull"
+    pkg="rostest" type="hztest">
+    <rosparam>
+      topic: /convex_hull/output
+      hz: 10
+      hzerror: 8
+      test_duration: 5.0
+    </rosparam>
+  </test>
+
+  <!-- TODO(wkentaro): Add sample visualization
+  <group if="$(arg gui)">
+    <node name="rviz"
+          pkg="rviz" type="rviz"
+          args="-d $(find pcl_ros)/samples/pcl_ros/surface/config/convex_hull.rviz">
+    </node>
+  </group>
+  -->
+
+</launch>

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/boundary.cpp

@@ -0,0 +1,75 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/boundary.hpp"
+
+void
+pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::BoundaryEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
+PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/feature.cpp

@@ -0,0 +1,641 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
+ *
+ */
+
+// #include <pluginlib/class_list_macros.h>
+// Include the implementations here instead of compiling them separately to speed up compile time
+// #include "normal_3d.cpp"
+// #include "boundary.cpp"
+// #include "fpfh.cpp"
+// #include "fpfh_omp.cpp"
+// #include "moment_invariants.cpp"
+// #include "normal_3d_omp.cpp"
+// #include "normal_3d_tbb.cpp"
+// #include "pfh.cpp"
+// #include "principal_curvatures.cpp"
+// #include "vfh.cpp"
+#include <pcl/common/io.h>
+#include <message_filters/null_types.h>
+#include <vector>
+#include "pcl_ros/features/feature.hpp"
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::onInit()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit();
+
+  // Call the child init
+  childInit(*pnh_);
+
+  // Allow each individual class that inherits from us to declare their own Publisher
+  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
+  // PointCloud<Normal>, etc
+  // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
+      "parameters before continuing.",
+      getName().c_str());
+    return;
+  }
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
+    return;
+  }
+
+  // ---[ Optional parameters
+  pnh_->getParam("use_surface", use_surface_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &Feature::config_callback, this, _1, _2);
+  srv_->setCallback(f);
+
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
+    " - use_surface    : %s\n"
+    " - k_search       : %d\n"
+    " - radius_search  : %f\n"
+    " - spatial_locator: %d",
+    getName().c_str(),
+    (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+
+  onInitPostProcess();
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::subscribe()
+{
+  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
+  if (use_indices_ || use_surface_) {
+    // Create the objects here
+    if (approximate_sync_) {
+      sync_input_surface_indices_a_ =
+        boost::make_shared<message_filters::Synchronizer<
+            sync_policies::ApproximateTime<
+              PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
+    } else {
+      sync_input_surface_indices_e_ =
+        boost::make_shared<message_filters::Synchronizer<
+            sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
+    }
+
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
+    if (use_indices_) {
+      // If indices are enabled, subscribe to the indices
+      sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
+      if (use_surface_) {   // Use both indices and surface
+        // If surface is enabled, subscribe to the surface,
+        // connect the input-indices-surface trio and register
+        sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
+        }
+      } else {              // Use only indices
+        sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
+        // surface not enabled, connect the input-indices duo and register
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
+        }
+      }
+    } else {                // Use only surface
+      sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
+      // indices not enabled, connect the input-surface duo and register
+      sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+      if (approximate_sync_) {
+        sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
+      } else {
+        sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
+      }
+    }
+    // Register callbacks
+    if (approximate_sync_) {
+      sync_input_surface_indices_a_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    } else {
+      sync_input_surface_indices_e_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    }
+  } else {
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ =
+      pnh_->subscribe<PointCloudIn>(
+      "input", max_queue_size_,
+      bind(
+        &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
+        PointIndicesConstPtr()));
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::unsubscribe()
+{
+  if (use_indices_ || use_surface_) {
+    sub_input_filter_.unsubscribe();
+    if (use_indices_) {
+      sub_indices_filter_.unsubscribe();
+      if (use_surface_) {
+        sub_surface_filter_.unsubscribe();
+      }
+    } else {
+      sub_surface_filter_.unsubscribe();
+    }
+  } else {
+    sub_input_.shutdown();
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
+{
+  if (k_ != config.k_search) {
+    k_ = config.k_search;
+    NODELET_DEBUG(
+      "[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
+  }
+  if (search_radius_ != config.radius_search) {
+    search_radius_ = config.radius_search;
+    NODELET_DEBUG(
+      "[config_callback] Setting the nearest neighbors search radius for each point: %f.",
+      search_radius_);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::input_surface_indices_callback(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers() <= 0) {
+    return;
+  }
+
+  // If cloud is given, check if it's valid
+  if (!isValid(cloud)) {
+    NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If surface is given, check if it's valid
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
+    NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices are given, check if they are valid
+  if (indices && !isValid(indices)) {
+    NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  /// DEBUG
+  if (cloud_surface) {
+    if (indices) {
+      NODELET_DEBUG(
+        "[input_surface_indices_callback]\n"
+        "                                         - PointCloud with %d data points (%s), stamp %f, "
+        "and frame %s on topic %s received.\n"
+        "                                         - PointCloud with %d data points (%s), stamp %f, "
+        "and frame %s on topic %s received.\n"
+        "                                         - PointIndices with %zu values, stamp %f, and "
+        "frame %s on topic %s received.",
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+          cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        indices->indices.size(), indices->header.stamp.toSec(),
+        indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+    } else {
+      NODELET_DEBUG(
+        "[input_surface_indices_callback]\n"
+        "                                 - PointCloud with %d data points (%s), stamp %f, and "
+        "frame %s on topic %s received.\n"
+        "                                 - PointCloud with %d data points (%s), stamp %f, and "
+        "frame %s on topic %s received.",
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+          cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
+    }
+  } else if (indices) {
+    NODELET_DEBUG(
+      "[input_surface_indices_callback]\n"
+      "                                 - PointCloud with %d data points (%s), stamp %f, and frame "
+      "%s on topic %s received.\n"
+      "                                 - PointIndices with %zu values, stamp %f, and frame %s on "
+      "topic %s received.",
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+        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("indices").c_str());
+  } else {
+    NODELET_DEBUG(
+      "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on "
+      "topic %s received.", cloud->width * cloud->height, fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str());
+  }
+  ///
+
+
+  if (static_cast<int>(cloud->width * cloud->height) < k_) {
+    NODELET_ERROR(
+      "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger "
+      "than the PointCloud size (%d)!", k_,
+      (int)(cloud->width * cloud->height));
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices given...
+  IndicesPtr vindices;
+  if (indices && !indices->header.frame_id.empty()) {
+    vindices.reset(new std::vector<int>(indices->indices));
+  }
+
+  computePublish(cloud, cloud_surface, vindices);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::onInit()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit();
+
+  // Call the child init
+  childInit(*pnh_);
+
+  // Allow each individual class that inherits from us to declare their own Publisher
+  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
+  // PointCloud<Normal>, etc
+  // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
+      "parameters before continuing.",
+      getName().c_str());
+    return;
+  }
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
+    return;
+  }
+  // ---[ Optional parameters
+  pnh_->getParam("use_surface", use_surface_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &FeatureFromNormals::config_callback, this, _1, _2);
+  srv_->setCallback(f);
+
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
+    " - use_surface    : %s\n"
+    " - k_search       : %d\n"
+    " - radius_search  : %f\n"
+    " - spatial_locator: %d",
+    getName().c_str(),
+    (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+
+  onInitPostProcess();
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::subscribe()
+{
+  sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
+  sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
+
+  // Create the objects here
+  if (approximate_sync_) {
+    sync_input_normals_surface_indices_a_ =
+      boost::make_shared<message_filters::Synchronizer<
+          sync_policies::ApproximateTime<PointCloudIn, PointCloudN,
+          PointCloudIn, PointIndices>>>(max_queue_size_);
+  } else {
+    sync_input_normals_surface_indices_e_ =
+      boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
+        PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
+  }
+
+  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
+  if (use_indices_ || use_surface_) {
+    if (use_indices_) {
+      // If indices are enabled, subscribe to the indices
+      sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
+      if (use_surface_) {   // Use both indices and surface
+        // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
+        // and register
+        sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+        if (approximate_sync_) {
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
+        }
+      } else {              // Use only indices
+        sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
+        if (approximate_sync_) {
+          // surface not enabled, connect the input-indices duo and register
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
+          // surface not enabled, connect the input-indices duo and register
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
+        }
+      }
+    } else {                // Use only surface
+      // indices not enabled, connect the input-surface duo and register
+      sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+
+      sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
+      if (approximate_sync_) {
+        sync_input_normals_surface_indices_a_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
+      } else {
+        sync_input_normals_surface_indices_e_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
+      }
+    }
+  } else {
+    sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
+
+    if (approximate_sync_) {
+      sync_input_normals_surface_indices_a_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    } else {
+      sync_input_normals_surface_indices_e_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    }
+  }
+
+  // Register callbacks
+  if (approximate_sync_) {
+    sync_input_normals_surface_indices_a_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  } else {
+    sync_input_normals_surface_indices_e_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::unsubscribe()
+{
+  sub_input_filter_.unsubscribe();
+  sub_normals_filter_.unsubscribe();
+  if (use_indices_ || use_surface_) {
+    if (use_indices_) {
+      sub_indices_filter_.unsubscribe();
+      if (use_surface_) {
+        sub_surface_filter_.unsubscribe();
+      }
+    } else {
+      sub_surface_filter_.unsubscribe();
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
+  const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
+  const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers() <= 0) {
+    return;
+  }
+
+  // If cloud+normals is given, check if it's valid
+  if (!isValid(cloud)) {  // || !isValid (cloud_normals, "normals"))
+    NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If surface is given, check if it's valid
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input surface!",
+      getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices are given, check if they are valid
+  if (indices && !isValid(indices)) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input indices!",
+      getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  /// DEBUG
+  if (cloud_surface) {
+    if (indices) {
+      NODELET_DEBUG(
+        "[%s::input_normals_surface_indices_callback]\n"
+        "                                                 - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                                 - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                                 - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                                 - PointIndices with %zu values, "
+        "stamp %f, and frame %s on topic %s received.",
+        getName().c_str(),
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+          cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+          *cloud_normals).c_str(), fromPCL(
+          cloud_normals->header).stamp.toSec(),
+        cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
+        indices->indices.size(), indices->header.stamp.toSec(),
+        indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+    } else {
+      NODELET_DEBUG(
+        "[%s::input_normals_surface_indices_callback]\n"
+        "                                         - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                         - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                         - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.",
+        getName().c_str(),
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+          cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(), fromPCL(
+          cloud_surface->header).stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+          *cloud_normals).c_str(), fromPCL(
+          cloud_normals->header).stamp.toSec(),
+        cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
+    }
+  } else if (indices) {
+    NODELET_DEBUG(
+      "[%s::input_normals_surface_indices_callback]\n"
+      "                                         - PointCloud with %d data points (%s), "
+      "stamp %f, and frame %s on topic %s received.\n"
+      "                                         - PointCloud with %d data points (%s), "
+      "stamp %f, and frame %s on topic %s received.\n"
+      "                                         - PointIndices with %zu values, "
+      "stamp %f, and frame %s on topic %s received.",
+      getName().c_str(),
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(), fromPCL(
+        cloud_normals->header).stamp.toSec(),
+      cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+  } else {
+    NODELET_DEBUG(
+      "[%s::input_normals_surface_indices_callback]\n"
+      "                                 - PointCloud with %d data points (%s), stamp %f, and "
+      "frame %s on topic %s received.\n"
+      "                                 - PointCloud with %d data points (%s), stamp %f, and "
+      "frame %s on topic %s received.",
+      getName().c_str(),
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
+        cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(), fromPCL(
+        cloud_normals->header).stamp.toSec(),
+      cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
+  }
+  ///
+
+  if (static_cast<int>(cloud->width * cloud->height) < k_) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) "
+      "is larger than the PointCloud size (%d)!",
+      getName().c_str(), k_, (int)(cloud->width * cloud->height));
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices given...
+  IndicesPtr vindices;
+  if (indices && !indices->header.frame_id.empty()) {
+    vindices.reset(new std::vector<int>(indices->indices));
+  }
+
+  computePublish(cloud, cloud_normals, cloud_surface, vindices);
+}

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh.cpp

@@ -0,0 +1,76 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/fpfh.hpp"
+
+void
+pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::FPFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::FPFHEstimation FPFHEstimation;
+PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/fpfh_omp.cpp

@@ -0,0 +1,76 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/fpfh_omp.hpp"
+
+void
+pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::FPFHEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
+PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/moment_invariants.cpp

@@ -0,0 +1,74 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/moment_invariants.hpp"
+
+void
+pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::MomentInvariantsEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
+PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d.cpp

@@ -0,0 +1,74 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d.hpp"
+
+void
+pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::NormalEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::NormalEstimation NormalEstimation;
+PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_omp.cpp

@@ -0,0 +1,74 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d_omp.hpp"
+
+void
+pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::NormalEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
+PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp

@@ -0,0 +1,81 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: normal_3d_tbb.cpp 35625 2011-01-31 07:56:13Z gbiggs $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d_tbb.hpp"
+
+#if defined HAVE_TBB
+
+void
+pcl_ros::NormalEstimationTBB::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloud output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::NormalEstimationTBB::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
+PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
+
+#endif  // HAVE_TBB

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/pfh.cpp

@@ -0,0 +1,76 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: pfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/pfh.hpp"
+
+void
+pcl_ros::PFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::PFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::PFHEstimation PFHEstimation;
+PLUGINLIB_EXPORT_CLASS(PFHEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/principal_curvatures.cpp

@@ -0,0 +1,76 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: principal_curvatures.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/principal_curvatures.hpp"
+
+void
+pcl_ros::PrincipalCurvaturesEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::PrincipalCurvaturesEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::PrincipalCurvaturesEstimation PrincipalCurvaturesEstimation;
+PLUGINLIB_EXPORT_CLASS(PrincipalCurvaturesEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot.cpp

@@ -0,0 +1,75 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, Ryohei Ueda, JSK Lab
+ *  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 JSK Lab. 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.
+ *
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/shot.hpp"
+
+void
+pcl_ros::SHOTEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::SHOTEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::SHOTEstimation SHOTEstimation;
+PLUGINLIB_EXPORT_CLASS(SHOTEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/shot_omp.cpp

@@ -0,0 +1,75 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, Ryohei Ueda, JSK Lab
+ *  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 JSK Lab. 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.
+ *
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/shot_omp.hpp"
+
+void
+pcl_ros::SHOTEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::SHOTEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::SHOTEstimationOMP SHOTEstimationOMP;
+PLUGINLIB_EXPORT_CLASS(SHOTEstimationOMP, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/features/vfh.cpp

@@ -0,0 +1,76 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: vfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/vfh.hpp"
+
+void
+pcl_ros::VFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+void
+pcl_ros::VFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+
+  // Set the inputs
+  impl_.setInputCloud(pcl_ptr(cloud));
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(pcl_ptr(surface));
+  impl_.setInputNormals(pcl_ptr(normals));
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(ros_ptr(output.makeShared()));
+}
+
+typedef pcl_ros::VFHEstimation VFHEstimation;
+PLUGINLIB_EXPORT_CLASS(VFHEstimation, nodelet::Nodelet)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/crop_box.cpp

@@ -0,0 +1,253 @@
+/*
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: cropbox.cpp
+ *
+ */
+
+#include "pcl_ros/filters/crop_box.hpp"
+
+pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options)
+: Filter("CropBoxNode", options)
+{
+  // This both declares and initializes the input and output frames
+  use_frame_params();
+
+  rcl_interfaces::msg::ParameterDescriptor min_x_desc;
+  min_x_desc.name = "min_x";
+  min_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  min_x_desc.description =
+    "Minimum x value below which points will be removed";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = -1000.0;
+    float_range.to_value = 1000.0;
+    min_x_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(min_x_desc.name, rclcpp::ParameterValue(-1.0), min_x_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor max_x_desc;
+  max_x_desc.name = "max_x";
+  max_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  max_x_desc.description =
+    "Maximum x value above which points will be removed";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = -1000.0;
+    float_range.to_value = 1000.0;
+    max_x_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(max_x_desc.name, rclcpp::ParameterValue(1.0), max_x_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor min_y_desc;
+  min_y_desc.name = "min_y";
+  min_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  min_y_desc.description =
+    "Minimum y value below which points will be removed";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = -1000.0;
+    float_range.to_value = 1000.0;
+    min_y_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(min_y_desc.name, rclcpp::ParameterValue(-1.0), min_y_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor max_y_desc;
+  max_y_desc.name = "max_y";
+  max_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  max_y_desc.description =
+    "Maximum y value above which points will be removed";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = -1000.0;
+    float_range.to_value = 1000.0;
+    max_y_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(max_y_desc.name, rclcpp::ParameterValue(1.0), max_y_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor min_z_desc;
+  min_z_desc.name = "min_z";
+  min_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  min_z_desc.description =
+    "Minimum z value below which points will be removed";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = -1000.0;
+    float_range.to_value = 1000.0;
+    min_z_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(min_z_desc.name, rclcpp::ParameterValue(-1.0), min_z_desc);
+
+  rcl_interfaces::msg::ParameterDescriptor max_z_desc;
+  max_z_desc.name = "max_z";
+  max_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  max_z_desc.description =
+    "Maximum z value above which points will be removed";
+  {
+    rcl_interfaces::msg::FloatingPointRange float_range;
+    float_range.from_value = -1000.0;
+    float_range.to_value = 1000.0;
+    max_z_desc.floating_point_range.push_back(float_range);
+  }
+  declare_parameter(max_z_desc.name, rclcpp::ParameterValue(1.0), max_z_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);
+
+  rcl_interfaces::msg::ParameterDescriptor negative_desc;
+  negative_desc.name = "negative";
+  negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  negative_desc.description =
+    "Set whether the inliers should be returned (true) or the outliers (false).";
+  declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc);
+
+  const std::vector<std::string> param_names {
+    min_x_desc.name,
+    max_x_desc.name,
+    min_y_desc.name,
+    max_y_desc.name,
+    min_z_desc.name,
+    max_z_desc.name,
+    keep_organized_desc.name,
+    negative_desc.name,
+  };
+
+  callback_handle_ =
+    add_on_set_parameters_callback(
+    std::bind(
+      &CropBox::config_callback, this,
+      std::placeholders::_1));
+
+  config_callback(get_parameters(param_names));
+
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
+}
+
+void
+pcl_ros::CropBox::filter(
+  const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+  PointCloud2 & output)
+{
+  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);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+rcl_interfaces::msg::SetParametersResult
+pcl_ros::CropBox::config_callback(const std::vector<rclcpp::Parameter> & params)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  Eigen::Vector4f min_point, max_point;
+  min_point = impl_.getMin();
+  max_point = impl_.getMax();
+
+  for (const rclcpp::Parameter & param : params) {
+    if (param.get_name() == "min_x") {
+      min_point(0) = param.as_double();
+    }
+    if (param.get_name() == "max_x") {
+      max_point(0) = param.as_double();
+    }
+    if (param.get_name() == "min_y") {
+      min_point(1) = param.as_double();
+    }
+    if (param.get_name() == "max_y") {
+      max_point(1) = param.as_double();
+    }
+    if (param.get_name() == "min_z") {
+      min_point(2) = param.as_double();
+    }
+    if (param.get_name() == "max_z") {
+      max_point(2) = param.as_double();
+    }
+    if (param.get_name() == "negative") {
+      // Check the current value for the negative flag
+      if (impl_.getNegative() != param.as_bool()) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the filter negative flag to: %s.",
+          param.as_bool() ? "true" : "false");
+        // Call the virtual method in the child
+        impl_.setNegative(param.as_bool());
+      }
+    }
+    if (param.get_name() == "keep_organized") {
+      // Check the current value for keep_organized
+      if (impl_.getKeepOrganized() != param.as_bool()) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the filter keep_organized value to: %s.",
+          param.as_bool() ? "true" : "false");
+        // Call the virtual method in the child
+        impl_.setKeepOrganized(param.as_bool());
+      }
+    }
+  }
+
+  // Check the current values for minimum point
+  if (min_point != impl_.getMin()) {
+    RCLCPP_DEBUG(
+      get_logger(), "Setting the minimum point to: %f %f %f.",
+      min_point(0), min_point(1), min_point(2));
+    impl_.setMin(min_point);
+  }
+
+  // Check the current values for the maximum point
+  if (max_point != impl_.getMax()) {
+    RCLCPP_DEBUG(
+      get_logger(), "Setting the maximum point to: %f %f %f.",
+      max_point(0), max_point(1), max_point(2));
+    impl_.setMax(max_point);
+  }
+
+  // Range constraints are enforced by rclcpp::Parameter.
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  return result;
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::CropBox)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/extract_indices.cpp

@@ -0,0 +1,102 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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.
+ *
+ * $Id: extract_indices.cpp 35876 2011-02-09 01:04:36Z rusu $
+ *
+ */
+
+#include "pcl_ros/filters/extract_indices.hpp"
+
+pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options)
+: Filter("ExtractIndicesNode", options)
+{
+  rcl_interfaces::msg::ParameterDescriptor neg_desc;
+  neg_desc.name = "negative";
+  neg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  neg_desc.description = "Extract indices or the negative (all-indices)";
+  declare_parameter(neg_desc.name, rclcpp::ParameterValue(false), neg_desc);
+
+  // Validate initial values using same callback
+  callback_handle_ =
+    add_on_set_parameters_callback(
+    std::bind(&ExtractIndices::config_callback, this, std::placeholders::_1));
+
+  std::vector<std::string> param_names{neg_desc.name};
+  auto result = config_callback(get_parameters(param_names));
+  if (!result.successful) {
+    throw std::runtime_error(result.reason);
+  }
+  // TODO(daisukes): lazy subscription after rclcpp#2060
+  subscribe();
+}
+
+void
+pcl_ros::ExtractIndices::filter(
+  const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
+  PointCloud2 & output)
+{
+  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);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+rcl_interfaces::msg::SetParametersResult
+pcl_ros::ExtractIndices::config_callback(const std::vector<rclcpp::Parameter> & params)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const rclcpp::Parameter & param : params) {
+    if (param.get_name() == "negative") {
+      // Check the current value for the negative flag
+      if (impl_.getNegative() != param.as_bool()) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the filter negative flag to: %s.",
+          param.as_bool() ? "true" : "false");
+        // Call the virtual method in the child
+        impl_.setNegative(param.as_bool());
+      }
+    }
+  }
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  return result;
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ExtractIndices)

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/boundary.cpp

@@ -0,0 +1,78 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: boundary.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/boundary.hpp"
+
+void
+pcl_ros::BoundaryEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+void
+pcl_ros::BoundaryEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  impl_.setInputNormals(normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+typedef pcl_ros::BoundaryEstimation BoundaryEstimation;
+PLUGINLIB_EXPORT_CLASS(BoundaryEstimation, nodelet::Nodelet);

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/feature.cpp

@@ -0,0 +1,592 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
+ *
+ */
+
+// #include <pluginlib/class_list_macros.h>
+// Include the implementations here instead of compiling them separately to speed up compile time
+// #include "normal_3d.cpp"
+// #include "boundary.cpp"
+// #include "fpfh.cpp"
+// #include "fpfh_omp.cpp"
+// #include "moment_invariants.cpp"
+// #include "normal_3d_omp.cpp"
+// #include "normal_3d_tbb.cpp"
+// #include "pfh.cpp"
+// #include "principal_curvatures.cpp"
+// #include "vfh.cpp"
+#include <pcl/common/io.h>
+#include <message_filters/null_types.h>
+#include <vector>
+#include "pcl_ros/features/feature.hpp"
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::onInit()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit();
+
+  // Call the child init
+  childInit(*pnh_);
+
+  // Allow each individual class that inherits from us to declare their own Publisher
+  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
+  // PointCloud<Normal>, etc
+  // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
+      "parameters before continuing.",
+      getName().c_str());
+    return;
+  }
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
+    return;
+  }
+
+  // ---[ Optional parameters
+  pnh_->getParam("use_surface", use_surface_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &Feature::config_callback, this, _1, _2);
+  srv_->setCallback(f);
+
+  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
+  if (use_indices_ || use_surface_) {
+    // Create the objects here
+    if (approximate_sync_) {
+      sync_input_surface_indices_a_ =
+        boost::make_shared<message_filters::Synchronizer<
+            sync_policies::ApproximateTime<
+              PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
+    } else {
+      sync_input_surface_indices_e_ =
+        boost::make_shared<message_filters::Synchronizer<
+            sync_policies::ExactTime<
+              PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
+    }
+
+    // Subscribe to the input using a filter
+    sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
+    if (use_indices_) {
+      // If indices are enabled, subscribe to the indices
+      sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
+      if (use_surface_) {   // Use both indices and surface
+        // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
+        // and register
+        sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, sub_surface_filter_,
+            sub_indices_filter_);
+        }
+      } else {              // Use only indices
+        sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
+        // surface not enabled, connect the input-indices duo and register
+        if (approximate_sync_) {
+          sync_input_surface_indices_a_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
+          sync_input_surface_indices_e_->connectInput(
+            sub_input_filter_, nf_pc_,
+            sub_indices_filter_);
+        }
+      }
+    } else {                // Use only surface
+      sub_input_filter_.registerCallback(bind(&Feature::input_callback, this, _1));
+      // indices not enabled, connect the input-surface duo and register
+      sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+      if (approximate_sync_) {
+        sync_input_surface_indices_a_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
+      } else {
+        sync_input_surface_indices_e_->connectInput(sub_input_filter_, sub_surface_filter_, nf_pi_);
+      }
+    }
+    // Register callbacks
+    if (approximate_sync_) {
+      sync_input_surface_indices_a_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    } else {
+      sync_input_surface_indices_e_->registerCallback(
+        bind(
+          &Feature::input_surface_indices_callback,
+          this, _1, _2, _3));
+    }
+  } else {
+    // Subscribe in an old fashion to input only (no filters)
+    sub_input_ =
+      pnh_->subscribe<PointCloudIn>(
+      "input", max_queue_size_,
+      bind(
+        &Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr(),
+        PointIndicesConstPtr()));
+  }
+
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
+    " - use_surface    : %s\n"
+    " - k_search       : %d\n"
+    " - radius_search  : %f\n"
+    " - spatial_locator: %d",
+    getName().c_str(),
+    (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::config_callback(FeatureConfig & config, uint32_t level)
+{
+  if (k_ != config.k_search) {
+    k_ = config.k_search;
+    NODELET_DEBUG(
+      "[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
+  }
+  if (search_radius_ != config.radius_search) {
+    search_radius_ = config.radius_search;
+    NODELET_DEBUG(
+      "[config_callback] Setting the nearest neighbors search radius for each point: %f.",
+      search_radius_);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Feature::input_surface_indices_callback(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers() <= 0) {
+    return;
+  }
+
+  // If cloud is given, check if it's valid
+  if (!isValid(cloud)) {
+    NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If surface is given, check if it's valid
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
+    NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input surface!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices are given, check if they are valid
+  if (indices && !isValid(indices)) {
+    NODELET_ERROR("[%s::input_surface_indices_callback] Invalid input indices!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  /// DEBUG
+  if (cloud_surface) {
+    if (indices) {
+      NODELET_DEBUG(
+        "[input_surface_indices_callback]\n"
+        "                                         - PointCloud with %d data points (%s), stamp %f, "
+        "and frame %s on topic %s received.\n"
+        "                                         - PointCloud with %d data points (%s), stamp %f, "
+        "and frame %s on topic %s received.\n"
+        "                                         - PointIndices with %zu values, stamp %f, "
+        "and frame %s on topic %s received.",
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+        cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        indices->indices.size(), indices->header.stamp.toSec(),
+        indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+    } else {
+      NODELET_DEBUG(
+        "[input_surface_indices_callback]\n"
+        "                                 - PointCloud with %d data points (%s), stamp %f, and "
+        "frame %s on topic %s received.\n"
+        "                                 - PointCloud with %d data points (%s), stamp %f, and "
+        "frame %s on topic %s received.",
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+        cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str());
+    }
+  } else if (indices) {
+    NODELET_DEBUG(
+      "[input_surface_indices_callback]\n"
+      "                                 - PointCloud with %d data points (%s), stamp %f, and "
+      "frame %s on topic %s received.\n"
+      "                                 - PointIndices with %zu values, stamp %f, and "
+      "frame %s on topic %s received.",
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+      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("indices").c_str());
+  } else {
+    NODELET_DEBUG(
+      "[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and "
+      "frame %s on topic %s received.", cloud->width * cloud->height,
+      cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str());
+  }
+  ///
+
+
+  if (static_cast<int>(cloud->width * cloud->height) < k_) {
+    NODELET_ERROR(
+      "[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger "
+      "than the PointCloud size (%d)!", k_,
+      (int)(cloud->width * cloud->height));
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices given...
+  IndicesPtr vindices;
+  if (indices && !indices->header.frame_id.empty()) {
+    vindices.reset(new std::vector<int>(indices->indices));
+  }
+
+  computePublish(cloud, cloud_surface, vindices);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::onInit()
+{
+  // Call the super onInit ()
+  PCLNodelet::onInit();
+
+  // Call the child init
+  childInit(*pnh_);
+
+  // Allow each individual class that inherits from us to declare their own Publisher
+  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
+  // PointCloud<Normal>, etc
+  // pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
+
+  // ---[ Mandatory parameters
+  if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
+      "parameters before continuing.",
+      getName().c_str());
+    return;
+  }
+  if (!pnh_->getParam("spatial_locator", spatial_locator_type_)) {
+    NODELET_ERROR(
+      "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
+      getName().c_str());
+    return;
+  }
+  // ---[ Optional parameters
+  pnh_->getParam("use_surface", use_surface_);
+
+  sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
+  sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
+
+  // Enable the dynamic reconfigure service
+  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig>>(*pnh_);
+  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind(
+    &FeatureFromNormals::config_callback, this, _1, _2);
+  srv_->setCallback(f);
+
+  // Create the objects here
+  if (approximate_sync_) {
+    sync_input_normals_surface_indices_a_ =
+      boost::make_shared<message_filters::Synchronizer<
+          sync_policies::ApproximateTime<
+            PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
+  } else {
+    sync_input_normals_surface_indices_e_ =
+      boost::make_shared<message_filters::Synchronizer<
+          sync_policies::ExactTime<
+            PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
+  }
+
+  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
+  if (use_indices_ || use_surface_) {
+    if (use_indices_) {
+      // If indices are enabled, subscribe to the indices
+      sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
+      if (use_surface_) {   // Use both indices and surface
+        // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
+        // and register
+        sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+        if (approximate_sync_) {
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
+        } else {
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_,
+            sub_surface_filter_,
+            sub_indices_filter_);
+        }
+      } else {              // Use only indices
+        sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
+        if (approximate_sync_) {
+          // surface not enabled, connect the input-indices duo and register
+          sync_input_normals_surface_indices_a_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
+        } else {
+          // surface not enabled, connect the input-indices duo and register
+          sync_input_normals_surface_indices_e_->connectInput(
+            sub_input_filter_,
+            sub_normals_filter_, nf_pc_,
+            sub_indices_filter_);
+        }
+      }
+    } else {                // Use only surface
+      // indices not enabled, connect the input-surface duo and register
+      sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
+
+      sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
+      if (approximate_sync_) {
+        sync_input_normals_surface_indices_a_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
+      } else {
+        sync_input_normals_surface_indices_e_->connectInput(
+          sub_input_filter_, sub_normals_filter_,
+          sub_surface_filter_, nf_pi_);
+      }
+    }
+  } else {
+    sub_input_filter_.registerCallback(bind(&FeatureFromNormals::input_callback, this, _1));
+
+    if (approximate_sync_) {
+      sync_input_normals_surface_indices_a_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    } else {
+      sync_input_normals_surface_indices_e_->connectInput(
+        sub_input_filter_, sub_normals_filter_,
+        nf_pc_, nf_pi_);
+    }
+  }
+
+  // Register callbacks
+  if (approximate_sync_) {
+    sync_input_normals_surface_indices_a_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  } else {
+    sync_input_normals_surface_indices_e_->registerCallback(
+      bind(
+        &FeatureFromNormals::
+        input_normals_surface_indices_callback, this, _1, _2, _3, _4));
+  }
+
+  NODELET_DEBUG(
+    "[%s::onInit] Nodelet successfully created with the following parameters:\n"
+    " - use_surface    : %s\n"
+    " - k_search       : %d\n"
+    " - radius_search  : %f\n"
+    " - spatial_locator: %d",
+    getName().c_str(),
+    (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
+  const PointCloudInConstPtr & cloud, const PointCloudNConstPtr & cloud_normals,
+  const PointCloudInConstPtr & cloud_surface, const PointIndicesConstPtr & indices)
+{
+  // No subscribers, no work
+  if (pub_output_.getNumSubscribers() <= 0) {
+    return;
+  }
+
+  // If cloud+normals is given, check if it's valid
+  if (!isValid(cloud)) {  // || !isValid (cloud_normals, "normals"))
+    NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If surface is given, check if it's valid
+  if (cloud_surface && !isValid(cloud_surface, "surface")) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input surface!",
+      getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices are given, check if they are valid
+  if (indices && !isValid(indices)) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Invalid input indices!",
+      getName().c_str());
+    emptyPublish(cloud);
+    return;
+  }
+
+  /// DEBUG
+  if (cloud_surface) {
+    if (indices) {
+      NODELET_DEBUG(
+        "[%s::input_normals_surface_indices_callback]\n"
+        "                                                 - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                                 - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                                 - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                                 - PointIndices with %zu values, "
+        "stamp %f, and frame %s on topic %s received.",
+        getName().c_str(),
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+        cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+          *cloud_normals).c_str(),
+        cloud_normals->header.stamp.toSec(),
+        cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(),
+        indices->indices.size(), indices->header.stamp.toSec(),
+        indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+    } else {
+      NODELET_DEBUG(
+        "[%s::input_normals_surface_indices_callback]\n"
+        "                                         - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                         - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.\n"
+        "                                         - PointCloud with %d data points (%s), "
+        "stamp %f, and frame %s on topic %s received.",
+        getName().c_str(),
+        cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+        cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+          "input").c_str(),
+        cloud_surface->width * cloud_surface->height, pcl::getFieldsList(
+          *cloud_surface).c_str(),
+        cloud_surface->header.stamp.toSec(),
+        cloud_surface->header.frame_id.c_str(), pnh_->resolveName("surface").c_str(),
+        cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+          *cloud_normals).c_str(),
+        cloud_normals->header.stamp.toSec(),
+        cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str());
+    }
+  } else if (indices) {
+    NODELET_DEBUG(
+      "[%s::input_normals_surface_indices_callback]\n"
+      "                                         - PointCloud with %d data points (%s), "
+      "stamp %f, and frame %s on topic %s received.\n"
+      "                                         - PointCloud with %d data points (%s), "
+      "stamp %f, and frame %s on topic %s received.\n"
+      "                                         - PointIndices with %zu values, "
+      "stamp %f, and frame %s on topic %s received.",
+      getName().c_str(),
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+      cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(),
+      cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
+      pnh_->resolveName("normals").c_str(),
+      indices->indices.size(), indices->header.stamp.toSec(),
+      indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
+  } else {
+    NODELET_DEBUG(
+      "[%s::input_normals_surface_indices_callback]\n"
+      "                                 - PointCloud with %d data points (%s), stamp %f, and "
+      "frame %s on topic %s received.\n"
+      "                                 - PointCloud with %d data points (%s), stamp %f, and "
+      "frame %s on topic %s received.",
+      getName().c_str(),
+      cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
+      cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
+        "input").c_str(),
+      cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
+        *cloud_normals).c_str(),
+      cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
+      pnh_->resolveName("normals").c_str());
+  }
+  ///
+
+  if (static_cast<int>(cloud->width * cloud->height) < k_) {
+    NODELET_ERROR(
+      "[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) "
+      "is larger than the PointCloud size (%d)!",
+      getName().c_str(), k_, (int)(cloud->width * cloud->height));
+    emptyPublish(cloud);
+    return;
+  }
+
+  // If indices given...
+  IndicesPtr vindices;
+  if (indices && !indices->header.frame_id.empty()) {
+    vindices.reset(new std::vector<int>(indices->indices));
+  }
+
+  computePublish(cloud, cloud_normals, cloud_surface, vindices);
+}

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: fpfh.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/fpfh.hpp"
+
+void
+pcl_ros::FPFHEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+void
+pcl_ros::FPFHEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  impl_.setInputNormals(normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+typedef pcl_ros::FPFHEstimation FPFHEstimation;
+PLUGINLIB_EXPORT_CLASS(FPFHEstimation, nodelet::Nodelet);

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/fpfh_omp.cpp

@@ -0,0 +1,79 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: fpfh_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/fpfh_omp.hpp"
+
+void
+pcl_ros::FPFHEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+void
+pcl_ros::FPFHEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudNConstPtr & normals,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  impl_.setInputNormals(normals);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+typedef pcl_ros::FPFHEstimationOMP FPFHEstimationOMP;
+PLUGINLIB_EXPORT_CLASS(FPFHEstimationOMP, nodelet::Nodelet);

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/moment_invariants.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: moment_invariants.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/moment_invariants.hpp"
+
+void
+pcl_ros::MomentInvariantsEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+void
+pcl_ros::MomentInvariantsEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+typedef pcl_ros::MomentInvariantsEstimation MomentInvariantsEstimation;
+PLUGINLIB_EXPORT_CLASS(MomentInvariantsEstimation, nodelet::Nodelet);

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: normal_3d.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d.hpp"
+
+void
+pcl_ros::NormalEstimation::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+void
+pcl_ros::NormalEstimation::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+typedef pcl_ros::NormalEstimation NormalEstimation;
+PLUGINLIB_EXPORT_CLASS(NormalEstimation, nodelet::Nodelet);

ros2_ws/src/perception_pcl/pcl_ros/src/pcl_ros/filters/features/normal_3d_omp.cpp

@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNERff 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.
+ *
+ * $Id: normal_3d_omp.cpp 35361 2011-01-20 04:34:49Z rusu $
+ *
+ */
+
+#include <pluginlib/class_list_macros.h>
+#include "pcl_ros/features/normal_3d_omp.hpp"
+
+void
+pcl_ros::NormalEstimationOMP::emptyPublish(const PointCloudInConstPtr & cloud)
+{
+  PointCloudOut output;
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+void
+pcl_ros::NormalEstimationOMP::computePublish(
+  const PointCloudInConstPtr & cloud,
+  const PointCloudInConstPtr & surface,
+  const IndicesPtr & indices)
+{
+  // Set the parameters
+  impl_.setKSearch(k_);
+  impl_.setRadiusSearch(search_radius_);
+  // Initialize the spatial locator
+  initTree(spatial_locator_type_, tree_, k_);
+  impl_.setSearchMethod(tree_);
+
+  // Set the inputs
+  impl_.setInputCloud(cloud);
+  impl_.setIndices(indices);
+  impl_.setSearchSurface(surface);
+  // Estimate the feature
+  PointCloudOut output;
+  impl_.compute(output);
+
+  // Publish a Boost shared ptr const data
+  // Enforce that the TF frame and the timestamp are copied
+  output.header = cloud->header;
+  pub_output_.publish(output.makeShared());
+}
+
+typedef pcl_ros::NormalEstimationOMP NormalEstimationOMP;
+PLUGINLIB_EXPORT_CLASS(NormalEstimationOMP, nodelet::Nodelet);