From 63cddabcf8d033bb94216304b7709354b67c77e2 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 30 Apr 2024 22:17:17 +0900 Subject: [PATCH 1/6] fix compile errors Signed-off-by: hakuturu583 --- .../points_concatenate_component.hpp | 2 +- .../pointcloud_projection_component.hpp | 4 +-- .../points_concatenate_component.cpp | 36 +++++++++---------- .../pointcloud_projection_component.cpp | 22 ++++++------ 4 files changed, 32 insertions(+), 32 deletions(-) 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..8e02951 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 @@ -72,7 +72,7 @@ namespace pcl_apps { typedef sensor_msgs::msg::PointCloud2 PointCloud2; typedef std::shared_ptr PointCloud2Ptr; -typedef const boost::optional & CallbackT; +typedef const std::optional & CallbackT; typedef message_synchronizer::MessageSynchronizer2 Sync2T; typedef std::shared_ptr Sync2PtrT; typedef message_synchronizer::MessageSynchronizer3 Sync3T; 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..b9e0639 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 @@ -77,8 +77,8 @@ typedef std::shared_ptr PointCloudArrayTPtr; typedef message_synchronizer::MessageSynchronizer2 CameraInfoAndPoints; -typedef const boost::optional & CameraInfoCallbackT; -typedef const boost::optional & PointCloudsCallbackT; +typedef const std::optional & CameraInfoCallbackT; +typedef const std::optional & PointCloudsCallbackT; class PointCloudProjectionComponent : public rclcpp::Node { 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..e2c4dde 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -67,16 +67,16 @@ void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1) bool empty = true; if (in0) { empty = false; - const PointCloud2Ptr pc = in0.get(); + const auto pc = in0.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; - const PointCloud2Ptr pc = in1.get(); + const auto pc = in1.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (!empty) { @@ -91,23 +91,23 @@ void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, Callbac bool empty = true; if (in0) { empty = false; - const PointCloud2Ptr pc = in0.get(); + const auto pc = in0.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; - const PointCloud2Ptr pc = in1.get(); + const auto pc = in1.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in2) { empty = false; - const PointCloud2Ptr pc = in2.get(); + const auto pc = in2.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (!empty) { @@ -123,30 +123,30 @@ void PointsConcatenateComponent::callback4( bool empty = true; if (in0) { empty = false; - const PointCloud2Ptr pc = in0.get(); + const auto pc = in0.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; - const PointCloud2Ptr pc = in1.get(); + const auto pc = in1.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in2) { empty = false; - const PointCloud2Ptr pc = in2.get(); + const auto pc = in2.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in3) { empty = false; - const PointCloud2Ptr pc = in3.get(); + const auto pc = in3.value(); pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*pc, *pc_cloud); + pcl::fromROSMsg(pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (!empty) { 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; From 1dc414d41bb97ac19387e5a706b10288a2b25b6f Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 30 Apr 2024 23:11:07 +0900 Subject: [PATCH 2/6] enable use type adapter in points_concatenate_component Signed-off-by: hakuturu583 --- .../points_concatenate_component.hpp | 22 +++---- .../points_concatenate_component.cpp | 60 +++++++------------ 2 files changed, 29 insertions(+), 53 deletions(-) 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 8e02951..7e91ae8 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,15 +70,19 @@ extern "C" { namespace pcl_apps { +using PointCloudType = std::shared_ptr>; +using AdapterType = rclcpp::TypeAdapter; +using CallbackT = const std::optional; + typedef sensor_msgs::msg::PointCloud2 PointCloud2; typedef std::shared_ptr PointCloud2Ptr; -typedef const std::optional & CallbackT; -typedef message_synchronizer::MessageSynchronizer2 Sync2T; +// typedef const std::optional & CallbackT; +typedef message_synchronizer::MessageSynchronizer2 Sync2T; typedef std::shared_ptr Sync2PtrT; -typedef message_synchronizer::MessageSynchronizer3 Sync3T; +typedef message_synchronizer::MessageSynchronizer3 Sync3T; typedef std::shared_ptr Sync3PtrT; typedef message_synchronizer::MessageSynchronizer4< - PointCloud2, PointCloud2, PointCloud2, PointCloud2> + AdapterType, AdapterType, AdapterType, AdapterType> Sync4T; typedef std::shared_ptr Sync4PtrT; @@ -92,16 +96,6 @@ 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_; 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 e2c4dde..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 auto pc = in0.value(); - 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 auto pc = in1.value(); - 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 auto pc = in0.value(); - 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 auto pc = in1.value(); - 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 auto pc = in2.value(); - 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 auto pc = in0.value(); - 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 auto pc = in1.value(); - 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 auto pc = in2.value(); - 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 auto pc = in3.value(); - 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()); From 1ef3682536fc390cfa2a7c036852b1dcb33b4563 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 30 Apr 2024 23:14:47 +0900 Subject: [PATCH 3/6] remove typedef Signed-off-by: hakuturu583 --- .../points_concatenate_component.hpp | 21 +++++++------------ 1 file changed, 7 insertions(+), 14 deletions(-) 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 7e91ae8..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 @@ -74,17 +74,10 @@ using PointCloudType = std::shared_ptr>; using AdapterType = rclcpp::TypeAdapter; using CallbackT = const std::optional; -typedef sensor_msgs::msg::PointCloud2 PointCloud2; -typedef std::shared_ptr PointCloud2Ptr; -// typedef const std::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< - AdapterType, AdapterType, AdapterType, AdapterType> - Sync4T; -typedef std::shared_ptr Sync4PtrT; +using Sync2T = message_synchronizer::MessageSynchronizer2; +using Sync3T = message_synchronizer::MessageSynchronizer3; +using Sync4T = + message_synchronizer::MessageSynchronizer4; class PointsConcatenateComponent : public rclcpp::Node { @@ -97,9 +90,9 @@ class PointsConcatenateComponent : public rclcpp::Node void callback3(CallbackT in0, CallbackT in1, CallbackT in2); void callback4(CallbackT in0, CallbackT in1, CallbackT in2, CallbackT in3); 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_; }; From 0da92278847371e1848ae7c145624df55f3a9ab7 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 30 Apr 2024 23:52:09 +0900 Subject: [PATCH 4/6] remove message filters Signed-off-by: hakuturu583 --- .../crop_hull_filter_component.hpp | 21 ++++++-------- pcl_apps/package.xml | 1 - .../crop_hull_filter_component.cpp | 28 +++++++++---------- 3 files changed, 23 insertions(+), 27 deletions(-) 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/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..c5fa497 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_->registerCallback(std::bind( - &CropHullFilterComponent::callback, this, std::placeholders::_1, std::placeholders::_2)); + 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); From a43d063f9b0c592fbd152ba06f9f26c6fe9f6212 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 30 Apr 2024 23:53:07 +0900 Subject: [PATCH 5/6] register callback Signed-off-by: hakuturu583 --- .../filter/crop_hull_filter/crop_hull_filter_component.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 c5fa497..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 @@ -36,8 +36,8 @@ CropHullFilterComponent::CropHullFilterComponent(const rclcpp::NodeOptions & opt 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)); + sync_->registerCallback(std::bind( + &CropHullFilterComponent::callback, this, std::placeholders::_1, std::placeholders::_2)); } void CropHullFilterComponent::callback( From f5d4ff6e71dc6a0bea25a55e55591b1c1d9ea587 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 30 Apr 2024 23:56:24 +0900 Subject: [PATCH 6/6] remove typedef Signed-off-by: hakuturu583 --- .../pointcloud_projection_component.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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 b9e0639..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 std::optional & CameraInfoCallbackT; -typedef const std::optional & PointCloudsCallbackT; +using CameraInfoAndPoints = + message_synchronizer::MessageSynchronizer2; +using CameraInfoCallbackT = const std::optional &; +using PointCloudsCallbackT = const std::optional &; class PointCloudProjectionComponent : public rclcpp::Node {