From 1b513d5f7940daf4d513d0e6b1501d425e6580b8 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 16 Apr 2024 01:15:17 +0900 Subject: [PATCH 01/20] use ament_cmake_auto Signed-off-by: hakuturu583 --- dependency.repos | 6 +- pcl_apps/CMakeLists.txt | 57 ++--------------- pcl_apps/package.xml | 1 + pcl_apps/src/clustering/CMakeLists.txt | 24 +------ pcl_apps/src/filter/CMakeLists.txt | 88 +++++--------------------- pcl_apps/src/io/CMakeLists.txt | 33 ++-------- pcl_apps/src/matching/CMakeLists.txt | 33 ++-------- pcl_apps/src/projection/CMakeLists.txt | 25 +------- 8 files changed, 38 insertions(+), 229 deletions(-) diff --git a/dependency.repos b/dependency.repos index 756ec91..7eb8eb9 100644 --- a/dependency.repos +++ b/dependency.repos @@ -17,4 +17,8 @@ repositories: color_names: type: git url: https://github.com/OUXT-Polaris/color_names.git - version: master \ No newline at end of file + version: master + pcl_type_adapter: + type: git + url: https://github.com/OUXT-Polaris/pcl_type_adapter.git + version: master diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index d747781..1882ee0 100644 --- a/pcl_apps/CMakeLists.txt +++ b/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 @@ -136,26 +106,7 @@ install( 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() diff --git a/pcl_apps/package.xml b/pcl_apps/package.xml index 6948e66..a5cb2d1 100644 --- a/pcl_apps/package.xml +++ b/pcl_apps/package.xml @@ -9,6 +9,7 @@ ament_cmake + ament_cmake_auto rclcpp rclcpp_components pcl_conversions diff --git a/pcl_apps/src/clustering/CMakeLists.txt b/pcl_apps/src/clustering/CMakeLists.txt index 408db97..40e9d59 100644 --- a/pcl_apps/src/clustering/CMakeLists.txt +++ b/pcl_apps/src/clustering/CMakeLists.txt @@ -1,30 +1,12 @@ # Euclidean Clustering Component/Node -add_library(euclidean_clustering_component SHARED -euclidean_clustering/euclidean_clustering_component.cpp +ament_auto_add_library(euclidean_clustering_component SHARED + euclidean_clustering/euclidean_clustering_component.cpp ) target_compile_definitions(euclidean_clustering_component PRIVATE "PCL_APPS_EUCLIDEAN_CLUSTERING_BUILDING_DLL") target_link_libraries(euclidean_clustering_component boost_system ${PCL_LIBRARIES}) -ament_target_dependencies(euclidean_clustering_component - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) -add_executable(euclidean_clustering_node +ament_auto_add_executable(euclidean_clustering_node euclidean_clustering/euclidean_clustering_node.cpp) target_link_libraries(euclidean_clustering_node euclidean_clustering_component ${PCL_LIBRARIES}) -ament_target_dependencies(euclidean_clustering_node - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) - -# install executables/libs -install(TARGETS - euclidean_clustering_node - DESTINATION lib/pcl_apps -) - -install(TARGETS - euclidean_clustering_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_export_libraries(euclidean_clustering_component) \ No newline at end of file diff --git a/pcl_apps/src/filter/CMakeLists.txt b/pcl_apps/src/filter/CMakeLists.txt index 2cd6dd3..44891a2 100644 --- a/pcl_apps/src/filter/CMakeLists.txt +++ b/pcl_apps/src/filter/CMakeLists.txt @@ -1,164 +1,106 @@ # Points Concatenate Component/Node -add_library(points_concatenate_component SHARED +ament_auto_add_library(points_concatenate_component SHARED points_concatenate/points_concatenate_component.cpp ) target_compile_definitions(points_concatenate_component PRIVATE "PCL_APPS_POINTS_CONCATENATE_BUILDING_DLL") target_link_libraries(points_concatenate_component boost_system ${PCL_LIBRARIES}) -ament_target_dependencies(points_concatenate_component - rclcpp rclcpp_components pcl_conversions sensor_msgs message_synchronizer) -add_executable(points_concatenate_node +ament_auto_add_executable(points_concatenate_node points_concatenate/points_concatenate_node.cpp) target_link_libraries(points_concatenate_node points_concatenate_component ${PCL_LIBRARIES}) -ament_target_dependencies(points_concatenate_node - rclcpp rclcpp_components pcl_conversions sensor_msgs message_synchronizer) # Points Transform Component/Node -add_library(points_transform_component SHARED +ament_auto_add_library(points_transform_component SHARED points_transform/points_transform_component.cpp ) target_compile_definitions(points_transform_component PRIVATE "PCL_APPS_POINTS_TRANSFORM_BUILDING_DLL") target_link_libraries(points_transform_component boost_system ${PCL_LIBRARIES}) -ament_target_dependencies(points_transform_component - rclcpp rclcpp_components pcl_conversions tf2_ros sensor_msgs tf2_eigen) -add_executable(points_transform_node +ament_auto_add_executable(points_transform_node points_transform/points_transform_node.cpp) target_link_libraries(points_transform_node points_transform_component ${PCL_LIBRARIES}) -ament_target_dependencies(points_transform_node - rclcpp rclcpp_components pcl_conversions tf2_ros sensor_msgs tf2_eigen) # Voxelgrid Filter Component/Node -add_library(voxelgrid_filter_component SHARED +ament_auto_add_library(voxelgrid_filter_component SHARED voxelgrid_filter/voxelgrid_filter_component.cpp ) target_compile_definitions(voxelgrid_filter_component PRIVATE "PCL_APPS_VOXELGRID_FILTER_BUILDING_DLL") target_link_libraries(voxelgrid_filter_component ${PCL_LIBRARIES}) -ament_target_dependencies(voxelgrid_filter_component - rclcpp rclcpp_components pcl_conversions sensor_msgs) -add_executable(voxelgrid_filter_node +ament_auto_add_executable(voxelgrid_filter_node voxelgrid_filter/voxelgrid_filter_node.cpp ) target_link_libraries(voxelgrid_filter_node voxelgrid_filter_component ${PCL_LIBRARIES}) -ament_target_dependencies(voxelgrid_filter_node - rclcpp rclcpp_components pcl_conversions sensor_msgs) # Radius Outlier Removal Component/Node -add_library(radius_outlier_removal_component SHARED +ament_auto_add_library(radius_outlier_removal_component SHARED radius_outlier_removal/radius_outlier_removal_component.cpp ) target_compile_definitions(radius_outlier_removal_component PRIVATE "PCL_APPS_RADIUS_OUTLIER_REMOVAL_BUILDING_DLL") target_link_libraries(radius_outlier_removal_component ${PCL_LIBRARIES} ) -ament_target_dependencies(radius_outlier_removal_component - rclcpp rclcpp_components pcl_conversions sensor_msgs) -add_executable(radius_outlier_removal_node +ament_auto_add_executable(radius_outlier_removal_node radius_outlier_removal/radius_outlier_removal_node.cpp ) target_link_libraries(radius_outlier_removal_node radius_outlier_removal_component ${PCL_LIBRARIES} ) -ament_target_dependencies(radius_outlier_removal_node - rclcpp rclcpp_components pcl_conversions sensor_msgs) # Crop Hull Filter Component/Node -add_library(crop_hull_filter_component SHARED +ament_auto_add_library(crop_hull_filter_component SHARED crop_hull_filter/crop_hull_filter_component.cpp ) target_compile_definitions(crop_hull_filter_component PRIVATE "PCL_APPS_CROP_HULL_FILTER_BUILDING_DLL") target_link_libraries(crop_hull_filter_component ${PCL_LIBRARIES} ) -ament_target_dependencies(crop_hull_filter_component - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs message_filters) -add_executable(crop_hull_filter_node +ament_auto_add_executable(crop_hull_filter_node crop_hull_filter/crop_hull_filter_node.cpp ) target_link_libraries(crop_hull_filter_node crop_hull_filter_component ${PCL_LIBRARIES} ) -ament_target_dependencies(crop_hull_filter_node - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs message_filters) # Crop Box Filter Component/Node -add_library(crop_box_filter_component SHARED +ament_auto_add_library(crop_box_filter_component SHARED crop_box_filter/crop_box_filter_component.cpp ) target_compile_definitions(crop_box_filter_component PRIVATE "PCL_APPS_CROP_HULL_FILTER_BUILDING_DLL") target_link_libraries(crop_box_filter_component ${PCL_LIBRARIES} ) -ament_target_dependencies(crop_box_filter_component - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) -add_executable(crop_box_filter_node +ament_auto_add_executable(crop_box_filter_node crop_box_filter/crop_box_filter_node.cpp ) target_link_libraries(crop_box_filter_node crop_box_filter_component ${PCL_LIBRARIES} ) -ament_target_dependencies(crop_box_filter_node - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) # Point Cloud To Laser Scan Component/Node -add_library(pointcloud_to_laserscan_component SHARED +ament_auto_add_library(pointcloud_to_laserscan_component SHARED pointcloud_to_laserscan/pointcloud_to_laserscan_component.cpp ) target_compile_definitions(pointcloud_to_laserscan_component PRIVATE "PCL_APPS_POINTCLOUD_TO_LASERSCAN_PUBLIC") target_link_libraries(pointcloud_to_laserscan_component ${PCL_LIBRARIES} ) -ament_target_dependencies(pointcloud_to_laserscan_component - rclcpp rclcpp_components pcl_conversions sensor_msgs) -add_executable(pointcloud_to_laserscan_node + +ament_auto_add_executable(pointcloud_to_laserscan_node pointcloud_to_laserscan/pointcloud_to_laserscan_node.cpp ) target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan_component ${PCL_LIBRARIES} ) -ament_target_dependencies(pointcloud_to_laserscan_node - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) - -# install executables/libs -install(TARGETS - points_transform_node - points_concatenate_node - voxelgrid_filter_node - radius_outlier_removal_node - crop_hull_filter_node - crop_box_filter_node - pointcloud_to_laserscan_node - DESTINATION lib/pcl_apps -) - -install(TARGETS - points_transform_component - points_concatenate_component - voxelgrid_filter_component - radius_outlier_removal_component - crop_hull_filter_component - crop_box_filter_component - pointcloud_to_laserscan_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_export_libraries(points_transform_component) -ament_export_libraries(points_concatenate_component) -ament_export_libraries(voxelgrid_filter_component) -ament_export_libraries(radius_outlier_removal_component) -ament_export_libraries(crop_hull_filter_component) -ament_export_libraries(crop_box_filter_component) diff --git a/pcl_apps/src/io/CMakeLists.txt b/pcl_apps/src/io/CMakeLists.txt index 71b1619..5af7d92 100644 --- a/pcl_apps/src/io/CMakeLists.txt +++ b/pcl_apps/src/io/CMakeLists.txt @@ -1,49 +1,24 @@ # Pcd Writer Component/Node -add_library(pcd_writer_component SHARED +ament_auto_add_library(pcd_writer_component SHARED pcd_writer/pcd_writer_component.cpp ) target_compile_definitions(pcd_writer_component PRIVATE "PCL_APPS_PCD_WRITER_BUILDING_DLL") target_link_libraries(pcd_writer_component boost_system ${PCL_LIBRARIES}) -ament_target_dependencies(pcd_writer_component - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) -add_executable(pcd_writer_node +ament_auto_add_executable(pcd_writer_node pcd_writer/pcd_writer_node.cpp) target_link_libraries(pcd_writer_node pcd_writer_component ${PCL_LIBRARIES}) -ament_target_dependencies(pcd_writer_node - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) # Pcd Loader Component/Node -add_library(pcd_loader_component SHARED +ament_auto_add_library(pcd_loader_component SHARED pcd_loader/pcd_loader_component.cpp ) target_compile_definitions(pcd_loader_component PRIVATE "PCL_APPS_PCD_LOADER_BUILDING_DLL") target_link_libraries(pcd_loader_component boost_system ${PCL_LIBRARIES}) -ament_target_dependencies(pcd_loader_component - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) -add_executable(pcd_loader_node +ament_auto_add_executable(pcd_loader_node pcd_loader/pcd_loader_node.cpp) target_link_libraries(pcd_loader_node pcd_loader_component ${PCL_LIBRARIES}) -ament_target_dependencies(pcd_loader_node - rclcpp rclcpp_components pcl_conversions sensor_msgs pcl_apps_msgs) - -# install executables/libs -install(TARGETS - pcd_writer_node - pcd_loader_node - DESTINATION lib/pcl_apps -) - -install(TARGETS - pcd_writer_component - pcd_loader_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_export_libraries(pcd_writer_component) -ament_export_libraries(pcd_loader_component) \ No newline at end of file diff --git a/pcl_apps/src/matching/CMakeLists.txt b/pcl_apps/src/matching/CMakeLists.txt index 9f35a09..d373582 100644 --- a/pcl_apps/src/matching/CMakeLists.txt +++ b/pcl_apps/src/matching/CMakeLists.txt @@ -1,52 +1,27 @@ # Ndt Matching Component/Node -add_library(ndt_matching_component SHARED +ament_auto_add_library(ndt_matching_component SHARED ndt_matching/ndt_matching_component.cpp ) target_compile_definitions(ndt_matching_component PRIVATE "PCL_APPS_NDT_MATCHING_BUILDING_DLL") target_link_libraries(ndt_matching_component ${PCL_LIBRARIES}) -ament_target_dependencies(ndt_matching_component - rclcpp rclcpp_components pcl_conversions sensor_msgs tf2_eigen ndt_omp tf2_geometry_msgs) -add_executable(ndt_matching_node +ament_auto_add_executable(ndt_matching_node ndt_matching/ndt_matching_node.cpp ) target_link_libraries(ndt_matching_node ndt_matching_component ${PCL_LIBRARIES}) -ament_target_dependencies(ndt_matching_node - rclcpp rclcpp_components pcl_conversions sensor_msgs tf2_eigen ndt_omp) # Ndt Matching Twist Estimator Component/Node -add_library(ndt_matching_twist_estimator_component SHARED +ament_auto_add_library(ndt_matching_twist_estimator_component SHARED ndt_matching/ndt_matching_twist_estimator_component.cpp ) target_compile_definitions(ndt_matching_twist_estimator_component PRIVATE "PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_BUILDING_DLL") target_link_libraries(ndt_matching_twist_estimator_component ${PCL_LIBRARIES}) -ament_target_dependencies(ndt_matching_twist_estimator_component - rclcpp rclcpp_components pcl_conversions sensor_msgs tf2_eigen) -add_executable(ndt_matching_twist_estimator_node +ament_auto_add_executable(ndt_matching_twist_estimator_node ndt_matching/ndt_matching_twist_estimator_node.cpp ) target_link_libraries(ndt_matching_twist_estimator_node ndt_matching_twist_estimator_component ${PCL_LIBRARIES}) -ament_target_dependencies(ndt_matching_twist_estimator_node - rclcpp rclcpp_components pcl_conversions sensor_msgs tf2_eigen) - -# install executables/libs -install(TARGETS - ndt_matching_node - ndt_matching_twist_estimator_node - DESTINATION lib/pcl_apps -) - -install(TARGETS - ndt_matching_component - ndt_matching_twist_estimator_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_export_libraries(ndt_matching_component) -ament_export_libraries(ndt_matching_twist_estimator_component) \ No newline at end of file diff --git a/pcl_apps/src/projection/CMakeLists.txt b/pcl_apps/src/projection/CMakeLists.txt index e399635..f3a5855 100644 --- a/pcl_apps/src/projection/CMakeLists.txt +++ b/pcl_apps/src/projection/CMakeLists.txt @@ -1,33 +1,12 @@ # Pointcloud Projection Component/Node -add_library(pointcloud_projection_component SHARED +ament_auto_add_library(pointcloud_projection_component SHARED pointcloud_projection/pointcloud_projection_component.cpp ) target_compile_definitions(pointcloud_projection_component PRIVATE "PCL_APPS_POINTCLOUD_PROJECTION_BUILDING_DLL") -ament_target_dependencies(pointcloud_projection_component - rclcpp rclcpp_components pcl_conversions sensor_msgs - perception_msgs image_geometry message_synchronizer pcl_apps_msgs - tf2_ros tf2_geometry_msgs visualization_msgs color_names) target_link_libraries(pointcloud_projection_component ${PCL_LIBRARIES}) -add_executable(pointcloud_projection_node +ament_auto_add_executable(pointcloud_projection_node pointcloud_projection/pointcloud_projection_node.cpp ) -ament_target_dependencies(pointcloud_projection_node - rclcpp rclcpp_components pcl_conversions sensor_msgs - perception_msgs image_geometry message_synchronizer pcl_apps_msgs - tf2_ros tf2_geometry_msgs visualization_msgs color_names) target_link_libraries(pointcloud_projection_node pointcloud_projection_component) - - # install executables/libs -install(TARGETS - pointcloud_projection_node - DESTINATION lib/pcl_apps) - -install(TARGETS - pointcloud_projection_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_export_libraries(pointcloud_projection_component) From 98c18b93b319c11f78b60306487d6218c9fda403 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 16 Apr 2024 02:22:33 +0900 Subject: [PATCH 02/20] add type adaptation in crop box filter Signed-off-by: hakuturu583 --- pcl_apps/include/pcl_apps/adapter.hpp | 26 ++++++++++++++++ .../crop_box_filter_component.hpp | 7 +++-- pcl_apps/package.xml | 30 ++++++++++--------- .../crop_box_filter_component.cpp | 20 +++++-------- 4 files changed, 54 insertions(+), 29 deletions(-) create mode 100644 pcl_apps/include/pcl_apps/adapter.hpp diff --git a/pcl_apps/include/pcl_apps/adapter.hpp b/pcl_apps/include/pcl_apps/adapter.hpp new file mode 100644 index 0000000..e68a608 --- /dev/null +++ b/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 + +namespace pcl_apps +{ +using PCLPointType = pcl::PointXYZI; +using PCLPointCloudType = pcl::PointCloud; +using PCLPointCloudTypePtr = std::shared_ptr; +using PointCloudAdapterType = + rclcpp::TypeAdapter; +using PointCloudPublisher = std::shared_ptr>; +using PointCloudSubscriber = std::shared_ptr>; +} // namespace pcl_apps diff --git a/pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp b/pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp index 1d8cb82..b0638d2 100644 --- a/pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp @@ -54,6 +54,7 @@ extern "C" { #endif #include +#include #include #include @@ -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::SharedPtr pub_; - rclcpp::Subscription::SharedPtr sub_; + PointCloudPublisher pub_; + PointCloudSubscriber sub_; }; } // namespace pcl_apps diff --git a/pcl_apps/package.xml b/pcl_apps/package.xml index a5cb2d1..d75e8d5 100644 --- a/pcl_apps/package.xml +++ b/pcl_apps/package.xml @@ -10,25 +10,27 @@ ament_cmake ament_cmake_auto - rclcpp - rclcpp_components - pcl_conversions - message_filters - tf2_ros - tf2_eigen - tf2_geometry_msgs - std_msgs + color_names geometry_msgs - message_synchronizer - sensor_msgs - pcl_apps_msgs - libpcl-all-dev + image_geometry libboost-system-dev + libpcl-all-dev + message_filters + message_synchronizer ndt_omp + pcl_apps_msgs + pcl_conversions + pcl_type_adapter perception_msgs - image_geometry + rclcpp_components + rclcpp + sensor_msgs + std_msgs + tf2_eigen + tf2_geometry_msgs + tf2_ros visualization_msgs - color_names + ament_lint_auto ouxt_lint_common diff --git a/pcl_apps/src/filter/crop_box_filter/crop_box_filter_component.cpp b/pcl_apps/src/filter/crop_box_filter/crop_box_filter_component.cpp index f219cd3..7c234aa 100644 --- a/pcl_apps/src/filter/crop_box_filter/crop_box_filter_component.cpp +++ b/pcl_apps/src/filter/crop_box_filter/crop_box_filter_component.cpp @@ -42,16 +42,14 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio get_parameter("keep_organized", keep_organized_); declare_parameter("negative", false); get_parameter("negative", negative_); - pub_ = create_publisher("~/points_filtered", 1); - sub_ = create_subscription( - "~/points", 1, std::bind(&CropBoxFilterComponent::pointsCallback, this, std::placeholders::_1)); + pub_ = create_publisher("~/points_filtered", 1); + sub_ = create_subscription( + "~/points", 1, [this](const PCLPointCloudTypePtr & msg) { pointsCallback(msg); }); } -void CropBoxFilterComponent::pointsCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +void CropBoxFilterComponent::pointsCallback(const PCLPointCloudTypePtr & msg) { - pcl::CropBox filter; - pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2()); - pcl_conversions::toPCL(*msg, *cloud); + pcl::CropBox filter; Eigen::Vector4f new_min_point, new_max_point; new_min_point << min_x_, min_y_, min_z_, 0.0; new_max_point << max_x_, max_y_, max_z_, 0.0; @@ -59,12 +57,10 @@ void CropBoxFilterComponent::pointsCallback(const sensor_msgs::msg::PointCloud2: filter.setMax(new_max_point); filter.setNegative(negative_); filter.setKeepOrganized(keep_organized_); - filter.setInputCloud(cloud); - pcl::PCLPointCloud2::Ptr filtered_cloud(new pcl::PCLPointCloud2()); + filter.setInputCloud(msg); + PCLPointCloudTypePtr filtered_cloud(new PCLPointCloudType()); filter.filter(*filtered_cloud); - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl_conversions::fromPCL(*filtered_cloud, output_cloud_msg); - pub_->publish(output_cloud_msg); + pub_->publish(filtered_cloud); } } // namespace pcl_apps From d27e1418c614e8ad3bfcc67ad2adb790f9f8f789 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Tue, 16 Apr 2024 10:22:20 +0900 Subject: [PATCH 03/20] use type adaptation in pointcloud_to_laserscan Signed-off-by: hakuturu583 --- .../pointcloud_to_laserscan_component.hpp | 5 ++-- .../pointcloud_to_laserscan_component.cpp | 30 +++++++++---------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp b/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp index 8e5e5c5..2e6b6ac 100644 --- a/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp @@ -56,6 +56,7 @@ extern "C" { #include #include #include +#include namespace pcl_apps { @@ -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::SharedPtr sub_; + void pointsCallback(const PCLPointCloudTypePtr & msg); + PointCloudSubscriber sub_; rclcpp::Publisher::SharedPtr pub_; double min_height_; double max_height_; diff --git a/pcl_apps/src/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.cpp b/pcl_apps/src/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.cpp index 6afbe29..7a1efa7 100644 --- a/pcl_apps/src/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.cpp +++ b/pcl_apps/src/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.cpp @@ -25,9 +25,6 @@ namespace pcl_apps PointCloudToLaserScanComponent::PointCloudToLaserScanComponent(const rclcpp::NodeOptions & options) : Node("pointcloud_to_laserscan", options) { - pub_ = create_publisher("output", 1); - sub_ = create_subscription( - "input", 1, std::bind(&PointCloudToLaserScanComponent::callback, this, std::placeholders::_1)); min_height_ = this->declare_parameter("min_height", std::numeric_limits::min()); max_height_ = this->declare_parameter("max_height", std::numeric_limits::max()); angle_min_ = this->declare_parameter("angle_min", -M_PI); @@ -38,12 +35,15 @@ PointCloudToLaserScanComponent::PointCloudToLaserScanComponent(const rclcpp::Nod range_max_ = this->declare_parameter("range_max", std::numeric_limits::max()); inf_epsilon_ = this->declare_parameter("inf_epsilon", 1.0); use_inf_ = this->declare_parameter("use_inf", false); + pub_ = create_publisher("output", 1); + sub_ = create_subscription( + "input", 1, [this](const PCLPointCloudTypePtr & msg) { pointsCallback(msg); }); } -void PointCloudToLaserScanComponent::callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +void PointCloudToLaserScanComponent::pointsCallback(const PCLPointCloudTypePtr & msg) { sensor_msgs::msg::LaserScan scan_msg; - scan_msg.header = msg->header; + pcl_conversions::fromPCL(msg->header, scan_msg.header); scan_msg.angle_min = angle_min_; scan_msg.angle_max = angle_max_; scan_msg.angle_increment = angle_increment_; @@ -58,34 +58,32 @@ void PointCloudToLaserScanComponent::callback(const sensor_msgs::msg::PointCloud } else { scan_msg.ranges.assign(ranges_size, scan_msg.range_max + inf_epsilon_); } - for (sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"), iter_y(*msg, "y"), - iter_z(*msg, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) { + for (auto itr = msg->begin(); itr != msg->end(); itr++) { + if (std::isnan(itr->x) || std::isnan(itr->y) || std::isnan(itr->z)) { RCLCPP_DEBUG( - this->get_logger(), "rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z); + this->get_logger(), "rejected for nan in point(%f, %f, %f)\n", itr->x, itr->y, itr->z); continue; } - if (*iter_z > max_height_ || *iter_z < min_height_) { + if (itr->z > max_height_ || itr->z < min_height_) { RCLCPP_DEBUG( - this->get_logger(), "rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, + this->get_logger(), "rejected for height %f not in range (%f, %f)\n", itr->z, min_height_, max_height_); continue; } - double range = hypot(*iter_x, *iter_y); + double range = hypot(itr->x, itr->y); if (range < range_min_) { RCLCPP_DEBUG( this->get_logger(), "rejected for range %f below minimum value %f. Point: (%f, %f, %f)", - range, range_min_, *iter_x, *iter_y, *iter_z); + range, range_min_, itr->x, itr->y, itr->z); continue; } if (range > range_max_) { RCLCPP_DEBUG( this->get_logger(), "rejected for range %f above maximum value %f. Point: (%f, %f, %f)", - range, range_max_, *iter_x, *iter_y, *iter_z); + range, range_max_, itr->x, itr->y, itr->z); continue; } - double angle = atan2(*iter_y, *iter_x); + double angle = atan2(itr->y, itr->x); if (angle < scan_msg.angle_min || angle > scan_msg.angle_max) { RCLCPP_DEBUG( this->get_logger(), "rejected for angle %f not in range (%f, %f)\n", angle, From 82be147b8987ff72c0c3c6100e8e9dae4c5b97ae Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Thu, 18 Apr 2024 22:31:05 +0900 Subject: [PATCH 04/20] use type adaptation in pointcloud_to_laserscan component Signed-off-by: hakuturu583 --- .../pointcloud_to_laserscan_component.hpp | 2 +- .../points_concatenate_component.hpp | 3 +-- .../points_transform_component.hpp | 6 ++--- .../points_transform_component.cpp | 26 ++++++++++--------- 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp b/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp index 2e6b6ac..2505876 100644 --- a/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/pointcloud_to_laserscan/pointcloud_to_laserscan_component.hpp @@ -53,10 +53,10 @@ extern "C" { } // extern "C" #endif +#include #include #include #include -#include 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 118a902..cf6b3b2 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 @@ -54,9 +54,8 @@ extern "C" { #endif // Headers in ROS2 -#include - #include +#include #include #include diff --git a/pcl_apps/include/pcl_apps/filter/points_transform/points_transform_component.hpp b/pcl_apps/include/pcl_apps/filter/points_transform/points_transform_component.hpp index 2778703..025c926 100644 --- a/pcl_apps/include/pcl_apps/filter/points_transform/points_transform_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/points_transform/points_transform_component.hpp @@ -54,7 +54,7 @@ extern "C" { #endif // Headers in ROS2 -#include +#include #ifdef USE_TF2_EIGEN_DEPRECATED_HEADER #include @@ -89,8 +89,8 @@ class PointsTransformComponent : public rclcpp::Node rclcpp::Clock ros_clock_; tf2_ros::Buffer buffer_; tf2_ros::TransformListener listener_; - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_; + PointCloudSubscriber sub_; + PointCloudPublisher pub_; std::string input_topic_; }; } // namespace pcl_apps diff --git a/pcl_apps/src/filter/points_transform/points_transform_component.cpp b/pcl_apps/src/filter/points_transform/points_transform_component.cpp index 9aad267..4c69bf6 100644 --- a/pcl_apps/src/filter/points_transform/points_transform_component.cpp +++ b/pcl_apps/src/filter/points_transform/points_transform_component.cpp @@ -40,25 +40,27 @@ PointsTransformComponent::PointsTransformComponent(const rclcpp::NodeOptions & o std::string output_topic_name; declare_parameter("output_topic", get_name() + std::string("/output")); get_parameter("output_topic", output_topic_name); - pub_ = create_publisher(output_topic_name, 10); - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { - if (msg->header.frame_id == output_frame_id_) { - pub_->publish(*msg); + pub_ = create_publisher(output_topic_name, 10); + auto callback = [this](const PCLPointCloudTypePtr msg) -> void { + std_msgs::msg::Header header; + pcl_conversions::fromPCL(msg->header, header); + if (header.frame_id == output_frame_id_) { + pub_->publish(msg); } else { tf2::TimePoint time_point = tf2::TimePoint( - std::chrono::seconds(msg->header.stamp.sec) + - std::chrono::nanoseconds(msg->header.stamp.nanosec)); + std::chrono::seconds(header.stamp.sec) + + std::chrono::nanoseconds(header.stamp.nanosec)); geometry_msgs::msg::TransformStamped transform_stamped = buffer_.lookupTransform( - output_frame_id_, msg->header.frame_id, time_point, tf2::durationFromSec(1.0)); - sensor_msgs::msg::PointCloud2 output_msg; - sensor_msgs::msg::PointCloud2 input_msg = *msg; - tf2::doTransform(input_msg, output_msg, transform_stamped); - pub_->publish(output_msg); + output_frame_id_, header.frame_id, time_point, tf2::durationFromSec(1.0)); + // sensor_msgs::msg::PointCloud2 output_msg; + // sensor_msgs::msg::PointCloud2 input_msg = *msg; + // tf2::doTransform(input_msg, output_msg, transform_stamped); + // pub_->publish(output_msg); } }; declare_parameter("input_topic", get_name() + std::string("/input")); get_parameter("input_topic", input_topic_); - sub_ = create_subscription(input_topic_, 10, callback); + // sub_ = create_subscription(input_topic_, 10, callback); } } // namespace pcl_apps From cb09d7f9684e64da893a902590df00ffac31d01d Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Thu, 18 Apr 2024 22:40:18 +0900 Subject: [PATCH 05/20] remove doTransform and use pcl::TransformPointCloud Signed-off-by: hakuturu583 --- .../radius_outlier_removal_component.hpp | 4 +--- .../points_transform_component.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp b/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp index ed2a3bd..fb1c8cb 100644 --- a/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp +++ b/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 - +#include #include #include diff --git a/pcl_apps/src/filter/points_transform/points_transform_component.cpp b/pcl_apps/src/filter/points_transform/points_transform_component.cpp index 4c69bf6..7c18814 100644 --- a/pcl_apps/src/filter/points_transform/points_transform_component.cpp +++ b/pcl_apps/src/filter/points_transform/points_transform_component.cpp @@ -48,19 +48,19 @@ PointsTransformComponent::PointsTransformComponent(const rclcpp::NodeOptions & o pub_->publish(msg); } else { tf2::TimePoint time_point = tf2::TimePoint( - std::chrono::seconds(header.stamp.sec) + - std::chrono::nanoseconds(header.stamp.nanosec)); + std::chrono::seconds(header.stamp.sec) + std::chrono::nanoseconds(header.stamp.nanosec)); geometry_msgs::msg::TransformStamped transform_stamped = buffer_.lookupTransform( output_frame_id_, header.frame_id, time_point, tf2::durationFromSec(1.0)); - // sensor_msgs::msg::PointCloud2 output_msg; - // sensor_msgs::msg::PointCloud2 input_msg = *msg; - // tf2::doTransform(input_msg, output_msg, transform_stamped); - // pub_->publish(output_msg); + Eigen::Matrix4f mat = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + PCLPointCloudTypePtr transformed; + pcl::transformPointCloud(*msg, *transformed, mat); + pub_->publish(transformed); } }; declare_parameter("input_topic", get_name() + std::string("/input")); get_parameter("input_topic", input_topic_); - // sub_ = create_subscription(input_topic_, 10, callback); + sub_ = create_subscription(input_topic_, 10, callback); } } // namespace pcl_apps From c93551e55c5e402d28ae35454eff22f9fb1be2c7 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Thu, 18 Apr 2024 23:33:55 +0900 Subject: [PATCH 06/20] refactor project architecture Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 79 +++++-------- .../radius_outlier_removal_component.hpp | 4 +- ...ndt_matching_twist_estimator_component.hpp | 0 pcl_apps/src/clustering/CMakeLists.txt | 12 -- pcl_apps/src/filter/CMakeLists.txt | 106 ------------------ .../radius_outlier_removal_component.cpp | 20 ++-- .../voxelgrid_filter_component.cpp | 74 ------------ .../voxelgrid_filter_node.cpp | 32 ------ pcl_apps/src/io/CMakeLists.txt | 24 ---- pcl_apps/src/matching/CMakeLists.txt | 27 ----- ...ndt_matching_twist_estimator_component.cpp | 2 +- .../ndt_matching_twist_estimator_node.cpp | 2 +- 12 files changed, 41 insertions(+), 341 deletions(-) rename pcl_apps/include/pcl_apps/matching/{ndt_matching => ndt_matching_twist_estimator}/ndt_matching_twist_estimator_component.hpp (100%) delete mode 100644 pcl_apps/src/clustering/CMakeLists.txt delete mode 100644 pcl_apps/src/filter/CMakeLists.txt delete mode 100644 pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp delete mode 100644 pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp delete mode 100644 pcl_apps/src/io/CMakeLists.txt delete mode 100644 pcl_apps/src/matching/CMakeLists.txt rename pcl_apps/src/matching/{ndt_matching => ndt_matching_twist_estimator}/ndt_matching_twist_estimator_component.cpp (98%) rename pcl_apps/src/matching/{ndt_matching => ndt_matching_twist_estimator}/ndt_matching_twist_estimator_node.cpp (90%) diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 1882ee0..2f7d4b6 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -44,65 +44,44 @@ link_directories( ) add_definitions(${PCL_DEFINITIONS}) +function(add_pcl_apps_component module component_name component_name_in_macro class_name) + 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}) + + rclcpp_components_register_nodes(${component_name}_component pcl_apps::${class_name}) +endfunction(add_pcl_apps_component) + + # Filter Modules -add_subdirectory(src/filter) +add_pcl_apps_component(filter points_concatenate POINTS_CONCATENATE PointsConcatenateComponent) +add_pcl_apps_component(filter points_transform POINTS_TRANSFORM PointsTransformComponent) +add_pcl_apps_component(filter radius_outlier_removal RADIUS_OUTLIER_REMOVAL RadiusOutlierRemovalComponent) +add_pcl_apps_component(filter crop_hull_filter CROP_HULL_FILTER CropHullFilterComponent) +add_pcl_apps_component(filter crop_box_filter CROP_BOX_FILTER CropBoxFilterComponent) +add_pcl_apps_component(filter pointcloud_to_laserscan POINTCLOUD_TO_LASERSCAN PointCloudToLaserScanComponent) # Matching Modules -add_subdirectory(src/matching) +add_pcl_apps_component(matching ndt_matching NDT_MATCHING NdtMatchingComponent) +add_pcl_apps_component(matching ndt_matching_twist_estimator NDT_MATCHING_TWIST_ESTIMATOR NdtMatchingTwistEstimatorComponent) # IO Module -add_subdirectory(src/io) +add_pcl_apps_component(io pcd_writer PCD_WRITER PcdWriter) +add_pcl_apps_component(io pcd_loader PCD_LOADER PcdLoader) # Clustering Module -add_subdirectory(src/clustering) +add_pcl_apps_component(clustering euclidean_clustering EUCLIDEAN_CLUSTERING 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 PointCloudProjectionComponent) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp b/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp index fb1c8cb..c9bba27 100644 --- a/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp @@ -74,8 +74,8 @@ class RadiusOutlierRemovalComponent : public rclcpp::Node private: std::string input_topic_; - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_; + PointCloudSubscriber sub_; + PointCloudPublisher pub_; pcl::RadiusOutlierRemoval filter_; double search_radius_; int min_neighbors_in_search_radius_; diff --git a/pcl_apps/include/pcl_apps/matching/ndt_matching/ndt_matching_twist_estimator_component.hpp b/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp similarity index 100% rename from pcl_apps/include/pcl_apps/matching/ndt_matching/ndt_matching_twist_estimator_component.hpp rename to pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp diff --git a/pcl_apps/src/clustering/CMakeLists.txt b/pcl_apps/src/clustering/CMakeLists.txt deleted file mode 100644 index 40e9d59..0000000 --- a/pcl_apps/src/clustering/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -# Euclidean Clustering Component/Node -ament_auto_add_library(euclidean_clustering_component SHARED - euclidean_clustering/euclidean_clustering_component.cpp -) -target_compile_definitions(euclidean_clustering_component PRIVATE "PCL_APPS_EUCLIDEAN_CLUSTERING_BUILDING_DLL") -target_link_libraries(euclidean_clustering_component - boost_system ${PCL_LIBRARIES}) - -ament_auto_add_executable(euclidean_clustering_node - euclidean_clustering/euclidean_clustering_node.cpp) -target_link_libraries(euclidean_clustering_node - euclidean_clustering_component ${PCL_LIBRARIES}) diff --git a/pcl_apps/src/filter/CMakeLists.txt b/pcl_apps/src/filter/CMakeLists.txt deleted file mode 100644 index 44891a2..0000000 --- a/pcl_apps/src/filter/CMakeLists.txt +++ /dev/null @@ -1,106 +0,0 @@ -# Points Concatenate Component/Node -ament_auto_add_library(points_concatenate_component SHARED - points_concatenate/points_concatenate_component.cpp -) -target_compile_definitions(points_concatenate_component PRIVATE "PCL_APPS_POINTS_CONCATENATE_BUILDING_DLL") -target_link_libraries(points_concatenate_component - boost_system ${PCL_LIBRARIES}) - -ament_auto_add_executable(points_concatenate_node - points_concatenate/points_concatenate_node.cpp) -target_link_libraries(points_concatenate_node - points_concatenate_component ${PCL_LIBRARIES}) - -# Points Transform Component/Node -ament_auto_add_library(points_transform_component SHARED - points_transform/points_transform_component.cpp -) -target_compile_definitions(points_transform_component PRIVATE "PCL_APPS_POINTS_TRANSFORM_BUILDING_DLL") -target_link_libraries(points_transform_component - boost_system ${PCL_LIBRARIES}) - -ament_auto_add_executable(points_transform_node - points_transform/points_transform_node.cpp) -target_link_libraries(points_transform_node - points_transform_component ${PCL_LIBRARIES}) - -# Voxelgrid Filter Component/Node -ament_auto_add_library(voxelgrid_filter_component SHARED - voxelgrid_filter/voxelgrid_filter_component.cpp -) -target_compile_definitions(voxelgrid_filter_component PRIVATE "PCL_APPS_VOXELGRID_FILTER_BUILDING_DLL") -target_link_libraries(voxelgrid_filter_component - ${PCL_LIBRARIES}) - -ament_auto_add_executable(voxelgrid_filter_node - voxelgrid_filter/voxelgrid_filter_node.cpp -) -target_link_libraries(voxelgrid_filter_node - voxelgrid_filter_component ${PCL_LIBRARIES}) - -# Radius Outlier Removal Component/Node -ament_auto_add_library(radius_outlier_removal_component SHARED - radius_outlier_removal/radius_outlier_removal_component.cpp -) -target_compile_definitions(radius_outlier_removal_component PRIVATE "PCL_APPS_RADIUS_OUTLIER_REMOVAL_BUILDING_DLL") -target_link_libraries(radius_outlier_removal_component - ${PCL_LIBRARIES} -) - -ament_auto_add_executable(radius_outlier_removal_node - radius_outlier_removal/radius_outlier_removal_node.cpp -) -target_link_libraries(radius_outlier_removal_node - radius_outlier_removal_component ${PCL_LIBRARIES} -) - -# Crop Hull Filter Component/Node -ament_auto_add_library(crop_hull_filter_component SHARED - crop_hull_filter/crop_hull_filter_component.cpp -) -target_compile_definitions(crop_hull_filter_component PRIVATE "PCL_APPS_CROP_HULL_FILTER_BUILDING_DLL") -target_link_libraries(crop_hull_filter_component - ${PCL_LIBRARIES} -) - -ament_auto_add_executable(crop_hull_filter_node - crop_hull_filter/crop_hull_filter_node.cpp -) -target_link_libraries(crop_hull_filter_node - crop_hull_filter_component - ${PCL_LIBRARIES} -) - -# Crop Box Filter Component/Node -ament_auto_add_library(crop_box_filter_component SHARED - crop_box_filter/crop_box_filter_component.cpp -) -target_compile_definitions(crop_box_filter_component PRIVATE "PCL_APPS_CROP_HULL_FILTER_BUILDING_DLL") -target_link_libraries(crop_box_filter_component - ${PCL_LIBRARIES} -) - -ament_auto_add_executable(crop_box_filter_node - crop_box_filter/crop_box_filter_node.cpp -) -target_link_libraries(crop_box_filter_node - crop_box_filter_component - ${PCL_LIBRARIES} -) - -# Point Cloud To Laser Scan Component/Node -ament_auto_add_library(pointcloud_to_laserscan_component SHARED - pointcloud_to_laserscan/pointcloud_to_laserscan_component.cpp -) -target_compile_definitions(pointcloud_to_laserscan_component PRIVATE "PCL_APPS_POINTCLOUD_TO_LASERSCAN_PUBLIC") -target_link_libraries(pointcloud_to_laserscan_component - ${PCL_LIBRARIES} -) - -ament_auto_add_executable(pointcloud_to_laserscan_node - pointcloud_to_laserscan/pointcloud_to_laserscan_node.cpp -) -target_link_libraries(pointcloud_to_laserscan_node - pointcloud_to_laserscan_component - ${PCL_LIBRARIES} -) diff --git a/pcl_apps/src/filter/radius_outlier_removal/radius_outlier_removal_component.cpp b/pcl_apps/src/filter/radius_outlier_removal/radius_outlier_removal_component.cpp index 8dd444f..8c2d5dc 100644 --- a/pcl_apps/src/filter/radius_outlier_removal/radius_outlier_removal_component.cpp +++ b/pcl_apps/src/filter/radius_outlier_removal/radius_outlier_removal_component.cpp @@ -67,23 +67,19 @@ RadiusOutlierRemovalComponent::RadiusOutlierRemovalComponent(const rclcpp::NodeO return *results; }); std::string output_topic_name = get_name() + std::string("/output"); - pub_ = create_publisher(output_topic_name, 10); - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); + pub_ = create_publisher(output_topic_name, 10); + auto callback = [this](const PCLPointCloudTypePtr msg) -> void { std::vector nan_index; - pcl::removeNaNFromPointCloud(*cloud, *cloud, nan_index); - if (cloud->size() != 0) { - filter_.setInputCloud(cloud); + pcl::removeNaNFromPointCloud(*msg, *msg, nan_index); + if (msg->size() != 0) { + filter_.setInputCloud(msg); filter_.setRadiusSearch(search_radius_); filter_.setMinNeighborsInRadius(min_neighbors_in_search_radius_); - pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); - filter_.filter(*cloud_filtered); - pcl::toROSMsg(*cloud_filtered, *msg); + filter_.filter(*msg); } - pub_->publish(*msg); + pub_->publish(msg); }; - sub_ = create_subscription(input_topic_, 10, callback); + sub_ = create_subscription(input_topic_, 10, callback); } } // namespace pcl_apps diff --git a/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp b/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp deleted file mode 100644 index bead956..0000000 --- a/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// 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 - -// Headers in ROS2 -#include - -// Headers in STL -#include -#include -#include - -namespace pcl_apps -{ -VoxelgridFilterComponent::VoxelgridFilterComponent(const rclcpp::NodeOptions & options) -: Node("voxelgrid_filter", options) -{ - declare_parameter("leaf_size", 1.0); - get_parameter("leaf_size", leaf_size_); - declare_parameter("input_topic", get_name() + std::string("/input")); - get_parameter("input_topic", input_topic_); - param_handler_ptr_ = add_on_set_parameters_callback( - [this]( - const std::vector params) -> rcl_interfaces::msg::SetParametersResult { - auto results = std::make_shared(); - for (auto param : params) { - if (param.get_name() == "leaf_size") { - if (leaf_size_ > 0) { - leaf_size_ = param.as_double(); - results->successful = true; - results->reason = ""; - } else { - results->successful = false; - results->reason = "leaf size must over 0"; - } - } - } - if (!results->successful) { - results->successful = false; - results->reason = ""; - } - return *results; - }); - std::string output_topic_name = get_name() + std::string("/output"); - pub_ = create_publisher(output_topic_name, 10); - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { - pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2()); - pcl_conversions::toPCL(*msg, *cloud); - filter_.setInputCloud(cloud); - assert(leaf_size_ > 0.0); - filter_.setLeafSize(leaf_size_, leaf_size_, leaf_size_); - pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2()); - filter_.filter(*cloud_filtered); - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl_conversions::fromPCL(*cloud_filtered, output_cloud_msg); - pub_->publish(output_cloud_msg); - }; - sub_ = create_subscription(input_topic_, 10, callback); -} -} // namespace pcl_apps - -RCLCPP_COMPONENTS_REGISTER_NODE(pcl_apps::VoxelgridFilterComponent) diff --git a/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp b/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp deleted file mode 100644 index 8ac2475..0000000 --- a/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// 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. - -// Headers in this package -#include - -// Headers in RCLCPP -#include - -// Headers in STL -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto component = std::make_shared(options); - rclcpp::spin(component); - rclcpp::shutdown(); - return 0; -} diff --git a/pcl_apps/src/io/CMakeLists.txt b/pcl_apps/src/io/CMakeLists.txt deleted file mode 100644 index 5af7d92..0000000 --- a/pcl_apps/src/io/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -# Pcd Writer Component/Node -ament_auto_add_library(pcd_writer_component SHARED - pcd_writer/pcd_writer_component.cpp -) -target_compile_definitions(pcd_writer_component PRIVATE "PCL_APPS_PCD_WRITER_BUILDING_DLL") -target_link_libraries(pcd_writer_component - boost_system ${PCL_LIBRARIES}) - -ament_auto_add_executable(pcd_writer_node - pcd_writer/pcd_writer_node.cpp) -target_link_libraries(pcd_writer_node - pcd_writer_component ${PCL_LIBRARIES}) - -# Pcd Loader Component/Node -ament_auto_add_library(pcd_loader_component SHARED - pcd_loader/pcd_loader_component.cpp -) -target_compile_definitions(pcd_loader_component PRIVATE "PCL_APPS_PCD_LOADER_BUILDING_DLL") -target_link_libraries(pcd_loader_component - boost_system ${PCL_LIBRARIES}) -ament_auto_add_executable(pcd_loader_node - pcd_loader/pcd_loader_node.cpp) -target_link_libraries(pcd_loader_node - pcd_loader_component ${PCL_LIBRARIES}) diff --git a/pcl_apps/src/matching/CMakeLists.txt b/pcl_apps/src/matching/CMakeLists.txt deleted file mode 100644 index d373582..0000000 --- a/pcl_apps/src/matching/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -# Ndt Matching Component/Node -ament_auto_add_library(ndt_matching_component SHARED - ndt_matching/ndt_matching_component.cpp -) -target_compile_definitions(ndt_matching_component PRIVATE "PCL_APPS_NDT_MATCHING_BUILDING_DLL") -target_link_libraries(ndt_matching_component - ${PCL_LIBRARIES}) - -ament_auto_add_executable(ndt_matching_node - ndt_matching/ndt_matching_node.cpp -) -target_link_libraries(ndt_matching_node - ndt_matching_component ${PCL_LIBRARIES}) - -# Ndt Matching Twist Estimator Component/Node -ament_auto_add_library(ndt_matching_twist_estimator_component SHARED - ndt_matching/ndt_matching_twist_estimator_component.cpp -) -target_compile_definitions(ndt_matching_twist_estimator_component PRIVATE "PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_BUILDING_DLL") -target_link_libraries(ndt_matching_twist_estimator_component - ${PCL_LIBRARIES}) - -ament_auto_add_executable(ndt_matching_twist_estimator_node - ndt_matching/ndt_matching_twist_estimator_node.cpp -) -target_link_libraries(ndt_matching_twist_estimator_node -ndt_matching_twist_estimator_component ${PCL_LIBRARIES}) diff --git a/pcl_apps/src/matching/ndt_matching/ndt_matching_twist_estimator_component.cpp b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp similarity index 98% rename from pcl_apps/src/matching/ndt_matching/ndt_matching_twist_estimator_component.cpp rename to pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp index db23190..20671bb 100644 --- a/pcl_apps/src/matching/ndt_matching/ndt_matching_twist_estimator_component.cpp +++ b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include // Headers in ROS2 #include diff --git a/pcl_apps/src/matching/ndt_matching/ndt_matching_twist_estimator_node.cpp b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_node.cpp similarity index 90% rename from pcl_apps/src/matching/ndt_matching/ndt_matching_twist_estimator_node.cpp rename to pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_node.cpp index 5831343..e524965 100644 --- a/pcl_apps/src/matching/ndt_matching/ndt_matching_twist_estimator_node.cpp +++ b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_node.cpp @@ -13,7 +13,7 @@ // limitations under the License. // Headers in this package -#include +#include // Headers in RCLCPP #include From 9eedffccd05abfa79fd975fec5daacbe19c7de9a Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Fri, 19 Apr 2024 00:04:19 +0900 Subject: [PATCH 07/20] fix cmake Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 40 ++++++++++++++++++-------- pcl_apps/src/projection/CMakeLists.txt | 12 -------- 2 files changed, 28 insertions(+), 24 deletions(-) delete mode 100644 pcl_apps/src/projection/CMakeLists.txt diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 2f7d4b6..7a962ae 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -44,7 +44,7 @@ link_directories( ) add_definitions(${PCL_DEFINITIONS}) -function(add_pcl_apps_component module component_name component_name_in_macro class_name) +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 ) @@ -55,33 +55,49 @@ function(add_pcl_apps_component module component_name component_name_in_macro cl 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}) + ${component_name}_component ${PCL_LIBRARIES}) - rclcpp_components_register_nodes(${component_name}_component pcl_apps::${class_name}) + 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_pcl_apps_component(filter points_concatenate POINTS_CONCATENATE PointsConcatenateComponent) -add_pcl_apps_component(filter points_transform POINTS_TRANSFORM PointsTransformComponent) +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_concatenate_component "pcl_apps::PointsTransformComponent") add_pcl_apps_component(filter radius_outlier_removal RADIUS_OUTLIER_REMOVAL RadiusOutlierRemovalComponent) -add_pcl_apps_component(filter crop_hull_filter CROP_HULL_FILTER CropHullFilterComponent) -add_pcl_apps_component(filter crop_box_filter CROP_BOX_FILTER CropBoxFilterComponent) +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") # Matching Modules -add_pcl_apps_component(matching ndt_matching NDT_MATCHING NdtMatchingComponent) -add_pcl_apps_component(matching ndt_matching_twist_estimator NDT_MATCHING_TWIST_ESTIMATOR NdtMatchingTwistEstimatorComponent) +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_pcl_apps_component(io pcd_writer PCD_WRITER PcdWriter) +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_pcl_apps_component(clustering euclidean_clustering EUCLIDEAN_CLUSTERING EuclideanClusteringComponent) +add_pcl_apps_component(clustering euclidean_clustering EUCLIDEAN_CLUSTERING) +rclcpp_components_register_nodes(pcd_loader_component "pcl_apps::EuclideanClusteringComponent") # Projection Module -add_pcl_apps_component(projection pointcloud_projection POINTCLOUD_PROJECTION PointCloudProjectionComponent) +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) diff --git a/pcl_apps/src/projection/CMakeLists.txt b/pcl_apps/src/projection/CMakeLists.txt deleted file mode 100644 index f3a5855..0000000 --- a/pcl_apps/src/projection/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -# Pointcloud Projection Component/Node -ament_auto_add_library(pointcloud_projection_component SHARED -pointcloud_projection/pointcloud_projection_component.cpp -) -target_compile_definitions(pointcloud_projection_component PRIVATE "PCL_APPS_POINTCLOUD_PROJECTION_BUILDING_DLL") -target_link_libraries(pointcloud_projection_component ${PCL_LIBRARIES}) - -ament_auto_add_executable(pointcloud_projection_node - pointcloud_projection/pointcloud_projection_node.cpp -) -target_link_libraries(pointcloud_projection_node - pointcloud_projection_component) From 6b0a623f25a2b73c396f7046d95b20490ceb9da8 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Fri, 19 Apr 2024 00:09:09 +0900 Subject: [PATCH 08/20] fix cmake Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 7a962ae..ddda054 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -69,7 +69,7 @@ endfunction(add_pcl_apps_component) 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_concatenate_component "pcl_apps::PointsTransformComponent") +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) From e156c98b9149658195a0f68cac1a5cef79d105af Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Fri, 19 Apr 2024 02:29:31 +0900 Subject: [PATCH 09/20] fix clash error in points transform Signed-off-by: hakuturu583 --- .../points_transform_component.cpp | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/pcl_apps/src/filter/points_transform/points_transform_component.cpp b/pcl_apps/src/filter/points_transform/points_transform_component.cpp index 7c18814..c8b3fcb 100644 --- a/pcl_apps/src/filter/points_transform/points_transform_component.cpp +++ b/pcl_apps/src/filter/points_transform/points_transform_component.cpp @@ -47,15 +47,22 @@ PointsTransformComponent::PointsTransformComponent(const rclcpp::NodeOptions & o if (header.frame_id == output_frame_id_) { pub_->publish(msg); } else { - tf2::TimePoint time_point = tf2::TimePoint( - std::chrono::seconds(header.stamp.sec) + std::chrono::nanoseconds(header.stamp.nanosec)); - geometry_msgs::msg::TransformStamped transform_stamped = buffer_.lookupTransform( - output_frame_id_, header.frame_id, time_point, tf2::durationFromSec(1.0)); - Eigen::Matrix4f mat = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - PCLPointCloudTypePtr transformed; - pcl::transformPointCloud(*msg, *transformed, mat); - pub_->publish(transformed); + try { + tf2::TimePoint time_point = tf2::TimePoint( + std::chrono::seconds(header.stamp.sec) + std::chrono::nanoseconds(header.stamp.nanosec)); + geometry_msgs::msg::TransformStamped transform_stamped = buffer_.lookupTransform( + output_frame_id_, header.frame_id, time_point, tf2::durationFromSec(1.0)); + Eigen::Matrix4f mat = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl::transformPointCloud(*msg, *msg, mat); + pub_->publish(msg); + } catch (tf2::ExtrapolationException & ex) { + RCLCPP_ERROR(get_logger(), ex.what()); + return; + } catch (tf2::LookupException & ex) { + RCLCPP_ERROR(get_logger(), ex.what()); + return; + } } }; declare_parameter("input_topic", get_name() + std::string("/input")); From 2a88212bdeeabfffa7ea047a8d48cf735365fc05 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Fri, 19 Apr 2024 02:40:20 +0900 Subject: [PATCH 10/20] modify frame id of the point cloud Signed-off-by: hakuturu583 --- .../src/filter/points_transform/points_transform_component.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/pcl_apps/src/filter/points_transform/points_transform_component.cpp b/pcl_apps/src/filter/points_transform/points_transform_component.cpp index c8b3fcb..e0d4fb6 100644 --- a/pcl_apps/src/filter/points_transform/points_transform_component.cpp +++ b/pcl_apps/src/filter/points_transform/points_transform_component.cpp @@ -55,6 +55,7 @@ PointsTransformComponent::PointsTransformComponent(const rclcpp::NodeOptions & o Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); pcl::transformPointCloud(*msg, *msg, mat); + msg->header.frame_id = output_frame_id_; pub_->publish(msg); } catch (tf2::ExtrapolationException & ex) { RCLCPP_ERROR(get_logger(), ex.what()); From 94eb0216e7da9ad8e706de4f2810a0de202781a5 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Fri, 19 Apr 2024 22:02:01 +0900 Subject: [PATCH 11/20] use type adapter Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 2 + .../euclidean_clustering_component.hpp | 3 +- .../crop_hull_filter_component.hpp | 5 +- .../radius_outlier_removal_component.hpp | 2 +- .../voxelgrid_filter_component.hpp | 9 ++- .../io/pcd_loader/pcd_loader_component.hpp | 5 +- .../io/pcd_writer/pcd_writer_component.hpp | 5 +- .../pointcloud_projection_component.hpp | 3 +- .../euclidean_clustering_component.cpp | 10 +-- .../crop_hull_filter_component.cpp | 18 ++--- .../points_concatenate_component.cpp | 24 +++---- .../voxelgrid_filter_component.cpp | 70 +++++++++++++++++++ .../voxelgrid_filter_node.cpp | 32 +++++++++ .../io/pcd_loader/pcd_loader_component.cpp | 13 ++-- .../pointcloud_projection_component.cpp | 12 ++-- 15 files changed, 156 insertions(+), 57 deletions(-) create mode 100644 pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp create mode 100644 pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index ddda054..668d5c6 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -78,6 +78,8 @@ 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_pcl_apps_component(matching ndt_matching NDT_MATCHING) diff --git a/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp b/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp index d41bd32..fe549ac 100644 --- a/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp +++ b/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp @@ -54,8 +54,7 @@ extern "C" { #endif // Headers in ROS2 -#include - +#include #include #include #include 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 5b52fb8..0f9b76a 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 @@ -59,6 +59,7 @@ extern "C" { #include #include +#include #include #include #include @@ -66,7 +67,7 @@ extern "C" { namespace pcl_apps { typedef message_filters::Subscriber PolygonSubscriber; -typedef message_filters::Subscriber PointCloudSubscriber; +typedef message_filters::Subscriber 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 polygon_sub_; - std::shared_ptr pointcloud_sub_; + std::shared_ptr pointcloud_sub_; }; } // namespace pcl_apps diff --git a/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp b/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp index c9bba27..7cf6622 100644 --- a/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/radius_outlier_removal/radius_outlier_removal_component.hpp @@ -76,7 +76,7 @@ class RadiusOutlierRemovalComponent : public rclcpp::Node std::string input_topic_; PointCloudSubscriber sub_; PointCloudPublisher pub_; - pcl::RadiusOutlierRemoval filter_; + pcl::RadiusOutlierRemoval filter_; double search_radius_; int min_neighbors_in_search_radius_; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_handler_ptr_; diff --git a/pcl_apps/include/pcl_apps/filter/voxelgrid_filter/voxelgrid_filter_component.hpp b/pcl_apps/include/pcl_apps/filter/voxelgrid_filter/voxelgrid_filter_component.hpp index a15de88..60f489b 100644 --- a/pcl_apps/include/pcl_apps/filter/voxelgrid_filter/voxelgrid_filter_component.hpp +++ b/pcl_apps/include/pcl_apps/filter/voxelgrid_filter/voxelgrid_filter_component.hpp @@ -54,8 +54,7 @@ extern "C" { #endif // Headers in ROS2 -#include - +#include #include #include @@ -74,9 +73,9 @@ class VoxelgridFilterComponent : public rclcpp::Node explicit VoxelgridFilterComponent(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_; - pcl::VoxelGrid filter_; + PointCloudSubscriber sub_; + PointCloudPublisher pub_; + pcl::VoxelGrid filter_; double leaf_size_; std::string input_topic_; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_handler_ptr_; diff --git a/pcl_apps/include/pcl_apps/io/pcd_loader/pcd_loader_component.hpp b/pcl_apps/include/pcl_apps/io/pcd_loader/pcd_loader_component.hpp index 44d3d1c..c07724e 100644 --- a/pcl_apps/include/pcl_apps/io/pcd_loader/pcd_loader_component.hpp +++ b/pcl_apps/include/pcl_apps/io/pcd_loader/pcd_loader_component.hpp @@ -54,8 +54,7 @@ extern "C" { #endif // Headers in ROS2 -#include - +#include #include #include @@ -75,7 +74,7 @@ class PcdLoaderComponent : public rclcpp::Node explicit PcdLoaderComponent(const rclcpp::NodeOptions & options); private: - std::shared_ptr> pub_; + PointCloudPublisher pub_; }; } // namespace pcl_apps diff --git a/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp b/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp index 8c8abaf..2f2b854 100644 --- a/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp +++ b/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp @@ -54,8 +54,7 @@ extern "C" { #endif // Headers in ROS2 -#include - +#include #include #include #include @@ -81,7 +80,7 @@ class PcdWriterComponent : public rclcpp::Node bool pointcloud_recieved_; bool save_every_pointcloud_; rclcpp::Service::SharedPtr server_; - pcl::PointCloud cloud_; + pcl::PointCloud cloud_; }; } // namespace pcl_apps 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 eae6d2b..365e3f2 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 @@ -62,6 +62,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -90,7 +91,7 @@ class PointCloudProjectionComponent : public rclcpp::Node explicit PointCloudProjectionComponent(const rclcpp::NodeOptions & options); private: - vision_msgs::msg::BoundingBox3D toBbox(pcl::PointCloud::Ptr pointcloud) const; + vision_msgs::msg::BoundingBox3D toBbox(pcl::PointCloud::Ptr pointcloud) const; rclcpp::Publisher::SharedPtr detection_pub_; rclcpp::Publisher::SharedPtr marker_pub_; visualization_msgs::msg::MarkerArray toMarker( diff --git a/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp b/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp index 37f9f77..3585a08 100644 --- a/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp +++ b/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp @@ -82,11 +82,11 @@ EuclideanClusteringComponent::EuclideanClusteringComponent(const rclcpp::NodeOpt auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { pcl_apps_msgs::msg::PointCloudArray clusters; clusters.header = msg->header; - pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr cloud; pcl::fromROSMsg(*msg, *cloud); std::vector cluster_indices; - pcl::EuclideanClusterExtraction clustering; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + pcl::EuclideanClusterExtraction clustering; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); tree->setInputCloud(cloud); clustering.setClusterTolerance(cluster_tolerance_); clustering.setMinClusterSize(min_cluster_size_); @@ -96,7 +96,7 @@ EuclideanClusteringComponent::EuclideanClusteringComponent(const rclcpp::NodeOpt clustering.extract(cluster_indices); for (auto cluster_itr = cluster_indices.begin(); cluster_itr != cluster_indices.end(); cluster_itr++) { - pcl::PointCloud pointcloud; + pcl::PointCloud pointcloud; pointcloud.width = cluster_itr->indices.size(); pointcloud.height = 1; pointcloud.is_dense = false; @@ -106,7 +106,7 @@ EuclideanClusteringComponent::EuclideanClusteringComponent(const rclcpp::NodeOpt double y = cloud->points[cluster_itr->indices[i]].y; double z = cloud->points[cluster_itr->indices[i]].z; double intensity = cloud->points[cluster_itr->indices[i]].intensity; - pcl::PointXYZI p; + PCLPointType p; p.x = x; p.y = y; p.z = z; 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 b5c9253..bea8ca8 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 @@ -15,7 +15,6 @@ #include #include #include -#include #include @@ -33,7 +32,8 @@ CropHullFilterComponent::CropHullFilterComponent(const rclcpp::NodeOptions & opt { pointcloud_pub_ = this->create_publisher("points_array", 1); polygon_sub_ = std::shared_ptr(new PolygonSubscriber(this, "polygon")); - pointcloud_sub_ = std::shared_ptr(new PointCloudSubscriber(this, "points")); + pointcloud_sub_ = + std::shared_ptr(new ROS2PointCloudSubscriber(this, "points")); sync_ = std::make_shared>( *pointcloud_sub_, *polygon_sub_, 10); @@ -47,17 +47,17 @@ void CropHullFilterComponent::callback( { pcl_apps_msgs::msg::PointCloudArray point_clouds; point_clouds.header = point_cloud->header; - pcl::CropHull filter; - pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); + 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; + pcl::ConvexHull convex_hull; std::vector convex_hull_polygons; - pcl::PointCloud::Ptr hull_cloud(new pcl::PointCloud()); - pcl::PointCloud::Ptr hull_points(new pcl::PointCloud()); + pcl::PointCloud::Ptr hull_cloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr hull_points(new pcl::PointCloud()); for (auto point_itr = poly_itr->points.begin(); point_itr != poly_itr->points.end(); point_itr++) { - pcl::PointXYZI p; + PCLPointType p; p.x = point_itr->x; p.y = point_itr->y; p.z = point_itr->z; @@ -69,7 +69,7 @@ void CropHullFilterComponent::callback( filter.setHullCloud(hull_points); filter.setDim(2); filter.setCropOutside(false); - pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); filter.setInputCloud(pcl_cloud); filter.filter(*filtered); sensor_msgs::msg::PointCloud2 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 72625b8..12a5bd7 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -63,19 +63,19 @@ PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); bool empty = true; if (in0) { empty = false; const PointCloud2Ptr pc = in0.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; const PointCloud2Ptr pc = in1.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } @@ -89,26 +89,26 @@ void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1) void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, CallbackT in2) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); bool empty = true; if (in0) { empty = false; const PointCloud2Ptr pc = in0.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; const PointCloud2Ptr pc = in1.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in2) { empty = false; const PointCloud2Ptr pc = in2.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } @@ -123,33 +123,33 @@ void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, Callbac void PointsConcatenateComponent::callback4( CallbackT in0, CallbackT in1, CallbackT in2, CallbackT in3) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); bool empty = true; if (in0) { empty = false; const PointCloud2Ptr pc = in0.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; const PointCloud2Ptr pc = in1.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in2) { empty = false; const PointCloud2Ptr pc = in2.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } if (in3) { empty = false; const PointCloud2Ptr pc = in3.get(); - pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); pcl::fromROSMsg(*pc, *pc_cloud); *cloud = *pc_cloud + *cloud; } diff --git a/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp b/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp new file mode 100644 index 0000000..4b922de --- /dev/null +++ b/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_component.cpp @@ -0,0 +1,70 @@ +// 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 + +// Headers in ROS2 +#include + +// Headers in STL +#include +#include +#include + +namespace pcl_apps +{ +VoxelgridFilterComponent::VoxelgridFilterComponent(const rclcpp::NodeOptions & options) +: Node("voxelgrid_filter", options) +{ + declare_parameter("leaf_size", 1.0); + get_parameter("leaf_size", leaf_size_); + declare_parameter("input_topic", get_name() + std::string("/input")); + get_parameter("input_topic", input_topic_); + param_handler_ptr_ = add_on_set_parameters_callback( + [this]( + const std::vector params) -> rcl_interfaces::msg::SetParametersResult { + auto results = std::make_shared(); + for (auto param : params) { + if (param.get_name() == "leaf_size") { + if (leaf_size_ > 0) { + leaf_size_ = param.as_double(); + results->successful = true; + results->reason = ""; + } else { + results->successful = false; + results->reason = "leaf size must over 0"; + } + } + } + if (!results->successful) { + results->successful = false; + results->reason = ""; + } + return *results; + }); + std::string output_topic_name = get_name() + std::string("/output"); + pub_ = create_publisher(output_topic_name, 10); + auto callback = [this](const PCLPointCloudTypePtr cloud) -> void { + filter_.setInputCloud(cloud); + assert(leaf_size_ > 0.0); + filter_.setLeafSize(leaf_size_, leaf_size_, leaf_size_); + pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2()); + filter_.filter(*cloud); + pub_->publish(cloud); + }; + sub_ = create_subscription(input_topic_, 10, callback); +} +} // namespace pcl_apps + +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_apps::VoxelgridFilterComponent) diff --git a/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp b/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp new file mode 100644 index 0000000..8ac2475 --- /dev/null +++ b/pcl_apps/src/filter/voxelgrid_filter/voxelgrid_filter_node.cpp @@ -0,0 +1,32 @@ +// 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. + +// Headers in this package +#include + +// Headers in RCLCPP +#include + +// Headers in STL +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/pcl_apps/src/io/pcd_loader/pcd_loader_component.cpp b/pcl_apps/src/io/pcd_loader/pcd_loader_component.cpp index 6f0a777..49865c4 100644 --- a/pcl_apps/src/io/pcd_loader/pcd_loader_component.cpp +++ b/pcl_apps/src/io/pcd_loader/pcd_loader_component.cpp @@ -34,15 +34,12 @@ PcdLoaderComponent::PcdLoaderComponent(const rclcpp::NodeOptions & options) std::string inference_id; declare_parameter("inference_id", std::string("map")); get_parameter("inference_id", inference_id); - pcl::PCLPointCloud2 cloud; - int result = pcl::io::loadPCDFile(file_path, cloud); + PCLPointCloudTypePtr cloud = PCLPointCloudTypePtr(new PCLPointCloudType); + int result = pcl::io::loadPCDFile(file_path, *cloud); if (result == 0) { - sensor_msgs::msg::PointCloud2 msg; - pcl_conversions::fromPCL(cloud, msg); - msg.header.frame_id = inference_id; - pub_ = create_publisher( - output_topic, rclcpp::QoS(10).transient_local()); - pub_->publish(msg); + cloud->header.frame_id = inference_id; + pub_ = create_publisher(output_topic, rclcpp::QoS(10).transient_local()); + pub_->publish(cloud); } else { RCLCPP_ERROR_STREAM(get_logger(), "Failed to load " << file_path); } 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 a03e407..34721b0 100644 --- a/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp +++ b/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp @@ -73,13 +73,13 @@ PointCloudProjectionComponent::PointCloudProjectionComponent( } vision_msgs::msg::BoundingBox3D PointCloudProjectionComponent::toBbox( - pcl::PointCloud::Ptr pointcloud) const + pcl::PointCloud::Ptr pointcloud) const { vision_msgs::msg::BoundingBox3D bbox; - pcl::MomentOfInertiaEstimation feature_extractor; - pcl::PointXYZI min_point; - pcl::PointXYZI max_point; - pcl::PointXYZI position; + pcl::MomentOfInertiaEstimation feature_extractor; + PCLPointType min_point; + PCLPointType max_point; + PCLPointType position; Eigen::Matrix3f rotational_matrix; feature_extractor.setInputCloud(pointcloud); feature_extractor.compute(); @@ -146,7 +146,7 @@ void PointCloudProjectionComponent::callback( polygon_type poly; typedef boost::geometry::ring_type::type ring_type; ring_type & ring = boost::geometry::exterior_ring(poly); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); pcl::fromROSMsg(point_cloud, *cloud); for (auto point_itr = cloud->begin(); point_itr != cloud->end(); point_itr++) { geometry_msgs::msg::PointStamped p; From 9c0efa9483fd2634c3a159f71bdcb32b9939fda8 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Fri, 19 Apr 2024 23:52:26 +0900 Subject: [PATCH 12/20] fix compile erros and use std::optional Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 4 +- .../points_concatenate_component.hpp | 32 ++--- ...ndt_matching_twist_estimator_component.hpp | 3 +- .../pointcloud_projection_component.hpp | 4 +- .../points_concatenate_component.cpp | 114 +++++++----------- ...ndt_matching_twist_estimator_component.cpp | 8 +- .../pointcloud_projection_component.cpp | 22 ++-- 7 files changed, 74 insertions(+), 113 deletions(-) diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 668d5c6..39a0c0d 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -66,8 +66,8 @@ endfunction(add_pcl_apps_component) # Filter Modules -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_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) 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 cf6b3b2..4d19af4 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,16 @@ 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::optional CallbackT; +typedef message_synchronizer::MessageSynchronizer2 + Sync2T; typedef std::shared_ptr Sync2PtrT; -typedef message_synchronizer::MessageSynchronizer3 Sync3T; +typedef message_synchronizer::MessageSynchronizer3< + PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType> + Sync3T; typedef std::shared_ptr Sync3PtrT; typedef message_synchronizer::MessageSynchronizer4< - PointCloud2, PointCloud2, PointCloud2, PointCloud2> + PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType> Sync4T; typedef std::shared_ptr Sync4PtrT; @@ -89,20 +90,11 @@ class PointsConcatenateComponent : public rclcpp::Node explicit PointsConcatenateComponent(const rclcpp::NodeOptions & options); private: - 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_; - */ - rclcpp::Publisher::SharedPtr pub_; + void callback2(const CallbackT & in0, const CallbackT & in1); + void callback3(const CallbackT & in0, const CallbackT & in1, const CallbackT & in2); + void callback4( + const CallbackT & in0, const CallbackT & in1, const CallbackT & in2, const CallbackT & in3); + PointCloudPublisher pub_; Sync2PtrT sync2_; Sync3PtrT sync3_; Sync4PtrT sync4_; diff --git a/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp b/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp index 65e9e88..64ccd4f 100644 --- a/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp +++ b/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp @@ -75,7 +75,6 @@ extern "C" { // Headers in Boost #include -#include // Headers in STL #include @@ -99,7 +98,7 @@ class NdtMatchingTwistEstimatorComponent : public rclcpp::Node double resolution_; int max_iterations_; std::string input_cloud_frame_id_; - boost::optional estimateCurrentTwist(); + std::optional estimateCurrentTwist(); pcl::NormalDistributionsTransform ndt_; double toSec(rclcpp::Duration duration) { 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 365e3f2..b4881ab 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 @@ -78,8 +78,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 12a5bd7..ad2b136 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -29,135 +29,105 @@ PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions get_parameter("num_input", num_input_); assert(num_input_ >= 2 && num_input_ <= 4); std::string output_topic_name = get_name() + std::string("/output"); - pub_ = create_publisher(output_topic_name, 10); + pub_ = create_publisher(output_topic_name, 10); for (int i = 0; i < num_input_; i++) { declare_parameter( "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]); } 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})); - auto func2 = std::bind( - &PointsConcatenateComponent::callback2, this, std::placeholders::_1, std::placeholders::_2); - sync2_->registerCallback(func2); + sync2_ = std::shared_ptr( + new Sync2T( + this, {input_topics_[0], input_topics_[1]}, std::chrono::milliseconds{100}, + std::chrono::milliseconds{100}), + options, [](const auto & data) { pcl_conversions::fromPCL(); }); + // 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})); - auto func3 = std::bind( - &PointsConcatenateComponent::callback3, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3); - sync3_->registerCallback(func3); + // sync3_ = std::shared_ptr(new Sync3T( + // this, {input_topics_[0], input_topics_[1], input_topics_[2]}, std::chrono::milliseconds{100}, + // std::chrono::milliseconds{100})); + // 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(new Sync4T( - this, {input_topics_[0], input_topics_[1], input_topics_[2], input_topics_[3]}, - std::chrono::milliseconds{100}, std::chrono::milliseconds{100})); - auto func4 = std::bind( - &PointsConcatenateComponent::callback4, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4); - sync4_->registerCallback(func4); + // 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})); + // auto func4 = std::bind( + // &PointsConcatenateComponent::callback4, this, std::placeholders::_1, std::placeholders::_2, + // std::placeholders::_3, std::placeholders::_4); + // sync4_->registerCallback(func4); } } -void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1) +void PointsConcatenateComponent::callback2(const CallbackT & in0, const CallbackT & in1) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 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(); } if (!empty) { - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl::toROSMsg(*cloud, output_cloud_msg); - output_cloud_msg.header.stamp = sync2_->getPollTimestamp(); - pub_->publish(output_cloud_msg); + cloud->header.stamp = pcl_conversions::toPCL(sync2_->getPollTimestamp()); + pub_->publish(cloud); } } -void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, CallbackT in2) +void PointsConcatenateComponent::callback3( + const CallbackT & in0, const CallbackT & in1, const CallbackT & in2) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 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(); } 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(); } if (!empty) { - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl::toROSMsg(*cloud, output_cloud_msg); - output_cloud_msg.header.stamp = sync3_->getPollTimestamp(); - pub_->publish(output_cloud_msg); + cloud->header.stamp = pcl_conversions::toPCL(sync3_->getPollTimestamp()); + pub_->publish(cloud); } } void PointsConcatenateComponent::callback4( - CallbackT in0, CallbackT in1, CallbackT in2, CallbackT in3) + const CallbackT & in0, const CallbackT & in1, const CallbackT & in2, const CallbackT & in3) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 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(); } 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(); } 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(); } if (!empty) { - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl::toROSMsg(*cloud, output_cloud_msg); - output_cloud_msg.header.stamp = sync4_->getPollTimestamp(); - pub_->publish(output_cloud_msg); + cloud->header.stamp = pcl_conversions::toPCL(sync4_->getPollTimestamp()); + pub_->publish(cloud); } } } // namespace pcl_apps diff --git a/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp index 20671bb..b054cec 100644 --- a/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp +++ b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp @@ -106,16 +106,16 @@ NdtMatchingTwistEstimatorComponent::NdtMatchingTwistEstimatorComponent( pcl::PointCloud::Ptr input_cloud; pcl::fromROSMsg(*msg, *input_cloud); buffer_.push_back(input_cloud); - boost::optional twist = estimateCurrentTwist(); + std::optional twist = estimateCurrentTwist(); if (twist) { - current_twist_pub_->publish(twist.get()); + current_twist_pub_->publish(twist.value()); } }; sub_input_cloud_ = create_subscription(input_cloud_topic_, 10, callback); } -boost::optional +std::optional NdtMatchingTwistEstimatorComponent::estimateCurrentTwist() { assert(timestamps_.size() == buffer_.size()); @@ -161,7 +161,7 @@ NdtMatchingTwistEstimatorComponent::estimateCurrentTwist() twist.twist.angular.z = yaw / diff_time; return twist; } - return boost::none; + return std::nullopt; } } // namespace pcl_apps 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 34721b0..7bd74e5 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 4fb0d8786cd7c061cf5b3ec9b6b5a3287fb2e429 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 00:44:27 +0900 Subject: [PATCH 13/20] fix compile errors Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 4 +- .../points_concatenate_component.hpp | 18 +++---- .../points_concatenate_component.cpp | 51 +++++++++++-------- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 39a0c0d..668d5c6 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -66,8 +66,8 @@ endfunction(add_pcl_apps_component) # Filter Modules -# 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_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) 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 4d19af4..3e45cef 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 @@ -71,17 +71,17 @@ extern "C" { namespace pcl_apps { typedef std::optional CallbackT; -typedef message_synchronizer::MessageSynchronizer2 +typedef message_synchronizer::MessageSynchronizer2< + PointCloudAdapterType, PointCloudAdapterType, PCLPointCloudType, PCLPointCloudType> Sync2T; -typedef std::shared_ptr Sync2PtrT; typedef message_synchronizer::MessageSynchronizer3< - PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType> + PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, PCLPointCloudType, + PCLPointCloudType, PCLPointCloudType> Sync3T; -typedef std::shared_ptr Sync3PtrT; typedef message_synchronizer::MessageSynchronizer4< - PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType> + PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, + PCLPointCloudType, PCLPointCloudType, PCLPointCloudType, PCLPointCloudType> Sync4T; -typedef std::shared_ptr Sync4PtrT; class PointsConcatenateComponent : public rclcpp::Node { @@ -95,9 +95,9 @@ class PointsConcatenateComponent : public rclcpp::Node void callback4( const CallbackT & in0, const CallbackT & in1, const CallbackT & in2, const 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_; }; 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 ad2b136..6b1b987 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -35,31 +35,38 @@ 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 std::shared_ptr & data) -> rclcpp::Time { + 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}), - options, [](const auto & data) { pcl_conversions::fromPCL(); }); - // auto func2 = std::bind( - // &PointsConcatenateComponent::callback2, this, std::placeholders::_1, std::placeholders::_2); - // sync2_->registerCallback(func2); + sync2_ = std::make_shared(Sync2T( + this, {input_topics_[0], input_topics_[1]}, 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})); - // auto func3 = std::bind( - // &PointsConcatenateComponent::callback3, this, std::placeholders::_1, std::placeholders::_2, - // std::placeholders::_3); - // sync3_->registerCallback(func3); + sync3_ = std::make_shared(Sync3T( + this, {input_topics_[0], input_topics_[1], input_topics_[2]}, 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); + sync3_->registerCallback(func3); } 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})); - // auto func4 = std::bind( - // &PointsConcatenateComponent::callback4, this, std::placeholders::_1, std::placeholders::_2, - // std::placeholders::_3, std::placeholders::_4); - // sync4_->registerCallback(func4); + sync4_ = std::make_shared(Sync4T( + this, {input_topics_[0], input_topics_[1], input_topics_[2], input_topics_[3]}, + 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); + sync4_->registerCallback(func4); } } From 4e4624a35edcc3954e51d9961401cfe860550010 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 02:45:50 +0900 Subject: [PATCH 14/20] change type and avoid runtime error Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 2 +- .../points_concatenate_component.hpp | 4 ++- .../pointcloud_projection_component.hpp | 4 +-- .../points_concatenate_component.cpp | 25 +++++++++++-------- .../pointcloud_projection_component.cpp | 20 +++++++-------- 5 files changed, 30 insertions(+), 25 deletions(-) diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 668d5c6..62c7aba 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -50,7 +50,7 @@ function(add_pcl_apps_component module component_name component_name_in_macro) ) 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}) + boost_system ${PCL_LIBRARIES} glog) ament_auto_add_executable(${component_name}_node src/${module}/${component_name}/${component_name}_node.cpp) 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 3e45cef..679a166 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 @@ -68,9 +68,11 @@ extern "C" { #include #include +#include + namespace pcl_apps { -typedef std::optional CallbackT; +typedef std::optional CallbackT; typedef message_synchronizer::MessageSynchronizer2< PointCloudAdapterType, PointCloudAdapterType, PCLPointCloudType, PCLPointCloudType> Sync2T; 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 b4881ab..d9f1769 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 @@ -78,8 +78,8 @@ typedef std::shared_ptr PointCloudArrayTPtr; typedef message_synchronizer::MessageSynchronizer2 CameraInfoAndPoints; -typedef const std::optional & CameraInfoCallbackT; -typedef const std::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 6b1b987..477e9e9 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -25,6 +25,9 @@ namespace pcl_apps PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions & options) : Node("points_concatenate", options) { + google::InitGoogleLogging(""); + google::InstallFailureSignalHandler(); + declare_parameter("num_input", 2); get_parameter("num_input", num_input_); assert(num_input_ >= 2 && num_input_ <= 4); @@ -35,8 +38,8 @@ 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 std::shared_ptr & data) -> rclcpp::Time { - return pcl_conversions::fromPCL(data->header.stamp); + const auto get_timestamp = [](const PCLPointCloudType & data) -> rclcpp::Time { + return pcl_conversions::fromPCL(data.header.stamp); }; if (num_input_ == 2) { sync2_ = std::make_shared(Sync2T( @@ -76,11 +79,11 @@ void PointsConcatenateComponent::callback2(const CallbackT & in0, const Callback bool empty = true; if (in0) { empty = false; - *cloud += *in0.value(); + *cloud += in0.value(); } if (in1) { empty = false; - *cloud += *in1.value(); + *cloud += in1.value(); } if (!empty) { cloud->header.stamp = pcl_conversions::toPCL(sync2_->getPollTimestamp()); @@ -95,15 +98,15 @@ void PointsConcatenateComponent::callback3( bool empty = true; if (in0) { empty = false; - *cloud += *in0.value(); + *cloud += in0.value(); } if (in1) { empty = false; - *cloud += *in1.value(); + *cloud += in1.value(); } if (in2) { empty = false; - *cloud += *in2.value(); + *cloud += in2.value(); } if (!empty) { cloud->header.stamp = pcl_conversions::toPCL(sync3_->getPollTimestamp()); @@ -118,19 +121,19 @@ void PointsConcatenateComponent::callback4( bool empty = true; if (in0) { empty = false; - *cloud += *in0.value(); + *cloud += in0.value(); } if (in1) { empty = false; - *cloud += *in1.value(); + *cloud += in1.value(); } if (in2) { empty = false; - *cloud += *in2.value(); + *cloud += in2.value(); } if (in3) { empty = false; - *cloud += *in3.value(); + *cloud += in3.value(); } 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 7bd74e5..13cafb7 100644 --- a/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp +++ b/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp @@ -129,8 +129,8 @@ void PointCloudProjectionComponent::callback( geometry_msgs::msg::TransformStamped transform_stamped; try { transform_stamped = buffer_.lookupTransform( - camera_info.value()->header.frame_id, point_clouds.value()->header.frame_id, - point_clouds.value()->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.value()->width, camera_info.value()->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.value()->header.frame_id; - detection_array.header.stamp = point_clouds.value()->header.stamp; - for (const auto & point_cloud : point_clouds.value()->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.value()->header.frame_id; - detection.header.stamp = point_clouds.value()->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.value()->header.frame_id; - bbox_header.stamp = point_clouds.value()->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 517af5a3885cce144fa4e6dfb00435fa3cd85543 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 20:40:46 +0900 Subject: [PATCH 15/20] fix compile error Signed-off-by: hakuturu583 --- .../points_concatenate_component.hpp | 51 +++++---- .../pointcloud_projection_component.hpp | 9 +- .../points_concatenate_component.cpp | 102 +++++++++++------- .../pointcloud_projection_component.cpp | 36 +++---- 4 files changed, 112 insertions(+), 86 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 679a166..835c8e5 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 @@ -54,8 +54,9 @@ extern "C" { #endif // Headers in ROS2 +#include + #include -#include #include #include @@ -68,22 +69,19 @@ extern "C" { #include #include -#include - namespace pcl_apps { -typedef std::optional CallbackT; -typedef message_synchronizer::MessageSynchronizer2< - PointCloudAdapterType, PointCloudAdapterType, PCLPointCloudType, PCLPointCloudType> - Sync2T; -typedef message_synchronizer::MessageSynchronizer3< - PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, PCLPointCloudType, - PCLPointCloudType, PCLPointCloudType> - Sync3T; +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< - PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, PointCloudAdapterType, - PCLPointCloudType, PCLPointCloudType, PCLPointCloudType, PCLPointCloudType> + PointCloud2, PointCloud2, PointCloud2, PointCloud2> Sync4T; +typedef std::shared_ptr Sync4PtrT; class PointsConcatenateComponent : public rclcpp::Node { @@ -92,17 +90,26 @@ class PointsConcatenateComponent : public rclcpp::Node explicit PointsConcatenateComponent(const rclcpp::NodeOptions & options); private: - void callback2(const CallbackT & in0, const CallbackT & in1); - void callback3(const CallbackT & in0, const CallbackT & in1, const CallbackT & in2); - void callback4( - const CallbackT & in0, const CallbackT & in1, const CallbackT & in2, const CallbackT & in3); - PointCloudPublisher pub_; - std::shared_ptr sync2_; - std::shared_ptr sync3_; - std::shared_ptr sync4_; + 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_; + */ + rclcpp::Publisher::SharedPtr pub_; + Sync2PtrT sync2_; + Sync3PtrT sync3_; + Sync4PtrT sync4_; std::array input_topics_; int num_input_; }; } // namespace pcl_apps -#endif // PCL_APPS__FILTER__POINTS_CONCATENATE__POINTS_CONCATENATE_COMPONENT_HPP_ +#endif // PCL_APPS__FILTER__POINTS_CONCATENATE__POINTS_CONCATENATE_COMPONENT_HPP_ \ No newline at end of file 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 d9f1769..35fd37e 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 @@ -62,7 +62,6 @@ extern "C" { #include #include #include -#include #include #include #include @@ -78,8 +77,8 @@ typedef std::shared_ptr PointCloudArrayTPtr; typedef message_synchronizer::MessageSynchronizer2 CameraInfoAndPoints; -typedef const std::optional & CameraInfoCallbackT; -typedef const std::optional & PointCloudsCallbackT; +typedef const boost::optional & CameraInfoCallbackT; +typedef const boost::optional & PointCloudsCallbackT; class PointCloudProjectionComponent : public rclcpp::Node { @@ -91,7 +90,7 @@ class PointCloudProjectionComponent : public rclcpp::Node explicit PointCloudProjectionComponent(const rclcpp::NodeOptions & options); private: - vision_msgs::msg::BoundingBox3D toBbox(pcl::PointCloud::Ptr pointcloud) const; + vision_msgs::msg::BoundingBox3D toBbox(pcl::PointCloud::Ptr pointcloud) const; rclcpp::Publisher::SharedPtr detection_pub_; rclcpp::Publisher::SharedPtr marker_pub_; visualization_msgs::msg::MarkerArray toMarker( @@ -103,4 +102,4 @@ class PointCloudProjectionComponent : public rclcpp::Node }; } // namespace pcl_apps -#endif // PCL_APPS__PROJECTION__POINTCLOUD_PROJECTION__POINTCLOUD_PROJECTION_COMPONENT_HPP_ +#endif // PCL_APPS__PROJECTION__POINTCLOUD_PROJECTION__POINTCLOUD_PROJECTION_COMPONENT_HPP_ \ No newline at end of file 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 477e9e9..72625b8 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -25,47 +25,35 @@ namespace pcl_apps PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions & options) : Node("points_concatenate", options) { - google::InitGoogleLogging(""); - google::InstallFailureSignalHandler(); - declare_parameter("num_input", 2); get_parameter("num_input", num_input_); assert(num_input_ >= 2 && num_input_ <= 4); std::string output_topic_name = get_name() + std::string("/output"); - pub_ = create_publisher(output_topic_name, 10); + pub_ = create_publisher(output_topic_name, 10); for (int i = 0; i < num_input_; i++) { declare_parameter( "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 PCLPointCloudType & data) -> rclcpp::Time { - return pcl_conversions::fromPCL(data.header.stamp); - }; if (num_input_ == 2) { - sync2_ = std::make_shared(Sync2T( + sync2_ = std::shared_ptr(new Sync2T( this, {input_topics_[0], input_topics_[1]}, std::chrono::milliseconds{100}, - std::chrono::milliseconds{100}, - rclcpp::SubscriptionOptionsWithAllocator>(), get_timestamp, - get_timestamp)); + std::chrono::milliseconds{100})); auto func2 = std::bind( &PointsConcatenateComponent::callback2, this, std::placeholders::_1, std::placeholders::_2); sync2_->registerCallback(func2); } else if (num_input_ == 3) { - sync3_ = std::make_shared(Sync3T( + sync3_ = std::shared_ptr(new Sync3T( this, {input_topics_[0], input_topics_[1], input_topics_[2]}, std::chrono::milliseconds{100}, - std::chrono::milliseconds{100}, - rclcpp::SubscriptionOptionsWithAllocator>(), get_timestamp, - get_timestamp, get_timestamp)); + std::chrono::milliseconds{100})); 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::make_shared(Sync4T( + 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}, - rclcpp::SubscriptionOptionsWithAllocator>(), get_timestamp, - get_timestamp, get_timestamp, get_timestamp)); + std::chrono::milliseconds{100}, std::chrono::milliseconds{100})); auto func4 = std::bind( &PointsConcatenateComponent::callback4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4); @@ -73,71 +61,103 @@ PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions } } -void PointsConcatenateComponent::callback2(const CallbackT & in0, const CallbackT & in1) +void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); bool empty = true; if (in0) { empty = false; - *cloud += in0.value(); + const PointCloud2Ptr pc = in0.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; - *cloud += in1.value(); + const PointCloud2Ptr pc = in1.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (!empty) { - cloud->header.stamp = pcl_conversions::toPCL(sync2_->getPollTimestamp()); - pub_->publish(cloud); + sensor_msgs::msg::PointCloud2 output_cloud_msg; + pcl::toROSMsg(*cloud, output_cloud_msg); + output_cloud_msg.header.stamp = sync2_->getPollTimestamp(); + pub_->publish(output_cloud_msg); } } -void PointsConcatenateComponent::callback3( - const CallbackT & in0, const CallbackT & in1, const CallbackT & in2) +void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, CallbackT in2) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); bool empty = true; if (in0) { empty = false; - *cloud += in0.value(); + const PointCloud2Ptr pc = in0.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; - *cloud += in1.value(); + const PointCloud2Ptr pc = in1.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (in2) { empty = false; - *cloud += in2.value(); + const PointCloud2Ptr pc = in2.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (!empty) { - cloud->header.stamp = pcl_conversions::toPCL(sync3_->getPollTimestamp()); - pub_->publish(cloud); + sensor_msgs::msg::PointCloud2 output_cloud_msg; + pcl::toROSMsg(*cloud, output_cloud_msg); + output_cloud_msg.header.stamp = sync3_->getPollTimestamp(); + pub_->publish(output_cloud_msg); } } void PointsConcatenateComponent::callback4( - const CallbackT & in0, const CallbackT & in1, const CallbackT & in2, const CallbackT & in3) + CallbackT in0, CallbackT in1, CallbackT in2, CallbackT in3) { - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); bool empty = true; if (in0) { empty = false; - *cloud += in0.value(); + const PointCloud2Ptr pc = in0.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (in1) { empty = false; - *cloud += in1.value(); + const PointCloud2Ptr pc = in1.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (in2) { empty = false; - *cloud += in2.value(); + const PointCloud2Ptr pc = in2.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (in3) { empty = false; - *cloud += in3.value(); + const PointCloud2Ptr pc = in3.get(); + pcl::PointCloud::Ptr pc_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*pc, *pc_cloud); + *cloud = *pc_cloud + *cloud; } if (!empty) { - cloud->header.stamp = pcl_conversions::toPCL(sync4_->getPollTimestamp()); - pub_->publish(cloud); + sensor_msgs::msg::PointCloud2 output_cloud_msg; + pcl::toROSMsg(*cloud, output_cloud_msg); + output_cloud_msg.header.stamp = sync4_->getPollTimestamp(); + pub_->publish(output_cloud_msg); } } } // namespace pcl_apps 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 13cafb7..6f22c54 100644 --- a/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp +++ b/pcl_apps/src/projection/pointcloud_projection/pointcloud_projection_component.cpp @@ -73,13 +73,13 @@ PointCloudProjectionComponent::PointCloudProjectionComponent( } vision_msgs::msg::BoundingBox3D PointCloudProjectionComponent::toBbox( - pcl::PointCloud::Ptr pointcloud) const + pcl::PointCloud::Ptr pointcloud) const { vision_msgs::msg::BoundingBox3D bbox; - pcl::MomentOfInertiaEstimation feature_extractor; - PCLPointType min_point; - PCLPointType max_point; - PCLPointType position; + pcl::MomentOfInertiaEstimation feature_extractor; + pcl::PointXYZI min_point; + pcl::PointXYZI max_point; + pcl::PointXYZI position; Eigen::Matrix3f rotational_matrix; feature_extractor.setInputCloud(pointcloud); feature_extractor.compute(); @@ -125,12 +125,12 @@ void PointCloudProjectionComponent::callback( return; } image_geometry::PinholeCameraModel cam_model; - cam_model.fromCameraInfo(camera_info.value()); + cam_model.fromCameraInfo(camera_info.get()); geometry_msgs::msg::TransformStamped transform_stamped; try { transform_stamped = buffer_.lookupTransform( - camera_info.value().header.frame_id, point_clouds.value().header.frame_id, - point_clouds.value().header.stamp, tf2::durationFromSec(1.0)); + camera_info.get()->header.frame_id, point_clouds.get()->header.frame_id, + point_clouds.get()->header.stamp, tf2::durationFromSec(1.0)); } catch (tf2::ExtrapolationException & ex) { RCLCPP_ERROR(get_logger(), ex.what()); return; @@ -138,15 +138,15 @@ 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.value().width, camera_info.value().height)); + box camera_bbox(point(0, 0), point(camera_info.get()->width, camera_info.get()->height)); perception_msgs::msg::Detection2DArray detection_array; - 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) { + 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) { polygon_type poly; typedef boost::geometry::ring_type::type ring_type; ring_type & ring = boost::geometry::exterior_ring(poly); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); pcl::fromROSMsg(point_cloud, *cloud); for (auto point_itr = cloud->begin(); point_itr != cloud->end(); point_itr++) { geometry_msgs::msg::PointStamped p; @@ -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.value().header.frame_id; - detection.header.stamp = point_clouds.value().header.stamp; + detection.header.frame_id = camera_info.get()->header.frame_id; + detection.header.stamp = point_clouds.get()->header.stamp; std_msgs::msg::Header bbox_header; - bbox_header.frame_id = point_clouds.value().header.frame_id; - bbox_header.stamp = point_clouds.value().header.stamp; + bbox_header.frame_id = point_clouds.get()->header.frame_id; + bbox_header.stamp = point_clouds.get()->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; @@ -188,4 +188,4 @@ void PointCloudProjectionComponent::callback( } } // namespace pcl_apps -RCLCPP_COMPONENTS_REGISTER_NODE(pcl_apps::PointCloudProjectionComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_apps::PointCloudProjectionComponent) \ No newline at end of file From 9ac1dcfa154d149ec634db02460c9284fde666f4 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 20:52:20 +0900 Subject: [PATCH 16/20] enable type adaptation in euclidian clustering Signed-off-by: hakuturu583 --- .../euclidean_clustering_component.hpp | 2 +- .../euclidean_clustering_component.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp b/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp index fe549ac..cb970fd 100644 --- a/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp +++ b/pcl_apps/include/pcl_apps/clustering/euclidean_clustering/euclidean_clustering_component.hpp @@ -79,7 +79,7 @@ class EuclideanClusteringComponent : public rclcpp::Node explicit EuclideanClusteringComponent(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_; + PointCloudSubscriber sub_; rclcpp::Publisher::SharedPtr pub_; std::string input_topic_; double cluster_tolerance_; diff --git a/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp b/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp index 3585a08..a36bfb3 100644 --- a/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp +++ b/pcl_apps/src/clustering/euclidean_clustering/euclidean_clustering_component.cpp @@ -79,11 +79,11 @@ EuclideanClusteringComponent::EuclideanClusteringComponent(const rclcpp::NodeOpt }); std::string output_topic_name = get_name() + std::string("/output"); pub_ = create_publisher(output_topic_name, 10); - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { + auto callback = [this](const PCLPointCloudTypePtr cloud) -> void { pcl_apps_msgs::msg::PointCloudArray clusters; - clusters.header = msg->header; - pcl::PointCloud::Ptr cloud; - pcl::fromROSMsg(*msg, *cloud); + clusters.header = pcl_conversions::fromPCL(cloud->header); + // pcl::PointCloud::Ptr cloud; + // pcl::fromROSMsg(*msg, *cloud); std::vector cluster_indices; pcl::EuclideanClusterExtraction clustering; pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); @@ -115,12 +115,12 @@ EuclideanClusteringComponent::EuclideanClusteringComponent(const rclcpp::NodeOpt } sensor_msgs::msg::PointCloud2 pointcloud_msg; pcl::toROSMsg(pointcloud, pointcloud_msg); - pointcloud_msg.header = msg->header; + pointcloud_msg.header = pcl_conversions::fromPCL(cloud->header); clusters.cloud.push_back(pointcloud_msg); } pub_->publish(clusters); }; - sub_ = create_subscription(input_topic_, 10, callback); + sub_ = create_subscription(input_topic_, 10, callback); } } // namespace pcl_apps From 17870412fd5d6b8a8dd20a6137c39b3a4f2d6ab0 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 22:30:49 +0900 Subject: [PATCH 17/20] use type adapter in ndt_matching and pcd_writer Signed-off-by: hakuturu583 --- .../io/pcd_writer/pcd_writer_component.hpp | 4 +-- .../ndt_matching/ndt_matching_component.hpp | 17 ++++++----- .../io/pcd_writer/pcd_writer_component.cpp | 12 ++++---- .../ndt_matching/ndt_matching_component.cpp | 29 +++++++------------ 4 files changed, 28 insertions(+), 34 deletions(-) diff --git a/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp b/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp index 2f2b854..3ad4205 100644 --- a/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp +++ b/pcl_apps/include/pcl_apps/io/pcd_writer/pcd_writer_component.hpp @@ -75,12 +75,12 @@ class PcdWriterComponent : public rclcpp::Node explicit PcdWriterComponent(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_; + PointCloudSubscriber sub_; std::string input_topic_; bool pointcloud_recieved_; bool save_every_pointcloud_; rclcpp::Service::SharedPtr server_; - pcl::PointCloud cloud_; + PCLPointCloudTypePtr cloud_; }; } // namespace pcl_apps diff --git a/pcl_apps/include/pcl_apps/matching/ndt_matching/ndt_matching_component.hpp b/pcl_apps/include/pcl_apps/matching/ndt_matching/ndt_matching_component.hpp index ec14921..a9c1cfc 100644 --- a/pcl_apps/include/pcl_apps/matching/ndt_matching/ndt_matching_component.hpp +++ b/pcl_apps/include/pcl_apps/matching/ndt_matching/ndt_matching_component.hpp @@ -54,9 +54,10 @@ extern "C" { #endif // Headers in ROS2 -#include #include #include + +#include #ifdef USE_TF2_EIGEN_DEPRECATED_HEADER #include #else @@ -110,7 +111,7 @@ class NdtMatchingComponent : public rclcpp::Node double step_size_; double resolution_; int max_iterations_; - pcl::PointCloud::Ptr reference_cloud_; + PCLPointCloudTypePtr reference_cloud_; std::shared_ptr broadcaster_; bool reference_cloud_recieved_; bool initial_pose_recieved_; @@ -120,14 +121,14 @@ class NdtMatchingComponent : public rclcpp::Node tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; void publishTF( - const std::string frame_id, const std::string child_frame_id, - const geometry_msgs::msg::PoseStamped pose); - rclcpp::Subscription::ConstSharedPtr sub_reference_cloud_; - rclcpp::Subscription::SharedPtr sub_input_cloud_; + const std::string & frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::PoseStamped & pose); + PointCloudSubscriber sub_reference_cloud_; + PointCloudSubscriber sub_input_cloud_; rclcpp::Subscription::SharedPtr sub_initial_pose_; rclcpp::Publisher::SharedPtr current_relative_pose_pub_; - void updateRelativePose(pcl::PointCloud::Ptr input_cloud, rclcpp::Time stamp); - std::shared_ptr> ndt_; + void updateRelativePose(PCLPointCloudTypePtr input_cloud, rclcpp::Time stamp); + std::shared_ptr> ndt_; geometry_msgs::msg::PoseStamped current_relative_pose_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_handler_ptr_; }; diff --git a/pcl_apps/src/io/pcd_writer/pcd_writer_component.cpp b/pcl_apps/src/io/pcd_writer/pcd_writer_component.cpp index 0f07d8a..61ac650 100644 --- a/pcl_apps/src/io/pcd_writer/pcd_writer_component.cpp +++ b/pcl_apps/src/io/pcd_writer/pcd_writer_component.cpp @@ -30,11 +30,11 @@ PcdWriterComponent::PcdWriterComponent(const rclcpp::NodeOptions & options) declare_parameter("input_topic", get_name() + std::string("/input")); get_parameter("input_topic", input_topic_); pointcloud_recieved_ = false; - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { - pcl::fromROSMsg(*msg, cloud_); + auto callback = [this](const PCLPointCloudTypePtr msg) -> void { + cloud_ = msg; pointcloud_recieved_ = true; }; - sub_ = create_subscription(input_topic_, 10, callback); + sub_ = create_subscription(input_topic_, 10, callback); auto write_pcd_callback = [this]( const std::shared_ptr request_header, @@ -44,11 +44,11 @@ PcdWriterComponent::PcdWriterComponent(const rclcpp::NodeOptions & options) if (pointcloud_recieved_) { int result = 0; if (request->format == request->ASCII) { - result = pcl::io::savePCDFileASCII(request->path, cloud_); + result = pcl::io::savePCDFileASCII(request->path, *cloud_); } else if (request->format == request->BINARY) { - result = pcl::io::savePCDFileBinary(request->path, cloud_); + result = pcl::io::savePCDFileBinary(request->path, *cloud_); } else if (request->format == request->BINARY_COMPRESSED) { - result = pcl::io::savePCDFileBinaryCompressed(request->path, cloud_); + result = pcl::io::savePCDFileBinaryCompressed(request->path, *cloud_); } else { response->result = response->FAIL; response->description = "invalid file format"; diff --git a/pcl_apps/src/matching/ndt_matching/ndt_matching_component.cpp b/pcl_apps/src/matching/ndt_matching/ndt_matching_component.cpp index dc6a1f0..68e2fea 100644 --- a/pcl_apps/src/matching/ndt_matching/ndt_matching_component.cpp +++ b/pcl_apps/src/matching/ndt_matching/ndt_matching_component.cpp @@ -51,7 +51,7 @@ NdtMatchingComponent::NdtMatchingComponent(const rclcpp::NodeOptions & options) get_parameter("omp_num_thread", omp_num_thread_); broadcaster_ = std::make_shared(this); - ndt_ = std::make_shared>(); + ndt_ = std::make_shared>(); ndt_->setNeighborhoodSearchMethod(pclomp::DIRECT7); if (0 < omp_num_thread_) ndt_->setNumThreads(omp_num_thread_); @@ -118,42 +118,33 @@ NdtMatchingComponent::NdtMatchingComponent(const rclcpp::NodeOptions & options) reference_cloud_recieved_ = false; initial_pose_recieved_ = false; - auto reference_cloud_callback = - [this](const typename sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + auto reference_cloud_callback = [this](const PCLPointCloudTypePtr reference_cloud_) -> void { ndt_map_mtx_.lock(); initial_pose_recieved_ = false; - assert(msg->header.frame_id == reference_frame_id_); reference_cloud_recieved_ = true; - pcl::fromROSMsg(*msg, *reference_cloud_); ndt_->setInputTarget(reference_cloud_); ndt_map_mtx_.unlock(); }; auto initial_pose_callback = [this](const typename geometry_msgs::msg::PoseStamped::SharedPtr msg) -> void { initial_pose_recieved_ = true; - assert(msg->header.frame_id == reference_frame_id_); current_relative_pose_ = *msg; }; - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { - std::lock_guard lock(ndt_map_mtx_); - pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *input_cloud); + auto callback = [this](const PCLPointCloudTypePtr input_cloud) -> void { std::vector nan_index; pcl::removeNaNFromPointCloud(*input_cloud, *input_cloud, nan_index); publishTF(reference_frame_id_, base_frame_id_, current_relative_pose_); - updateRelativePose(input_cloud, msg->header.stamp); + updateRelativePose(input_cloud, pcl_conversions::fromPCL(input_cloud->header.stamp)); current_relative_pose_pub_->publish(current_relative_pose_); }; - sub_reference_cloud_ = create_subscription( + sub_reference_cloud_ = create_subscription( reference_cloud_topic_, rclcpp::QoS{1}.transient_local(), reference_cloud_callback); - sub_input_cloud_ = - create_subscription(input_cloud_topic_, 10, callback); + sub_input_cloud_ = create_subscription(input_cloud_topic_, 10, callback); sub_initial_pose_ = create_subscription( initial_pose_topic_, 1, initial_pose_callback); } -void NdtMatchingComponent::updateRelativePose( - pcl::PointCloud::Ptr input_cloud, rclcpp::Time stamp) +void NdtMatchingComponent::updateRelativePose(PCLPointCloudTypePtr input_cloud, rclcpp::Time stamp) { ndt_->setTransformationEpsilon(transform_epsilon_); ndt_->setStepSize(step_size_); @@ -166,7 +157,7 @@ void NdtMatchingComponent::updateRelativePose( transform.translation.z = current_relative_pose_.pose.position.z; transform.rotation = current_relative_pose_.pose.orientation; Eigen::Matrix4f mat = tf2::transformToEigen(transform).matrix().cast(); - auto output_cloud = std::make_shared>(); + auto output_cloud = std::make_shared>(); ndt_->align(*output_cloud, mat); Eigen::Matrix4f final_transform = ndt_->getFinalTransformation(); tf2::Matrix3x3 rotation_mat; @@ -188,8 +179,10 @@ void NdtMatchingComponent::updateRelativePose( current_relative_pose_.pose.orientation.z = quat.z(); current_relative_pose_.pose.orientation.w = quat.w(); } + void NdtMatchingComponent::publishTF( - std::string frame_id, std::string child_frame_id, geometry_msgs::msg::PoseStamped pose) + const std::string & frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::TransformStamped transform_stamped; From 3e7639e2b2515feaa6689e2ecbcdb340d4781c62 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 22:38:05 +0900 Subject: [PATCH 18/20] use type adaptation in ndt_matching_twist_estimator_component Signed-off-by: hakuturu583 --- .../ndt_matching_twist_estimator_component.hpp | 7 ++++--- .../ndt_matching_twist_estimator_component.cpp | 14 +++++--------- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp b/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp index 64ccd4f..7c014e1 100644 --- a/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp +++ b/pcl_apps/include/pcl_apps/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.hpp @@ -66,6 +66,7 @@ extern "C" { #include #include +#include #include #include @@ -89,9 +90,9 @@ class NdtMatchingTwistEstimatorComponent : public rclcpp::Node private: std::string input_cloud_topic_; - rclcpp::Subscription::SharedPtr sub_input_cloud_; + PointCloudSubscriber sub_input_cloud_; rclcpp::Publisher::SharedPtr current_twist_pub_; - boost::circular_buffer::Ptr> buffer_; + boost::circular_buffer::Ptr> buffer_; boost::circular_buffer timestamps_; double transform_epsilon_; double step_size_; @@ -99,7 +100,7 @@ class NdtMatchingTwistEstimatorComponent : public rclcpp::Node int max_iterations_; std::string input_cloud_frame_id_; std::optional estimateCurrentTwist(); - pcl::NormalDistributionsTransform ndt_; + pcl::NormalDistributionsTransform ndt_; double toSec(rclcpp::Duration duration) { double ret; diff --git a/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp index b054cec..272ecf6 100644 --- a/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp +++ b/pcl_apps/src/matching/ndt_matching_twist_estimator/ndt_matching_twist_estimator_component.cpp @@ -98,21 +98,17 @@ NdtMatchingTwistEstimatorComponent::NdtMatchingTwistEstimatorComponent( std::string output_topic_name = get_name() + std::string("/current_relative_pose"); current_twist_pub_ = create_publisher(output_topic_name, 10); // Setup Subscriber - buffer_ = boost::circular_buffer::Ptr>(2); + buffer_ = boost::circular_buffer(2); timestamps_ = boost::circular_buffer(2); - auto callback = [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void { - assert(input_cloud_frame_id_ == msg->header.frame_id); - timestamps_.push_back(msg->header.stamp); - pcl::PointCloud::Ptr input_cloud; - pcl::fromROSMsg(*msg, *input_cloud); + auto callback = [this](const PCLPointCloudTypePtr input_cloud) -> void { + timestamps_.push_back(pcl_conversions::fromPCL(input_cloud->header.stamp)); buffer_.push_back(input_cloud); std::optional twist = estimateCurrentTwist(); if (twist) { current_twist_pub_->publish(twist.value()); } }; - sub_input_cloud_ = - create_subscription(input_cloud_topic_, 10, callback); + sub_input_cloud_ = create_subscription(input_cloud_topic_, 10, callback); } std::optional @@ -135,7 +131,7 @@ NdtMatchingTwistEstimatorComponent::estimateCurrentTwist() transform.rotation.z = 0.0; transform.rotation.w = 1.0; Eigen::Matrix4f mat = tf2::transformToEigen(transform).matrix().cast(); - pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + PCLPointCloudTypePtr output_cloud(new pcl::PointCloud); ndt_.align(*output_cloud, mat); Eigen::Matrix4f final_transform = ndt_.getFinalTransformation(); tf2::Matrix3x3 rotation_mat; From 01ebb7c44d0d6e26a1e28c153780ba5bcb0f8d00 Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 22:42:53 +0900 Subject: [PATCH 19/20] use type adaptation in publish concatinated pointcloud Signed-off-by: hakuturu583 --- .../points_concatenate_component.hpp | 5 ++--- .../points_concatenate_component.cpp | 20 +++++++------------ 2 files changed, 9 insertions(+), 16 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 835c8e5..93a7c10 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 @@ -54,9 +54,8 @@ extern "C" { #endif // Headers in ROS2 -#include - #include +#include #include #include @@ -103,7 +102,7 @@ class PointsConcatenateComponent : public rclcpp::Node std::array, 8> sub_ptrs_; message_filters::PassThrough nf_; */ - rclcpp::Publisher::SharedPtr pub_; + PointCloudPublisher pub_; Sync2PtrT sync2_; Sync3PtrT sync3_; Sync4PtrT sync4_; 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 72625b8..6d10916 100644 --- a/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp +++ b/pcl_apps/src/filter/points_concatenate/points_concatenate_component.cpp @@ -29,7 +29,7 @@ PointsConcatenateComponent::PointsConcatenateComponent(const rclcpp::NodeOptions get_parameter("num_input", num_input_); assert(num_input_ >= 2 && num_input_ <= 4); std::string output_topic_name = get_name() + std::string("/output"); - pub_ = create_publisher(output_topic_name, 10); + pub_ = create_publisher(output_topic_name, 10); for (int i = 0; i < num_input_; i++) { declare_parameter( "input_topic" + std::to_string(i), get_name() + std::string("/input") + std::to_string(i)); @@ -80,10 +80,8 @@ void PointsConcatenateComponent::callback2(CallbackT in0, CallbackT in1) *cloud = *pc_cloud + *cloud; } if (!empty) { - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl::toROSMsg(*cloud, output_cloud_msg); - output_cloud_msg.header.stamp = sync2_->getPollTimestamp(); - pub_->publish(output_cloud_msg); + cloud->header.stamp = pcl_conversions::toPCL(sync2_->getPollTimestamp()); + pub_->publish(cloud); } } @@ -113,10 +111,8 @@ void PointsConcatenateComponent::callback3(CallbackT in0, CallbackT in1, Callbac *cloud = *pc_cloud + *cloud; } if (!empty) { - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl::toROSMsg(*cloud, output_cloud_msg); - output_cloud_msg.header.stamp = sync3_->getPollTimestamp(); - pub_->publish(output_cloud_msg); + cloud->header.stamp = pcl_conversions::toPCL(sync3_->getPollTimestamp()); + pub_->publish(cloud); } } @@ -154,10 +150,8 @@ void PointsConcatenateComponent::callback4( *cloud = *pc_cloud + *cloud; } if (!empty) { - sensor_msgs::msg::PointCloud2 output_cloud_msg; - pcl::toROSMsg(*cloud, output_cloud_msg); - output_cloud_msg.header.stamp = sync4_->getPollTimestamp(); - pub_->publish(output_cloud_msg); + cloud->header.stamp = pcl_conversions::toPCL(sync4_->getPollTimestamp()); + pub_->publish(cloud); } } } // namespace pcl_apps From 901baacfc34f9ed64ea9678f5ac302f49a667d0f Mon Sep 17 00:00:00 2001 From: hakuturu583 Date: Sat, 20 Apr 2024 22:51:49 +0900 Subject: [PATCH 20/20] fix cmake Signed-off-by: hakuturu583 --- pcl_apps/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcl_apps/CMakeLists.txt b/pcl_apps/CMakeLists.txt index 62c7aba..668d5c6 100644 --- a/pcl_apps/CMakeLists.txt +++ b/pcl_apps/CMakeLists.txt @@ -50,7 +50,7 @@ function(add_pcl_apps_component module component_name component_name_in_macro) ) 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} glog) + boost_system ${PCL_LIBRARIES}) ament_auto_add_executable(${component_name}_node src/${module}/${component_name}/${component_name}_node.cpp)