Feature/type adapter

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

@@ -53,21 +53,22 @@ extern "C" {
 }  // extern "C"
 #endif
 
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/exact_time.h>
-#include <message_filters/time_synchronizer.h>
 #include <pcl/filters/crop_hull.h>
 
 #include <memory>
+#include <message_synchronizer/message_synchronizer.hpp>
 #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> ROS2PointCloudSubscriber;
+using PointCloudType = std::shared_ptr<pcl::PointCloud<pcl::PointXYZI>>;
+using AdapterType = rclcpp::TypeAdapter<PointCloudType, sensor_msgs::msg::PointCloud2>;
+using Sync2T =
+  message_synchronizer::MessageSynchronizer2<AdapterType, pcl_apps_msgs::msg::PolygonArray>;
+
 class CropHullFilterComponent : public rclcpp::Node
 {
 public:
@@ -76,14 +77,10 @@ class CropHullFilterComponent : public rclcpp::Node
 
 private:
   rclcpp::Publisher<pcl_apps_msgs::msg::PointCloudArray>::SharedPtr pointcloud_pub_;
-  std::shared_ptr<message_filters::TimeSynchronizer<
-    sensor_msgs::msg::PointCloud2, pcl_apps_msgs::msg::PolygonArray>>
-    sync_;
+  std::shared_ptr<Sync2T> sync_;
   void callback(
-    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<ROS2PointCloudSubscriber> pointcloud_sub_;
+    const std::optional<PointCloudType> point_cloud,
+    const std::optional<pcl_apps_msgs::msg::PolygonArray> polygon);
 };
 }  // namespace pcl_apps
 

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

