From d7a79b927f8e1c5e08e8f76a94c5dd058e86f177 Mon Sep 17 00:00:00 2001 From: Sean Kelly Date: Wed, 12 Aug 2020 15:12:51 -0400 Subject: [PATCH] Port transforms library to ROS2 (#301) * Port transforms library to ROS2 - Port the transforms library to ROS2 - Update CMakeLists - Update package.xml - Enable the package Signed-off-by: Sean Kelly * Feedback from PR --- pcl_ros/CMakeLists.txt | 381 +++++++++----------- pcl_ros/COLCON_IGNORE | 0 pcl_ros/include/pcl_ros/impl/transforms.hpp | 132 ++++--- pcl_ros/include/pcl_ros/transforms.hpp | 101 ++++-- pcl_ros/package.xml | 43 ++- pcl_ros/src/transforms.cpp | 250 ++++++++----- perception_pcl/package.xml | 2 +- 7 files changed, 517 insertions(+), 392 deletions(-) delete mode 100644 pcl_ros/COLCON_IGNORE diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 7d845687..7311251c 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -1,245 +1,198 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) project(pcl_ros) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + ## Find system dependencies -find_package(cmake_modules REQUIRED) -find_package(Boost REQUIRED COMPONENTS system filesystem thread) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS core features filters io segmentation surface) +find_package(PCL REQUIRED QUIET COMPONENTS core features filters io segmentation surface) -if(NOT "${PCL_LIBRARIES}" STREQUAL "") - # For debian: https://github.com/ros-perception/perception_pcl/issues/139 - list(REMOVE_ITEM PCL_LIBRARIES - "vtkGUISupportQt" - "vtkGUISupportQtOpenGL" - "vtkGUISupportQtSQL" - "vtkGUISupportQtWebkit" - "vtkViewsQt" - "vtkRenderingQt") -endif() +## Find ROS package dependencies +find_package(ament_cmake_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) -# There is a bug in the Ubuntu Artful (17.10) version of the VTK package, -# where it includes /usr/include/*-linux-gnu/freetype2 in the include -# directories (which doesn't exist). This filters down to the PCL_INCLUDE_DIRS, -# and causes downstream projects trying to use these libraries to fail to -# configure properly. Here we remove those bogus entries so that downstream -# consumers of this package succeed. -if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") - foreach(item ${PCL_INCLUDE_DIRS}) - string(REGEX MATCH "/usr/include/.*-linux-gnu/freetype2" item ${item}) - if(item) - list(REMOVE_ITEM PCL_INCLUDE_DIRS ${item}) - endif() - endforeach() -endif() - -## Find catkin packages -find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - message_filters - nodelet - nodelet_topic_tools +set(dependencies pcl_conversions - pcl_msgs - pluginlib - rosbag - rosconsole - roscpp - roslib + rclcpp sensor_msgs - std_msgs - tf - tf2_eigen + geometry_msgs + tf2 + tf2_geometry_msgs + tf2_ros + EIGEN3 + PCL ) -## Add include directories -include_directories(include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} +include_directories( + include ${PCL_INCLUDE_DIRS} ) -## Generate dynamic_reconfigure -generate_dynamic_reconfigure_options( - cfg/EuclideanClusterExtraction.cfg - cfg/ExtractIndices.cfg - cfg/ExtractPolygonalPrismData.cfg - cfg/Feature.cfg - cfg/Filter.cfg - cfg/MLS.cfg - cfg/SACSegmentation.cfg - cfg/SACSegmentationFromNormals.cfg - cfg/SegmentDifferences.cfg - cfg/StatisticalOutlierRemoval.cfg - cfg/RadiusOutlierRemoval.cfg - cfg/VoxelGrid.cfg - cfg/CropBox.cfg -) - -## Declare the catkin package -catkin_package( - INCLUDE_DIRS include - LIBRARIES - pcl_ros_filters - pcl_ros_io - pcl_ros_tf - CATKIN_DEPENDS - dynamic_reconfigure - message_filters - nodelet - nodelet_topic_tools - pcl_conversions - pcl_msgs - rosbag - roscpp - sensor_msgs - std_msgs - tf - DEPENDS - Boost - EIGEN3 - PCL -) - ## Declare the pcl_ros_tf library add_library(pcl_ros_tf src/transforms.cpp) -target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_tf ${catkin_EXPORTED_TARGETS}) - -## Nodelets - -## Declare the pcl_ros_io library -add_library(pcl_ros_io - src/pcl_ros/io/bag_io.cpp - src/pcl_ros/io/concatenate_data.cpp - src/pcl_ros/io/concatenate_fields.cpp - src/pcl_ros/io/io.cpp - src/pcl_ros/io/pcd_io.cpp +ament_target_dependencies(pcl_ros_tf + ${dependencies} ) -target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -class_loader_hide_library_symbols(pcl_ros_io) +target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES}) -## Declare the pcl_ros_features library -add_library(pcl_ros_features - src/pcl_ros/features/feature.cpp - # Compilation is much faster if we include all the following CPP files in feature.cpp - src/pcl_ros/features/boundary.cpp - src/pcl_ros/features/fpfh.cpp - src/pcl_ros/features/fpfh_omp.cpp - src/pcl_ros/features/shot.cpp - src/pcl_ros/features/shot_omp.cpp - src/pcl_ros/features/moment_invariants.cpp - src/pcl_ros/features/normal_3d.cpp - src/pcl_ros/features/normal_3d_omp.cpp - src/pcl_ros/features/pfh.cpp - src/pcl_ros/features/principal_curvatures.cpp - src/pcl_ros/features/vfh.cpp -) -target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_features) - - -## Declare the pcl_ros_filters library -add_library(pcl_ros_filters - src/pcl_ros/filters/extract_indices.cpp - src/pcl_ros/filters/filter.cpp - src/pcl_ros/filters/passthrough.cpp - src/pcl_ros/filters/project_inliers.cpp - src/pcl_ros/filters/radius_outlier_removal.cpp - src/pcl_ros/filters/statistical_outlier_removal.cpp - src/pcl_ros/filters/voxel_grid.cpp - src/pcl_ros/filters/crop_box.cpp -) -target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_filters) - -## Declare the pcl_ros_segmentation library -add_library (pcl_ros_segmentation - src/pcl_ros/segmentation/extract_clusters.cpp - src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp - src/pcl_ros/segmentation/sac_segmentation.cpp - src/pcl_ros/segmentation/segment_differences.cpp - src/pcl_ros/segmentation/segmentation.cpp -) -target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_segmentation) - -## Declare the pcl_ros_surface library -add_library (pcl_ros_surface - src/pcl_ros/surface/surface.cpp - # Compilation is much faster if we include all the following CPP files in surface.cpp - src/pcl_ros/surface/convex_hull.cpp - src/pcl_ros/surface/moving_least_squares.cpp -) -target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) -class_loader_hide_library_symbols(pcl_ros_surface) - -## Tools - -add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) -target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) -target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(bag_to_pcd tools/bag_to_pcd.cpp) -target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) -target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -## Downloads - -catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd - DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data - MD5 546b5b4822fb1de21b0cf83d41ad6683 -) -add_custom_target(download ALL DEPENDS download_extra_data) +### Nodelets +# +### Declare the pcl_ros_io library +#add_library(pcl_ros_io +# src/pcl_ros/io/bag_io.cpp +# src/pcl_ros/io/concatenate_data.cpp +# src/pcl_ros/io/concatenate_fields.cpp +# src/pcl_ros/io/io.cpp +# src/pcl_ros/io/pcd_io.cpp +#) +#target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#class_loader_hide_library_symbols(pcl_ros_io) +# +### Declare the pcl_ros_features library +#add_library(pcl_ros_features +# src/pcl_ros/features/feature.cpp +# # Compilation is much faster if we include all the following CPP files in feature.cpp +# src/pcl_ros/features/boundary.cpp +# src/pcl_ros/features/fpfh.cpp +# src/pcl_ros/features/fpfh_omp.cpp +# src/pcl_ros/features/shot.cpp +# src/pcl_ros/features/shot_omp.cpp +# src/pcl_ros/features/moment_invariants.cpp +# src/pcl_ros/features/normal_3d.cpp +# src/pcl_ros/features/normal_3d_omp.cpp +# src/pcl_ros/features/pfh.cpp +# src/pcl_ros/features/principal_curvatures.cpp +# src/pcl_ros/features/vfh.cpp +#) +#target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_features) +# +# +### Declare the pcl_ros_filters library +#add_library(pcl_ros_filters +# src/pcl_ros/filters/extract_indices.cpp +# src/pcl_ros/filters/filter.cpp +# src/pcl_ros/filters/passthrough.cpp +# src/pcl_ros/filters/project_inliers.cpp +# src/pcl_ros/filters/radius_outlier_removal.cpp +# src/pcl_ros/filters/statistical_outlier_removal.cpp +# src/pcl_ros/filters/voxel_grid.cpp +# src/pcl_ros/filters/crop_box.cpp +#) +#target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_filters) +# +### Declare the pcl_ros_segmentation library +#add_library (pcl_ros_segmentation +# src/pcl_ros/segmentation/extract_clusters.cpp +# src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +# src/pcl_ros/segmentation/sac_segmentation.cpp +# src/pcl_ros/segmentation/segment_differences.cpp +# src/pcl_ros/segmentation/segmentation.cpp +#) +#target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_segmentation) +# +### Declare the pcl_ros_surface library +#add_library (pcl_ros_surface +# src/pcl_ros/surface/surface.cpp +# # Compilation is much faster if we include all the following CPP files in surface.cpp +# src/pcl_ros/surface/convex_hull.cpp +# src/pcl_ros/surface/moving_least_squares.cpp +#) +#target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +#add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) +#class_loader_hide_library_symbols(pcl_ros_surface) +# +### Tools +# +#add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) +#target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) +#target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(bag_to_pcd tools/bag_to_pcd.cpp) +#target_link_libraries(bag_to_pcd pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) +#target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +#add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) +#target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +# +### Downloads +# +#catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd +# DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data +# MD5 546b5b4822fb1de21b0cf83d41ad6683 +#) +#add_custom_target(download ALL DEPENDS download_extra_data) ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp) - target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) - add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) -endif(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp) + #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/filters/sample_statistical_outlier_removal.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) + #add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) +endif() -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install( + DIRECTORY include/ + DESTINATION include +) install( TARGETS pcl_ros_tf - pcl_ros_io - pcl_ros_features - pcl_ros_filters - pcl_ros_surface - pcl_ros_segmentation - pcd_to_pointcloud - pointcloud_to_pcd - bag_to_pcd - convert_pcd_to_image - convert_pointcloud_to_image - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# pcl_ros_io +# pcl_ros_features +# pcl_ros_filters +# pcl_ros_surface +# pcl_ros_segmentation +# pcd_to_pointcloud +# pointcloud_to_pcd +# bag_to_pcd +# convert_pcd_to_image +# convert_pointcloud_to_image + EXPORT export_pcl_ros + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include ) install(DIRECTORY plugins samples - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_libraries(pcl_ros_tf) +ament_export_dependencies(${dependencies}) +ament_export_targets(export_pcl_ros HAS_LIBRARY_TARGET) +ament_package() diff --git a/pcl_ros/COLCON_IGNORE b/pcl_ros/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/pcl_ros/include/pcl_ros/impl/transforms.hpp b/pcl_ros/include/pcl_ros/impl/transforms.hpp index 2f2a7580..81fb9169 100644 --- a/pcl_ros/include/pcl_ros/impl/transforms.hpp +++ b/pcl_ros/include/pcl_ros/impl/transforms.hpp @@ -37,9 +37,22 @@ #ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_ #define PCL_ROS__IMPL__TRANSFORMS_HPP_ -#include -#include #include "pcl_ros/transforms.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using pcl_conversions::fromPCL; using pcl_conversions::toPCL; @@ -51,16 +64,16 @@ template void transformPointCloudWithNormals( const pcl::PointCloud & cloud_in, - pcl::PointCloud & cloud_out, const tf::Transform & transform) + pcl::PointCloud & cloud_out, const tf2::Transform & transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. - tf::Quaternion q = transform.getRotation(); + tf2::Quaternion q = transform.getRotation(); Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) - tf::Vector3 v = transform.getOrigin(); + tf2::Vector3 v = transform.getOrigin(); Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform @@ -69,21 +82,33 @@ transformPointCloudWithNormals( transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation); } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, const geometry_msgs::msg::TransformStamped & transform) +{ + tf2::Transform tf; + tf2::convert(transform.transform, tf); + transformPointCloudWithNormals(cloud_in, cloud_out, tf); +} + ////////////////////////////////////////////////////////////////////////////////////////////// template void transformPointCloud( const pcl::PointCloud & cloud_in, - pcl::PointCloud & cloud_out, const tf::Transform & transform) + pcl::PointCloud & cloud_out, const tf2::Transform & transform) { // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to // the conversion of the point cloud anyway. Idem for the origin. - tf::Quaternion q = transform.getRotation(); + tf2::Quaternion q = transform.getRotation(); Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) - tf::Vector3 v = transform.getOrigin(); + tf2::Vector3 v = transform.getOrigin(); Eigen::Vector3f origin(v.x(), v.y(), v.z()); // Eigen::Translation3f translation(v); // Assemble an Eigen Transform @@ -92,6 +117,18 @@ transformPointCloud( transformPointCloud(cloud_in, cloud_out, origin, rotation); } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, const geometry_msgs::msg::TransformStamped & transform) +{ + tf2::Transform tf; + tf2::convert(transform.transform, tf); + transformPointCloud(cloud_in, cloud_out, tf); +} + ////////////////////////////////////////////////////////////////////////////////////////////// template bool @@ -99,23 +136,24 @@ transformPointCloudWithNormals( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; return true; } - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( - target_frame, cloud_in.header.frame_id, fromPCL( - cloud_in.header).stamp, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + transform = + tf_buffer.lookupTransform( + target_frame, cloud_in.header.frame_id, + fromPCL(cloud_in.header.stamp)); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } @@ -131,25 +169,27 @@ transformPointCloud( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { if (cloud_in.header.frame_id == target_frame) { cloud_out = cloud_in; return true; } - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( - target_frame, cloud_in.header.frame_id, fromPCL( - cloud_in.header).stamp, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + transform = + tf_buffer.lookupTransform( + target_frame, cloud_in.header.frame_id, + fromPCL(cloud_in.header.stamp)); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } + transformPointCloud(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; return true; @@ -160,28 +200,28 @@ template bool transformPointCloud( const std::string & target_frame, - const ros::Time & target_time, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( + transform = tf_buffer.lookupTransform( target_frame, target_time, cloud_in.header.frame_id, - fromPCL(cloud_in.header).stamp, fixed_frame, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + fromPCL(cloud_in.header.stamp), fixed_frame); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } transformPointCloud(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - std_msgs::Header header; + std_msgs::msg::Header header; header.stamp = target_time; cloud_out.header = toPCL(header); return true; @@ -192,28 +232,28 @@ template bool transformPointCloudWithNormals( const std::string & target_frame, - const ros::Time & target_time, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener) + const tf2_ros::Buffer & tf_buffer) { - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform( + transform = tf_buffer.lookupTransform( target_frame, target_time, cloud_in.header.frame_id, - fromPCL(cloud_in.header).stamp, fixed_frame, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + fromPCL(cloud_in.header.stamp), fixed_frame); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } transformPointCloudWithNormals(cloud_in, cloud_out, transform); cloud_out.header.frame_id = target_frame; - std_msgs::Header header; + std_msgs::msg::Header header; header.stamp = target_time; cloud_out.header = toPCL(header); return true; diff --git a/pcl_ros/include/pcl_ros/transforms.hpp b/pcl_ros/include/pcl_ros/transforms.hpp index bd890749..1b5312f3 100644 --- a/pcl_ros/include/pcl_ros/transforms.hpp +++ b/pcl_ros/include/pcl_ros/transforms.hpp @@ -37,10 +37,14 @@ #ifndef PCL_ROS__TRANSFORMS_HPP_ #define PCL_ROS__TRANSFORMS_HPP_ -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include namespace pcl_ros @@ -56,13 +60,26 @@ void transformPointCloudWithNormals( const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::Transform & transform); + const tf2::Transform & transform); + +/** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ +template +void +transformPointCloudWithNormals( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const geometry_msgs::msg::TransformStamped & transform); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool @@ -70,7 +87,7 @@ transformPointCloudWithNormals( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to @@ -78,17 +95,17 @@ transformPointCloudWithNormals( * \param cloud_in the input point cloud * \param fixed_frame fixed TF frame * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool transformPointCloudWithNormals( const std::string & target_frame, - const ros::Time & target_time, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Apply a rigid transform defined by a 3D offset and a quaternion * \param cloud_in the input point cloud @@ -101,13 +118,26 @@ void transformPointCloud( const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::Transform & transform); + const tf2::Transform & transform); + +/** \brief Apply a rigid transform defined by a 3D offset and a quaternion + * \param cloud_in the input point cloud + * \param cloud_out the input point cloud + * \param transform a rigid transformation from tf + * \note calls the Eigen version + */ +template +void +transformPointCloud( + const pcl::PointCloud & cloud_in, + pcl::PointCloud & cloud_out, + const geometry_msgs::msg::TransformStamped & transform); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to * \param cloud_in the input point cloud * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool @@ -115,7 +145,7 @@ transformPointCloud( const std::string & target_frame, const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Transforms a point cloud in a given target TF frame using a TransformListener * \param target_frame the target TF frame the point cloud should be transformed to @@ -123,29 +153,30 @@ transformPointCloud( * \param cloud_in the input point cloud * \param fixed_frame fixed TF frame * \param cloud_out the input point cloud - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ template bool transformPointCloud( - const std::string & target_frame, const ros::Time & target_time, + const std::string & target_frame, + const rclcpp::Time & target_time, const pcl::PointCloud & cloud_in, const std::string & fixed_frame, pcl::PointCloud & cloud_out, - const tf::TransformListener & tf_listener); + const tf2_ros::Buffer & tf_buffer); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame * \param in the input PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset - * \param tf_listener a TF listener object + * \param tf_buffer a TF buffer object */ bool transformPointCloud( const std::string & target_frame, - const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out, - const tf::TransformListener & tf_listener); + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out, + const tf2_ros::Buffer & tf_buffer); /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. * \param target_frame the target TF frame @@ -156,9 +187,22 @@ transformPointCloud( void transformPointCloud( const std::string & target_frame, - const tf::Transform & net_transform, - const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out); + const tf2::Transform & net_transform, + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out); + +/** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + * \param target_frame the target TF frame + * \param net_transform the TF transformer object + * \param in the input PointCloud2 dataset + * \param out the resultant transformed PointCloud2 dataset + */ +void +transformPointCloud( + const std::string & target_frame, + const geometry_msgs::msg::TransformStamped & net_transform, + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out); /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix. * \param transform the transformation to use on the points @@ -168,15 +212,22 @@ transformPointCloud( void transformPointCloud( const Eigen::Matrix4f & transform, - const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out); + const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out); /** \brief Obtain the transformation matrix from TF into an Eigen form * \param bt the TF transformation * \param out_mat the Eigen transformation */ void -transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat); +transformAsMatrix(const tf2::Transform & bt, Eigen::Matrix4f & out_mat); + +/** \brief Obtain the transformation matrix from TF into an Eigen form + * \param bt the TF transformation + * \param out_mat the Eigen transformation + */ +void +transformAsMatrix(const geometry_msgs::msg::TransformStamped & bt, Eigen::Matrix4f & out_mat); } // namespace pcl_ros #endif // PCL_ROS__TRANSFORMS_HPP_ diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index 88d40c40..cf23b4f7 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -1,5 +1,6 @@ - + + pcl_ros 2.2.0 @@ -10,10 +11,6 @@ - Open Perception - Julius Kammerl - William Woodall - Paul Bovbel Steve Macenski Kentaro Wada @@ -24,35 +21,37 @@ https://github.com/ros-perception/perception_pcl/issues https://github.com/ros-perception/perception_pcl - catkin - cmake_modules - rosconsole - roslib + Open Perception + Julius Kammerl + William Woodall - dynamic_reconfigure + ament_cmake eigen - message_filters - nodelet - nodelet_topic_tools libpcl-all-dev pcl_conversions - pcl_msgs - pluginlib - rosbag - roscpp + rclcpp sensor_msgs - std_msgs - tf - tf2_eigen + geometry_msgs + tf2 + tf2_geometry_msgs + tf2_ros - rostest + ament_lint_auto + ament_lint_common + ament_cmake_gtest - + + + + ament_cmake diff --git a/pcl_ros/src/transforms.cpp b/pcl_ros/src/transforms.cpp index fcea320b..ffb74a00 100644 --- a/pcl_ros/src/transforms.cpp +++ b/pcl_ros/src/transforms.cpp @@ -34,22 +34,34 @@ * */ -#include -#include -#include -#include -#include -#include #include "pcl_ros/transforms.hpp" #include "pcl_ros/impl/transforms.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace pcl_ros { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool transformPointCloud( - const std::string & target_frame, const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener) + const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out, const tf2_ros::Buffer & tf_buffer) { if (in.header.frame_id == target_frame) { out = in; @@ -57,14 +69,17 @@ transformPointCloud( } // Get the TF transform - tf::StampedTransform transform; + geometry_msgs::msg::TransformStamped transform; try { - tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform); - } catch (tf::LookupException & e) { - ROS_ERROR("%s", e.what()); + transform = + tf_buffer.lookupTransform( + target_frame, in.header.frame_id, tf2_ros::fromMsg( + in.header.stamp)); + } catch (tf2::LookupException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; - } catch (tf::ExtrapolationException & e) { - ROS_ERROR("%s", e.what()); + } catch (tf2::ExtrapolationException & e) { + RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what()); return false; } @@ -81,8 +96,8 @@ transformPointCloud( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud( - const std::string & target_frame, const tf::Transform & net_transform, - const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out) + const std::string & target_frame, const tf2::Transform & net_transform, + const sensor_msgs::msg::PointCloud2 & in, sensor_msgs::msg::PointCloud2 & out) { if (in.header.frame_id == target_frame) { out = in; @@ -98,11 +113,21 @@ transformPointCloud( out.header.frame_id = target_frame; } +void +transformPointCloud( + const std::string & target_frame, const geometry_msgs::msg::TransformStamped & net_transform, + const sensor_msgs::msg::PointCloud2 & in, sensor_msgs::msg::PointCloud2 & out) +{ + tf2::Transform transform; + tf2::convert(net_transform.transform, transform); + transformPointCloud(target_frame, transform, in, out); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void transformPointCloud( - const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in, - sensor_msgs::PointCloud2 & out) + const Eigen::Matrix4f & transform, const sensor_msgs::msg::PointCloud2 & in, + sensor_msgs::msg::PointCloud2 & out) { // Get X-Y-Z indices int x_idx = pcl::getFieldIndex(in, "x"); @@ -110,15 +135,19 @@ transformPointCloud( int z_idx = pcl::getFieldIndex(in, "z"); if (x_idx == -1 || y_idx == -1 || z_idx == -1) { - ROS_ERROR("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); + RCLCPP_ERROR( + rclcpp::get_logger("pcl_ros"), + "Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format."); return; } - if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || - in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || - in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32) + if (in.fields[x_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[y_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 || + in.fields[z_idx].datatype != sensor_msgs::msg::PointField::FLOAT32) { - ROS_ERROR("X-Y-Z coordinates not floats. Currently only floats are supported."); + RCLCPP_ERROR( + rclcpp::get_logger("pcl_ros"), + "X-Y-Z coordinates not floats. Currently only floats are supported."); return; } @@ -144,15 +173,15 @@ transformPointCloud( in.fields[z_idx].offset, 0); for (size_t i = 0; i < in.width * in.height; ++i) { - Eigen::Vector4f pt(*reinterpret_cast(&in.data[xyz_offset[0]]), - *reinterpret_cast(&in.data[xyz_offset[1]]), - *reinterpret_cast(&in.data[xyz_offset[2]], 1)); + Eigen::Vector4f pt(*reinterpret_cast(&in.data[xyz_offset[0]]), + *reinterpret_cast(&in.data[xyz_offset[1]]), + *reinterpret_cast(&in.data[xyz_offset[2]]), 1); Eigen::Vector4f pt_out; bool max_range_point = false; int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset; - float * distance_ptr = - (dist_idx < 0 ? NULL : reinterpret_cast(&in.data[distance_ptr_offset])); + const float * distance_ptr = (dist_idx < 0 ? + NULL : reinterpret_cast(&in.data[distance_ptr_offset])); if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point pt_out = pt; @@ -160,8 +189,6 @@ transformPointCloud( pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt_out = transform * pt; max_range_point = true; - // std::cout << pt[0]<<","< "<( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloudWithNormals( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloudWithNormals( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloudWithNormals( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloudWithNormals( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloudWithNormals( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); template void pcl_ros::transformPointCloud( const pcl::PointCloud &, pcl::PointCloud &, - const tf::Transform &); + const tf2::Transform &); + +////////////////////////////////////////////////////////////////////////////////////////////// +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, + pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); +template void pcl_ros::transformPointCloud( + const pcl::PointCloud &, pcl::PointCloud &, + const geometry_msgs::msg::TransformStamped &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, const pcl::PointCloud &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, const rclcpp::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, pcl::PointCloud &, - const tf::TransformListener &); + const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( - const std::string &, const ros::Time &, + const std::string &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); template bool pcl_ros::transformPointCloud( const std::string &, - const ros::Time &, + const rclcpp::Time &, const pcl::PointCloud &, const std::string &, - pcl::PointCloud &, const tf::TransformListener &); + pcl::PointCloud &, const tf2_ros::Buffer &); diff --git a/perception_pcl/package.xml b/perception_pcl/package.xml index 2a68f39c..acfc5854 100644 --- a/perception_pcl/package.xml +++ b/perception_pcl/package.xml @@ -27,7 +27,7 @@ pcl_conversions pcl_msgs - + pcl_ros ament_cmake