Feature/pcl type adapter

dependency.repos

@@ -17,4 +17,8 @@ repositories:
   color_names:
     type: git
     url: https://github.com/OUXT-Polaris/color_names.git
-    version: master
+    version: master
+  pcl_type_adapter:
+    type: git
+    url: https://github.com/OUXT-Polaris/pcl_type_adapter.git
+    version: master

pcl_apps/CMakeLists.txt

@@ -25,43 +25,13 @@ else()
 endif()
 
 # find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(color_names REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(image_geometry REQUIRED)
-find_package(message_filters REQUIRED)
-find_package(message_synchronizer REQUIRED)
-find_package(ndt_omp REQUIRED)
-find_package(pcl_apps_msgs REQUIRED)
-find_package(pcl_conversions REQUIRED)
-find_package(perception_msgs REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(tf2_eigen REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(tf2_sensor_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
 find_package(PCL REQUIRED)
 find_package(Boost REQUIRED system)
 
-if(${tf2_eigen_VERSION} VERSION_LESS 0.18.0)
-  add_compile_definitions(USE_TF2_EIGEN_DEPRECATED_HEADER)
-endif()
-
-if(${tf2_sensor_msgs_VERSION} VERSION_LESS 0.18.0)
-  add_compile_definitions(USE_TF2_SENSOR_MSGS_DEPRECATED_HEADER)
-endif()
-
-if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0)
-  add_compile_definitions(USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER)
-endif()
-
-if ($ENV{ROS_DISTRO} STREQUAL "humble")
-  add_compile_definitions(HUMBLE)
-endif()
+add_compile_definitions(HUMBLE)
 
 include_directories(
   include
@@ -74,88 +44,66 @@ link_directories(
 )
 add_definitions(${PCL_DEFINITIONS})
 
+function(add_pcl_apps_component module component_name component_name_in_macro)
+  ament_auto_add_library(${component_name}_component SHARED
+  src/${module}/${component_name}/${component_name}_component.cpp
+  )
+  target_compile_definitions(${component_name}_component PRIVATE "PCL_APPS_${component_name_in_macro}_BUILDING_DLL")
+  target_link_libraries(${component_name}_component
+    boost_system ${PCL_LIBRARIES})
+
+  ament_auto_add_executable(${component_name}_node
+    src/${module}/${component_name}/${component_name}_node.cpp)
+  target_link_libraries(${component_name}_node
+    ${component_name}_component ${PCL_LIBRARIES})
+
+  install(TARGETS ${component_name}_component
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin)
+  install(TARGETS ${component_name}_node DESTINATION lib/pcl_apps)
+endfunction(add_pcl_apps_component)
+
+
 # Filter Modules
-add_subdirectory(src/filter)
+add_pcl_apps_component(filter points_concatenate POINTS_CONCATENATE)
+rclcpp_components_register_nodes(points_concatenate_component "pcl_apps::PointsConcatenateComponent")
+add_pcl_apps_component(filter points_transform POINTS_TRANSFORM)
+rclcpp_components_register_nodes(points_transform_component "pcl_apps::PointsTransformComponent")
+add_pcl_apps_component(filter radius_outlier_removal RADIUS_OUTLIER_REMOVAL RadiusOutlierRemovalComponent)
+rclcpp_components_register_nodes(radius_outlier_removal_component "pcl_apps::RadiusOutlierRemovalComponent")
+add_pcl_apps_component(filter crop_hull_filter CROP_HULL_FILTER)
+rclcpp_components_register_nodes(crop_hull_filter_component "pcl_apps::CropHullFilterComponent")
+add_pcl_apps_component(filter crop_box_filter CROP_BOX_FILTER)
+rclcpp_components_register_nodes(crop_box_filter_component "pcl_apps::CropBoxFilterComponent")
+add_pcl_apps_component(filter pointcloud_to_laserscan POINTCLOUD_TO_LASERSCAN PointCloudToLaserScanComponent)
+rclcpp_components_register_nodes(pointcloud_to_laserscan_component "pcl_apps::PointCloudToLaserScanComponent")
+add_pcl_apps_component(filter voxelgrid_filter VOEXLGRID_FILTER VoxelgridFilterComponent)
+rclcpp_components_register_nodes(voxelgrid_filter_component "pcl_apps::VoxelgridFilterComponent")
 
 # Matching Modules
-add_subdirectory(src/matching)
+add_pcl_apps_component(matching ndt_matching NDT_MATCHING)
+rclcpp_components_register_nodes(ndt_matching_component "pcl_apps::NdtMatchingComponent")
+add_pcl_apps_component(matching ndt_matching_twist_estimator NDT_MATCHING_TWIST_ESTIMATOR)
+rclcpp_components_register_nodes(ndt_matching_twist_estimator_component "pcl_apps::NdtMatchingTwistEstimatorComponent")
 
 # IO Module
-add_subdirectory(src/io)
+add_pcl_apps_component(io pcd_writer PCD_WRITER)
+rclcpp_components_register_nodes(pcd_writer_component "pcl_apps::PcdWriterComponent")
+add_pcl_apps_component(io pcd_loader PCD_LOADER PcdLoader)
+rclcpp_components_register_nodes(pcd_loader_component "pcl_apps::PcdLoaderComponent")
 
 # Clustering Module
-add_subdirectory(src/clustering)
+add_pcl_apps_component(clustering euclidean_clustering EUCLIDEAN_CLUSTERING)
+rclcpp_components_register_nodes(pcd_loader_component "pcl_apps::EuclideanClusteringComponent")
 
 # Projection Module
-add_subdirectory(src/projection)
-
-rclcpp_components_register_nodes(points_concatenate_component
-  "pcl_apps::PointsConcatenateComponent")
-
-rclcpp_components_register_nodes(points_transform_component
-  "pcl_apps::PointsTransformComponent")
-
-rclcpp_components_register_nodes(voxelgrid_filter_component
-  "pcl_apps::VoxelgridFilterComponent")
-
-rclcpp_components_register_nodes(ndt_matching_component
-  "pcl_apps::NdtMatchingComponent")
-
-rclcpp_components_register_nodes(ndt_matching_twist_estimator_component
-  "pcl_apps::NdtMatchingTwistEstimatorComponent")
-
-rclcpp_components_register_nodes(pcd_writer_component
-  "pcl_apps::PcdWriterComponent")
-
-rclcpp_components_register_nodes(pcd_loader_component
-  "pcl_apps::PcdLoaderComponent")
-
-rclcpp_components_register_nodes(radius_outlier_removal_component
-  "pcl_apps::RadiusOutlierRemovalComponent")
-
-rclcpp_components_register_nodes(crop_hull_filter_component
-  "pcl_apps::CropHullFilterComponent")
-
-rclcpp_components_register_nodes(crop_box_filter_component
-  "pcl_apps::CropBoxFilterComponent")
-
-rclcpp_components_register_nodes(euclidean_clustering_component
-  "pcl_apps::EuclideanClusteringComponent")
-
-rclcpp_components_register_nodes(pointcloud_to_laserscan_component
-  "pcl_apps::PointCloudToLaserScanComponent")
-
-rclcpp_components_register_nodes(pointcloud_projection_component
-  "pcl_apps::PointCloudProjectionComponent")
-
-# Install header files
-install(
-  DIRECTORY "include/"
-  DESTINATION include
-)
+add_pcl_apps_component(projection pointcloud_projection POINTCLOUD_PROJECTION)
+rclcpp_components_register_nodes(pointcloud_projection_component "pcl_apps::PointCloudProjectionComponent")
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-# export
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(rclcpp)
-ament_export_dependencies(rclcpp_components)
-ament_export_dependencies(pcl_conversions)
-ament_export_dependencies(tf2_ros)
-ament_export_dependencies(sensor_msgs)
-ament_export_dependencies(geometry_msgs)
-ament_export_dependencies(pcl_apps_msgs)
-ament_export_dependencies(rosidl_default_runtime)
-ament_export_dependencies(ndt_omp)
-ament_export_include_directories(include)
-
-ament_package()
+ament_auto_package()

pcl_apps/include/pcl_apps/adapter.hpp

@@ -0,0 +1,26 @@
+// Copyright (c) 2019 OUXT Polaris
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <pcl_type_adapter/pcl_type_adapter.hpp>
+
+namespace pcl_apps
+{
+using PCLPointType = pcl::PointXYZI;
+using PCLPointCloudType = pcl::PointCloud<PCLPointType>;
+using PCLPointCloudTypePtr = std::shared_ptr<PCLPointCloudType>;
+using PointCloudAdapterType =
+  rclcpp::TypeAdapter<PCLPointCloudTypePtr, sensor_msgs::msg::PointCloud2>;
+using PointCloudPublisher = std::shared_ptr<rclcpp::Publisher<PointCloudAdapterType>>;
+using PointCloudSubscriber = std::shared_ptr<rclcpp::Subscription<PointCloudAdapterType>>;
+}  // namespace pcl_apps

pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp

@@ -54,8 +54,7 @@ extern "C" {
 #endif
 
 // Headers in ROS2
-#include <pcl_conversions/pcl_conversions.h>
-
+#include <pcl_apps/adapter.hpp>
 #include <pcl_apps_msgs/msg/point_cloud_array.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -80,7 +79,7 @@ class EuclideanClusteringComponent : public rclcpp::Node
   explicit EuclideanClusteringComponent(const rclcpp::NodeOptions & options);
 
 private:
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
+  PointCloudSubscriber sub_;
   rclcpp::Publisher<pcl_apps_msgs::msg::PointCloudArray>::SharedPtr pub_;
   std::string input_topic_;
   double cluster_tolerance_;

pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp

@@ -54,6 +54,7 @@ extern "C" {
 #endif
 
 #include <memory>
+#include <pcl_apps/adapter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
@@ -66,12 +67,12 @@ class CropBoxFilterComponent : public rclcpp::Node
   explicit CropBoxFilterComponent(const rclcpp::NodeOptions & options);
 
 private:
-  void pointsCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+  void pointsCallback(const PCLPointCloudTypePtr & msg);
   double max_x_, max_y_, max_z_;
   double min_x_, min_y_, min_z_;
   bool keep_organized_, negative_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
+  PointCloudPublisher pub_;
+  PointCloudSubscriber sub_;
 };
 }  // namespace pcl_apps
 

pcl_apps/include/pcl_apps/filter/crop_hull_filter/crop_hull_filter_component.hpp

@@ -59,14 +59,15 @@ extern "C" {
 #include <pcl/filters/crop_hull.h>
 
 #include <memory>
+#include <pcl_apps/adapter.hpp>
 #include <pcl_apps_msgs/msg/point_cloud_array.hpp>
 #include <pcl_apps_msgs/msg/polygon_array.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 namespace pcl_apps
 {
 typedef message_filters::Subscriber<pcl_apps_msgs::msg::PolygonArray> PolygonSubscriber;
-typedef message_filters::Subscriber<sensor_msgs::msg::PointCloud2> PointCloudSubscriber;
+typedef message_filters::Subscriber<sensor_msgs::msg::PointCloud2> ROS2PointCloudSubscriber;
 class CropHullFilterComponent : public rclcpp::Node
 {
 public:
@@ -82,7 +83,7 @@ class CropHullFilterComponent : public rclcpp::Node
     const sensor_msgs::msg::PointCloud2::ConstSharedPtr point_cloud,
     const pcl_apps_msgs::msg::PolygonArray::ConstSharedPtr polygon);
   std::shared_ptr<PolygonSubscriber> polygon_sub_;
-  std::shared_ptr<PointCloudSubscriber> pointcloud_sub_;
+  std::shared_ptr<ROS2PointCloudSubscriber> pointcloud_sub_;
 };
 }  // namespace pcl_apps
 

pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp

@@ -53,6 +53,7 @@ extern "C" {
 }  // extern "C"
 #endif
 
+#include <pcl_apps/adapter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -66,8 +67,8 @@ class PointCloudToLaserScanComponent : public rclcpp::Node
   explicit PointCloudToLaserScanComponent(const rclcpp::NodeOptions & options);
 
 private:
-  void callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
+  void pointsCallback(const PCLPointCloudTypePtr & msg);
+  PointCloudSubscriber sub_;
   rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_;
   double min_height_;
   double max_height_;

pcl_apps/include/pcl_apps/filter/points_concatenate/points_concatenate_component.hpp

@@ -54,9 +54,8 @@ extern "C" {
 #endif
 
 // Headers in ROS2
-#include <pcl_conversions/pcl_conversions.h>
-
 #include <message_synchronizer/message_synchronizer.hpp>
+#include <pcl_apps/adapter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
@@ -103,7 +102,7 @@ class PointsConcatenateComponent : public rclcpp::Node
   std::array<boost::shared_ptr<PointCloudSubsciber>, 8> sub_ptrs_;
   message_filters::PassThrough<PointCloud2> nf_;
   */
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
+  PointCloudPublisher pub_;
   Sync2PtrT sync2_;
   Sync3PtrT sync3_;
   Sync4PtrT sync4_;
@@ -112,4 +111,4 @@ class PointsConcatenateComponent : public rclcpp::Node
 };
 }  // namespace pcl_apps
 
-#endif  // PCL_APPS__FILTER__POINTS_CONCATENATE__POINTS_CONCATENATE_COMPONENT_HPP_
+#endif  // PCL_APPS__FILTER__POINTS_CONCATENATE__POINTS_CONCATENATE_COMPONENT_HPP_

pcl_apps/include/pcl_apps/filter/points_transform/points_transform_component.hpp

@@ -54,7 +54,7 @@ extern "C" {
 #endif
 
 // Headers in ROS2
-#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_apps/adapter.hpp>
 
 #ifdef USE_TF2_EIGEN_DEPRECATED_HEADER
 #include <tf2_eigen/tf2_eigen.h>
@@ -89,8 +89,8 @@ class PointsTransformComponent : public rclcpp::Node
   rclcpp::Clock ros_clock_;
   tf2_ros::Buffer buffer_;
   tf2_ros::TransformListener listener_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
+  PointCloudSubscriber sub_;
+  PointCloudPublisher pub_;
   std::string input_topic_;
 };
 }  // namespace pcl_apps

pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp

@@ -53,9 +53,7 @@ extern "C" {
 }  // extern "C"
 #endif
 
-// Headers in ROS2
-#include <pcl_conversions/pcl_conversions.h>
-
+#include <pcl_apps/adapter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
@@ -76,9 +74,9 @@ class RadiusOutlierRemovalComponent : public rclcpp::Node
 
 private:
   std::string input_topic_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
-  pcl::RadiusOutlierRemoval<pcl::PointXYZI> filter_;
+  PointCloudSubscriber sub_;
+  PointCloudPublisher pub_;
+  pcl::RadiusOutlierRemoval<PCLPointType> filter_;
   double search_radius_;
   int min_neighbors_in_search_radius_;
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_handler_ptr_;