@@ -70,17 +70,14 @@ extern "C" {
 
 namespace pcl_apps
 {
-typedef sensor_msgs::msg::PointCloud2 PointCloud2;
-typedef std::shared_ptr<PointCloud2> PointCloud2Ptr;
-typedef const boost::optional<const PointCloud2Ptr> & CallbackT;
-typedef message_synchronizer::MessageSynchronizer2<PointCloud2, PointCloud2> Sync2T;
-typedef std::shared_ptr<Sync2T> Sync2PtrT;
-typedef message_synchronizer::MessageSynchronizer3<PointCloud2, PointCloud2, PointCloud2> Sync3T;
-typedef std::shared_ptr<Sync3T> Sync3PtrT;
-typedef message_synchronizer::MessageSynchronizer4<
-  PointCloud2, PointCloud2, PointCloud2, PointCloud2>
-  Sync4T;
-typedef std::shared_ptr<Sync4T> Sync4PtrT;
+using PointCloudType = std::shared_ptr<pcl::PointCloud<pcl::PointXYZI>>;
+using AdapterType = rclcpp::TypeAdapter<PointCloudType, sensor_msgs::msg::PointCloud2>;
+using CallbackT = const std::optional<PointCloudType>;
+
+using Sync2T = message_synchronizer::MessageSynchronizer2<AdapterType, AdapterType>;
+using Sync3T = message_synchronizer::MessageSynchronizer3<AdapterType, AdapterType, AdapterType>;
+using Sync4T =
+  message_synchronizer::MessageSynchronizer4<AdapterType, AdapterType, AdapterType, AdapterType>;
 
 class PointsConcatenateComponent : public rclcpp::Node
 {
@@ -92,20 +89,10 @@ class PointsConcatenateComponent : public rclcpp::Node
   void callback2(CallbackT in0, CallbackT in1);
   void callback3(CallbackT in0, CallbackT in1, CallbackT in2);
   void callback4(CallbackT in0, CallbackT in1, CallbackT in2, CallbackT in3);
-  /*
-  boost::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
-  void input(
-    const PointCloud2::SharedPtr & in0, const PointCloud2::SharedPtr & in1,
-    const PointCloud2::SharedPtr & in2, const PointCloud2::SharedPtr & in3,
-    const PointCloud2::SharedPtr & in4, const PointCloud2::SharedPtr & in5,
-    const PointCloud2::SharedPtr & in6, const PointCloud2::SharedPtr & in7);
-  std::array<boost::shared_ptr<PointCloudSubsciber>, 8> sub_ptrs_;
-  message_filters::PassThrough<PointCloud2> nf_;
-  */
   PointCloudPublisher pub_;
-  Sync2PtrT sync2_;
-  Sync3PtrT sync3_;
-  Sync4PtrT sync4_;
+  std::shared_ptr<Sync2T> sync2_;
+  std::shared_ptr<Sync3T> sync3_;
+  std::shared_ptr<Sync4T> sync4_;
   std::array<std::string, 8> input_topics_;
   int num_input_;
 };

pcl_apps/include/pcl_apps/projection/pointcloud_projection/pointcloud_projection_component.hpp

@@ -70,15 +70,15 @@ extern "C" {
 
 namespace pcl_apps
 {
-typedef sensor_msgs::msg::CameraInfo CameraInfoT;
-typedef std::shared_ptr<CameraInfoT> CameraInfoTPtr;
-typedef pcl_apps_msgs::msg::PointCloudArray PointCloudArrayT;
-typedef std::shared_ptr<PointCloudArrayT> PointCloudArrayTPtr;
+using CameraInfoT = sensor_msgs::msg::CameraInfo;
+using CameraInfoTPtr = std::shared_ptr<CameraInfoT>;
+using PointCloudArrayT = pcl_apps_msgs::msg::PointCloudArray;
+using PointCloudArrayTPtr = std::shared_ptr<PointCloudArrayT>;
 
-typedef message_synchronizer::MessageSynchronizer2<CameraInfoT, PointCloudArrayT>
-  CameraInfoAndPoints;
-typedef const boost::optional<const CameraInfoTPtr> & CameraInfoCallbackT;
-typedef const boost::optional<const PointCloudArrayTPtr> & PointCloudsCallbackT;
+using CameraInfoAndPoints =
+  message_synchronizer::MessageSynchronizer2<CameraInfoT, PointCloudArrayT>;
+using CameraInfoCallbackT = const std::optional<const CameraInfoT> &;
+using PointCloudsCallbackT = const std::optional<const PointCloudArrayT> &;
 
 class PointCloudProjectionComponent : public rclcpp::Node
 {

pcl_apps/package.xml

@@ -15,7 +15,6 @@
   <depend>image_geometry</depend>
   <depend>libboost-system-dev</depend>
   <depend>libpcl-all-dev</depend>
-  <depend>message_filters</depend>
   <depend>message_synchronizer</depend>
   <depend>ndt_omp</depend>
   <depend>pcl_apps_msgs</depend>

pcl_apps/src/filter/crop_hull_filter/crop_hull_filter_component.cpp

@@ -31,25 +31,25 @@ CropHullFilterComponent::CropHullFilterComponent(const rclcpp::NodeOptions & opt
 : Node("crop_hull_filter_node", options)
 {
   pointcloud_pub_ = this->create_publisher<pcl_apps_msgs::msg::PointCloudArray>("points_array", 1);
-  polygon_sub_ = std::shared_ptr<PolygonSubscriber>(new PolygonSubscriber(this, "polygon"));
-  pointcloud_sub_ =
-    std::shared_ptr<ROS2PointCloudSubscriber>(new ROS2PointCloudSubscriber(this, "points"));
-  sync_ = std::make_shared<message_filters::TimeSynchronizer<
-    sensor_msgs::msg::PointCloud2, pcl_apps_msgs::msg::PolygonArray>>(
-    *pointcloud_sub_, *polygon_sub_, 10);
+  sync_ = std::shared_ptr<Sync2T>(new Sync2T(
+    this, {"points", "polygon"}, std::chrono::milliseconds{100}, std::chrono::milliseconds{100},
+    rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>(),
+    [](const auto & data) { return pcl_conversions::fromPCL(data->header).stamp; },
+    [](const auto & data) { return data.header.stamp; }));
   sync_->registerCallback(std::bind(
     &CropHullFilterComponent::callback, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 void CropHullFilterComponent::callback(
-  const sensor_msgs::msg::PointCloud2::ConstSharedPtr point_cloud,
-  const pcl_apps_msgs::msg::PolygonArray::ConstSharedPtr polygon)
+  const std::optional<PointCloudType> pcl_cloud,
+  const std::optional<pcl_apps_msgs::msg::PolygonArray> polygon)
 {
+  if (!pcl_cloud || !polygon) {
+    return;
+  }
   pcl_apps_msgs::msg::PointCloudArray point_clouds;
-  point_clouds.header = point_cloud->header;
+  point_clouds.header = pcl_conversions::fromPCL(pcl_cloud.value()->header);
   pcl::CropHull<PCLPointType> filter;
-  pcl::PointCloud<PCLPointType>::Ptr pcl_cloud(new pcl::PointCloud<PCLPointType>());
-  pcl::fromROSMsg(*point_cloud, *pcl_cloud);
   for (auto poly_itr = polygon->polygon.begin(); poly_itr != polygon->polygon.end(); poly_itr++) {
     pcl::ConvexHull<PCLPointType> convex_hull;
     std::vector<pcl::Vertices> convex_hull_polygons;
@@ -70,7 +70,7 @@ void CropHullFilterComponent::callback(
     filter.setDim(2);
     filter.setCropOutside(false);
     pcl::PointCloud<PCLPointType>::Ptr filtered(new pcl::PointCloud<PCLPointType>());
-    filter.setInputCloud(pcl_cloud);
+    filter.setInputCloud(pcl_cloud.value());
     filter.filter(*filtered);
     sensor_msgs::msg::PointCloud2 filtered_msg;
     pcl::toROSMsg(*filtered, filtered_msg);

pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp

@@ -35,25 +35,34 @@ PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions
       "input_topic" + std::to_string(i), get_name() + std::string("/input") + std::to_string(i));
     get_parameter("input_topic" + std::to_string(i), input_topics_[i]);
   }
+  const auto get_timestamp = [](const auto & data) {
+    return pcl_conversions::fromPCL(data->header).stamp;
+  };
   if (num_input_ == 2) {
     sync2_ = std::shared_ptr<Sync2T>(new Sync2T(
       this, {input_topics_[0], input_topics_[1]}, std::chrono::milliseconds{100},
-      std::chrono::milliseconds{100}));
+      std::chrono::milliseconds{100},
+      rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>(), get_timestamp,
+      get_timestamp));
     auto func2 = std::bind(
       &PointsConcatenateComponent::callback2, this, std::placeholders::_1, std::placeholders::_2);
     sync2_->registerCallback(func2);
   } else if (num_input_ == 3) {
     sync3_ = std::shared_ptr<Sync3T>(new Sync3T(
       this, {input_topics_[0], input_topics_[1], input_topics_[2]}, std::chrono::milliseconds{100},
-      std::chrono::milliseconds{100}));
+      std::chrono::milliseconds{100},
+      rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>(), get_timestamp,
+      get_timestamp, get_timestamp));
     auto func3 = std::bind(
       &PointsConcatenateComponent::callback3, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3);
     sync3_->registerCallback(func3);
   } else if (num_input_ == 4) {
     sync4_ = std::shared_ptr<Sync4T>(new Sync4T(
       this, {input_topics_[0], input_topics_[1], input_topics_[2], input_topics_[3]},
-      std::chrono::milliseconds{100}, std::chrono::milliseconds{100}));
+      std::chrono::milliseconds{100}, std::chrono::milliseconds{100},
+      rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>(), get_timestamp,
+      get_timestamp, get_timestamp, get_timestamp));
     auto func4 = std::bind(
       &PointsConcatenateComponent::callback4, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3, std::placeholders::_4);
@@ -67,17 +76,11 @@ void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1)
   bool empty = true;
   if (in0) {
     empty = false;
-    const PointCloud2Ptr pc = in0.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in0.value();
   }
   if (in1) {
     empty = false;
-    const PointCloud2Ptr pc = in1.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in1.value() + *cloud;
   }
   if (!empty) {
     cloud->header.stamp = pcl_conversions::toPCL(sync2_->getPollTimestamp());
@@ -91,24 +94,15 @@ void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, Callbac
   bool empty = true;
   if (in0) {
     empty = false;
-    const PointCloud2Ptr pc = in0.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in0.value();
   }
   if (in1) {
     empty = false;
-    const PointCloud2Ptr pc = in1.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in1.value() + *cloud;
   }
   if (in2) {
     empty = false;
-    const PointCloud2Ptr pc = in2.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in2.value() + *cloud;
   }
   if (!empty) {
     cloud->header.stamp = pcl_conversions::toPCL(sync3_->getPollTimestamp());
@@ -123,31 +117,19 @@ void PointsConcatenateComponent::callback4(
   bool empty = true;
   if (in0) {
     empty = false;
-    const PointCloud2Ptr pc = in0.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in0.value();
   }
   if (in1) {
     empty = false;
-    const PointCloud2Ptr pc = in1.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in1.value() + *cloud;
   }
   if (in2) {
     empty = false;
-    const PointCloud2Ptr pc = in2.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in2.value() + *cloud;
   }
   if (in3) {
     empty = false;
-    const PointCloud2Ptr pc = in3.get();
-    pcl::PointCloud<pcl::PointXYZI>::Ptr pc_cloud(new pcl::PointCloud<pcl::PointXYZI>);
-    pcl::fromROSMsg(*pc, *pc_cloud);
-    *cloud = *pc_cloud + *cloud;
+    *cloud = *in3.value() + *cloud;
   }
   if (!empty) {
     cloud->header.stamp = pcl_conversions::toPCL(sync4_->getPollTimestamp());

pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp

@@ -125,24 +125,24 @@ void PointCloudProjectionComponent::callback(
     return;
   }
   image_geometry::PinholeCameraModel cam_model;
-  cam_model.fromCameraInfo(camera_info.get());
+  cam_model.fromCameraInfo(camera_info.value());
   geometry_msgs::msg::TransformStamped transform_stamped;
   try {
     transform_stamped = buffer_.lookupTransform(
-      camera_info.get()->header.frame_id, point_clouds.get()->header.frame_id,
-      point_clouds.get()->header.stamp, tf2::durationFromSec(1.0));
+      camera_info.value().header.frame_id, point_clouds.value().header.frame_id,
+      point_clouds.value().header.stamp, tf2::durationFromSec(1.0));
   } catch (tf2::ExtrapolationException & ex) {
     RCLCPP_ERROR(get_logger(), ex.what());
     return;
   }
   typedef boost::geometry::model::d2::point_xy<double> point;
   typedef boost::geometry::model::polygon<point> polygon_type;
   typedef boost::geometry::model::box<point> box;
-  box camera_bbox(point(0, 0), point(camera_info.get()->width, camera_info.get()->height));
+  box camera_bbox(point(0, 0), point(camera_info.value().width, camera_info.value().height));
   perception_msgs::msg::Detection2DArray detection_array;
-  detection_array.header.frame_id = camera_info.get()->header.frame_id;
-  detection_array.header.stamp = point_clouds.get()->header.stamp;
-  for (const auto & point_cloud : point_clouds.get()->cloud) {
+  detection_array.header.frame_id = camera_info.value().header.frame_id;
+  detection_array.header.stamp = point_clouds.value().header.stamp;
+  for (const auto & point_cloud : point_clouds.value().cloud) {
     polygon_type poly;
     typedef boost::geometry::ring_type<polygon_type>::type ring_type;
     ring_type & ring = boost::geometry::exterior_ring(poly);
@@ -164,11 +164,11 @@ void PointCloudProjectionComponent::callback(
     box out;
     if (boost::geometry::intersection(camera_bbox, bx, out)) {
       perception_msgs::msg::Detection2D detection;
-      detection.header.frame_id = camera_info.get()->header.frame_id;
-      detection.header.stamp = point_clouds.get()->header.stamp;
+      detection.header.frame_id = camera_info.value().header.frame_id;
+      detection.header.stamp = point_clouds.value().header.stamp;
       std_msgs::msg::Header bbox_header;
-      bbox_header.frame_id = point_clouds.get()->header.frame_id;
-      bbox_header.stamp = point_clouds.get()->header.stamp;
+      bbox_header.frame_id = point_clouds.value().header.frame_id;
+      bbox_header.stamp = point_clouds.value().header.stamp;
 #ifdef HUMBLE
       detection.bbox.center.position.x = (out.max_corner().x() + out.min_corner().x()) * 0.5;
       detection.bbox.center.position.y = (out.max_corner().y() + out.min_corner().y()) * 0.5;