diff --git a/pcl_apps/include/pcl_apps/filter/crop_hull_filter/crop_hull_filter_component.hpp b/pcl_apps/include/pcl_apps/filter/crop_hull_filter/crop_hull_filter_component.hpp index 0f9b76a..3ae4919 100644 --- a/pcl_apps/include/pcl_apps/filter/crop_hull_filter/crop_hull_filter_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/crop_hull_filter/crop_hull_filter_component.hpp @@ -53,12 +53,10 @@ extern "C" { } // extern "C" #endif -#include -#include -#include #include #include +#include #include #include #include @@ -66,8 +64,11 @@ extern "C" { namespace pcl_apps { -typedef message_filters::Subscriber PolygonSubscriber; -typedef message_filters::Subscriber ROS2PointCloudSubscriber; +using PointCloudType = std::shared_ptr>; +using AdapterType = rclcpp::TypeAdapter; +using Sync2T = + message_synchronizer::MessageSynchronizer2; + class CropHullFilterComponent : public rclcpp::Node { public: @@ -76,14 +77,10 @@ class CropHullFilterComponent : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pointcloud_pub_; - std::shared_ptr> - sync_; + std::shared_ptr sync_; void callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr point_cloud, - const pcl_apps_msgs::msg::PolygonArray::ConstSharedPtr polygon); - std::shared_ptr polygon_sub_; - std::shared_ptr pointcloud_sub_; + const std::optional point_cloud, + const std::optional polygon); }; } // namespace pcl_apps diff --git a/pcl_apps/include/pcl_apps/filter/points_concatenate/points_concatenate_component.hpp b/pcl_apps/include/pcl_apps/filter/points_concatenate/points_concatenate_component.hpp index 93a7c10..fc9e847 100644 --- a/pcl_apps/include/pcl_apps/filter/points_concatenate/points_concatenate_component.hpp +++ b/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 PointCloud2Ptr; -typedef const boost::optional & CallbackT; -typedef message_synchronizer::MessageSynchronizer2 Sync2T; -typedef std::shared_ptr Sync2PtrT; -typedef message_synchronizer::MessageSynchronizer3 Sync3T; -typedef std::shared_ptr Sync3PtrT; -typedef message_synchronizer::MessageSynchronizer4< - PointCloud2, PointCloud2, PointCloud2, PointCloud2> - Sync4T; -typedef std::shared_ptr Sync4PtrT; +using PointCloudType = std::shared_ptr>; +using AdapterType = rclcpp::TypeAdapter; +using CallbackT = const std::optional; + +using Sync2T = message_synchronizer::MessageSynchronizer2; +using Sync3T = message_synchronizer::MessageSynchronizer3; +using Sync4T = + message_synchronizer::MessageSynchronizer4; 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> 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, 8> sub_ptrs_; - message_filters::PassThrough nf_; - */ PointCloudPublisher pub_; - Sync2PtrT sync2_; - Sync3PtrT sync3_; - Sync4PtrT sync4_; + std::shared_ptr sync2_; + std::shared_ptr sync3_; + std::shared_ptr sync4_; std::array input_topics_; int num_input_; }; diff --git a/pcl_apps/include/pcl_apps/projection/pointcloud_projection/pointcloud_projection_component.hpp b/pcl_apps/include/pcl_apps/projection/pointcloud_projection/pointcloud_projection_component.hpp index 35fd37e..da6bf3a 100644 --- a/pcl_apps/include/pcl_apps/projection/pointcloud_projection/pointcloud_projection_component.hpp +++ b/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 CameraInfoTPtr; -typedef pcl_apps_msgs::msg::PointCloudArray PointCloudArrayT; -typedef std::shared_ptr PointCloudArrayTPtr; +using CameraInfoT = sensor_msgs::msg::CameraInfo; +using CameraInfoTPtr = std::shared_ptr; +using PointCloudArrayT = pcl_apps_msgs::msg::PointCloudArray; +using PointCloudArrayTPtr = std::shared_ptr; -typedef message_synchronizer::MessageSynchronizer2 - CameraInfoAndPoints; -typedef const boost::optional & CameraInfoCallbackT; -typedef const boost::optional & PointCloudsCallbackT; +using CameraInfoAndPoints = + message_synchronizer::MessageSynchronizer2; +using CameraInfoCallbackT = const std::optional &; +using PointCloudsCallbackT = const std::optional &; class PointCloudProjectionComponent : public rclcpp::Node { diff --git a/pcl_apps/package.xml b/pcl_apps/package.xml index d75e8d5..27df5aa 100644 --- a/pcl_apps/package.xml +++ b/pcl_apps/package.xml @@ -15,7 +15,6 @@ image_geometry libboost-system-dev libpcl-all-dev - message_filters message_synchronizer ndt_omp pcl_apps_msgs diff --git a/pcl_apps/src/filter/crop_hull_filter/crop_hull_filter_component.cpp b/pcl_apps/src/filter/crop_hull_filter/crop_hull_filter_component.cpp index bea8ca8..8240387 100644 --- a/pcl_apps/src/filter/crop_hull_filter/crop_hull_filter_component.cpp +++ b/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("points_array", 1); - polygon_sub_ = std::shared_ptr(new PolygonSubscriber(this, "polygon")); - pointcloud_sub_ = - std::shared_ptr(new ROS2PointCloudSubscriber(this, "points")); - sync_ = std::make_shared>( - *pointcloud_sub_, *polygon_sub_, 10); + sync_ = std::shared_ptr(new Sync2T( + this, {"points", "polygon"}, std::chrono::milliseconds{100}, std::chrono::milliseconds{100}, + rclcpp::SubscriptionOptionsWithAllocator>(), + [](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 pcl_cloud, + const std::optional 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 filter; - pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); - pcl::fromROSMsg(*point_cloud, *pcl_cloud); for (auto poly_itr = polygon->polygon.begin(); poly_itr != polygon->polygon.end(); poly_itr++) { pcl::ConvexHull convex_hull; std::vector convex_hull_polygons; @@ -70,7 +70,7 @@ void CropHullFilterComponent::callback( filter.setDim(2); filter.setCropOutside(false); pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); - filter.setInputCloud(pcl_cloud); + filter.setInputCloud(pcl_cloud.value()); filter.filter(*filtered); sensor_msgs::msg::PointCloud2 filtered_msg; pcl::toROSMsg(*filtered, filtered_msg); diff --git a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp index 6d10916..988556e 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -35,17 +35,24 @@ 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(new Sync2T( this, {input_topics_[0], input_topics_[1]}, std::chrono::milliseconds{100}, - std::chrono::milliseconds{100})); + std::chrono::milliseconds{100}, + rclcpp::SubscriptionOptionsWithAllocator>(), 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(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>(), get_timestamp, + get_timestamp, get_timestamp)); auto func3 = std::bind( &PointsConcatenateComponent::callback3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); @@ -53,7 +60,9 @@ PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions } else if (num_input_ == 4) { sync4_ = std::shared_ptr(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>(), 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::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in0.value(); } if (in1) { empty = false; - const PointCloud2Ptr pc = in1.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - 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::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in0.value(); } if (in1) { empty = false; - const PointCloud2Ptr pc = in1.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in1.value() + *cloud; } if (in2) { empty = false; - const PointCloud2Ptr pc = in2.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - 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::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in0.value(); } if (in1) { empty = false; - const PointCloud2Ptr pc = in1.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in1.value() + *cloud; } if (in2) { empty = false; - const PointCloud2Ptr pc = in2.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in2.value() + *cloud; } if (in3) { empty = false; - const PointCloud2Ptr pc = in3.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); - *cloud = *pc_cloud + *cloud; + *cloud = *in3.value() + *cloud; } if (!empty) { cloud->header.stamp = pcl_conversions::toPCL(sync4_->getPollTimestamp()); diff --git a/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp b/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp index 6f22c54..6271332 100644 --- a/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp +++ b/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp @@ -125,12 +125,12 @@ 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; @@ -138,11 +138,11 @@ void PointCloudProjectionComponent::callback( typedef boost::geometry::model::d2::point_xy point; typedef boost::geometry::model::polygon polygon_type; typedef boost::geometry::model::box 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::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;