apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Port transforms library to ROS2 (#301)

Browse Source 
* Port transforms library to ROS2

- Port the transforms library to ROS2
- Update CMakeLists
- Update package.xml
- Enable the package

Signed-off-by: Sean Kelly <sean@seankelly.dev>

* Feedback from PR
This commit is contained in:
Sean Kelly 2020-08-12 15:12:51 -04:00 committed by GitHub
parent 420f5b032b
commit d7a79b927f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 517 additions and 392 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

383
pcl_ros/CMakeLists.txt
View File

@ -1,245 +1,198 @@
cmake_minimum_required(VERSION 2.8) cmake_minimum_required(VERSION 3.5)
project(pcl_ros) 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 system dependencies
find_package(cmake_modules REQUIRED)
find_package(Boost REQUIRED COMPONENTS system filesystem thread)
find_package(Eigen3 REQUIRED) 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 "") ## Find ROS package dependencies
# For debian: https://github.com/ros-perception/perception_pcl/issues/139 find_package(ament_cmake_ros REQUIRED)
list(REMOVE_ITEM PCL_LIBRARIES find_package(pcl_conversions REQUIRED)
"vtkGUISupportQt" find_package(rclcpp REQUIRED)
"vtkGUISupportQtOpenGL" find_package(sensor_msgs REQUIRED)
"vtkGUISupportQtSQL" find_package(geometry_msgs REQUIRED)
"vtkGUISupportQtWebkit" find_package(tf2 REQUIRED)
"vtkViewsQt" find_package(tf2_geometry_msgs REQUIRED)
"vtkRenderingQt") find_package(tf2_ros REQUIRED)
endif()
# There is a bug in the Ubuntu Artful (17.10) version of the VTK package, set(dependencies
# 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
pcl_conversions pcl_conversions
pcl_msgs rclcpp
pluginlib
rosbag
rosconsole
roscpp
roslib
sensor_msgs sensor_msgs
std_msgs geometry_msgs
tf tf2
tf2_eigen tf2_geometry_msgs
) tf2_ros
## Add include directories
include_directories(include
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${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 EIGEN3
PCL PCL
) )
include_directories(
include
${PCL_INCLUDE_DIRS}
)
## Declare the pcl_ros_tf library ## Declare the pcl_ros_tf library
add_library(pcl_ros_tf src/transforms.cpp) add_library(pcl_ros_tf src/transforms.cpp)
target_link_libraries(pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) ament_target_dependencies(pcl_ros_tf
add_dependencies(pcl_ros_tf ${catkin_EXPORTED_TARGETS}) ${dependencies}
## 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}) target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES})
class_loader_hide_library_symbols(pcl_ros_io)
## Declare the pcl_ros_features library ### Nodelets
add_library(pcl_ros_features #
src/pcl_ros/features/feature.cpp ### Declare the pcl_ros_io library
# Compilation is much faster if we include all the following CPP files in feature.cpp #add_library(pcl_ros_io
src/pcl_ros/features/boundary.cpp # src/pcl_ros/io/bag_io.cpp
src/pcl_ros/features/fpfh.cpp # src/pcl_ros/io/concatenate_data.cpp
src/pcl_ros/features/fpfh_omp.cpp # src/pcl_ros/io/concatenate_fields.cpp
src/pcl_ros/features/shot.cpp # src/pcl_ros/io/io.cpp
src/pcl_ros/features/shot_omp.cpp # src/pcl_ros/io/pcd_io.cpp
src/pcl_ros/features/moment_invariants.cpp #)
src/pcl_ros/features/normal_3d.cpp #target_link_libraries(pcl_ros_io pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
src/pcl_ros/features/normal_3d_omp.cpp #class_loader_hide_library_symbols(pcl_ros_io)
src/pcl_ros/features/pfh.cpp #
src/pcl_ros/features/principal_curvatures.cpp ### Declare the pcl_ros_features library
src/pcl_ros/features/vfh.cpp #add_library(pcl_ros_features
) # src/pcl_ros/features/feature.cpp
target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) # # Compilation is much faster if we include all the following CPP files in feature.cpp
add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg) # src/pcl_ros/features/boundary.cpp
class_loader_hide_library_symbols(pcl_ros_features) # src/pcl_ros/features/fpfh.cpp
# src/pcl_ros/features/fpfh_omp.cpp
# src/pcl_ros/features/shot.cpp
## Declare the pcl_ros_filters library # src/pcl_ros/features/shot_omp.cpp
add_library(pcl_ros_filters # src/pcl_ros/features/moment_invariants.cpp
src/pcl_ros/filters/extract_indices.cpp # src/pcl_ros/features/normal_3d.cpp
src/pcl_ros/filters/filter.cpp # src/pcl_ros/features/normal_3d_omp.cpp
src/pcl_ros/filters/passthrough.cpp # src/pcl_ros/features/pfh.cpp
src/pcl_ros/filters/project_inliers.cpp # src/pcl_ros/features/principal_curvatures.cpp
src/pcl_ros/filters/radius_outlier_removal.cpp # src/pcl_ros/features/vfh.cpp
src/pcl_ros/filters/statistical_outlier_removal.cpp #)
src/pcl_ros/filters/voxel_grid.cpp #target_link_libraries (pcl_ros_features ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
src/pcl_ros/filters/crop_box.cpp #add_dependencies(pcl_ros_features ${PROJECT_NAME}_gencfg)
) #class_loader_hide_library_symbols(pcl_ros_features)
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_filters library
#add_library(pcl_ros_filters
## Declare the pcl_ros_segmentation library # src/pcl_ros/filters/extract_indices.cpp
add_library (pcl_ros_segmentation # src/pcl_ros/filters/filter.cpp
src/pcl_ros/segmentation/extract_clusters.cpp # src/pcl_ros/filters/passthrough.cpp
src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp # src/pcl_ros/filters/project_inliers.cpp
src/pcl_ros/segmentation/sac_segmentation.cpp # src/pcl_ros/filters/radius_outlier_removal.cpp
src/pcl_ros/segmentation/segment_differences.cpp # src/pcl_ros/filters/statistical_outlier_removal.cpp
src/pcl_ros/segmentation/segmentation.cpp # src/pcl_ros/filters/voxel_grid.cpp
) # src/pcl_ros/filters/crop_box.cpp
target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) #)
add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) #target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
class_loader_hide_library_symbols(pcl_ros_segmentation) #add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg)
#class_loader_hide_library_symbols(pcl_ros_filters)
## Declare the pcl_ros_surface library #
add_library (pcl_ros_surface ### Declare the pcl_ros_segmentation library
src/pcl_ros/surface/surface.cpp #add_library (pcl_ros_segmentation
# Compilation is much faster if we include all the following CPP files in surface.cpp # src/pcl_ros/segmentation/extract_clusters.cpp
src/pcl_ros/surface/convex_hull.cpp # src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
src/pcl_ros/surface/moving_least_squares.cpp # src/pcl_ros/segmentation/sac_segmentation.cpp
) # src/pcl_ros/segmentation/segment_differences.cpp
target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) # src/pcl_ros/segmentation/segmentation.cpp
add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg) #)
class_loader_hide_library_symbols(pcl_ros_surface) #target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg)
## Tools #class_loader_hide_library_symbols(pcl_ros_segmentation)
#
add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp) ### Declare the pcl_ros_surface library
target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) #add_library (pcl_ros_surface
# src/pcl_ros/surface/surface.cpp
add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp) # # Compilation is much faster if we include all the following CPP files in surface.cpp
target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) # src/pcl_ros/surface/convex_hull.cpp
# src/pcl_ros/surface/moving_least_squares.cpp
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}) #target_link_libraries(pcl_ros_surface ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
#add_dependencies(pcl_ros_surface ${PROJECT_NAME}_gencfg)
add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) #class_loader_hide_library_symbols(pcl_ros_surface)
target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) #
### Tools
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}) #add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp)
#target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
## Downloads #
#add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
catkin_download(table_scene_lms400.pcd http://download.ros.org/data/pcl/table_scene_lms400.pcd #target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data #
MD5 546b5b4822fb1de21b0cf83d41ad6683 #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_custom_target(download ALL DEPENDS download_extra_data) #
#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 ## ## Testing ##
############# #############
if(CATKIN_ENABLE_TESTING) if(BUILD_TESTING)
find_package(rostest REQUIRED) find_package(ament_lint_auto REQUIRED)
add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp) set(ament_cmake_copyright_FOUND TRUE)
target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) ament_lint_auto_find_test_dependencies()
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) find_package(ament_cmake_gtest REQUIRED)
add_rostest(samples/pcl_ros/filters/sample_voxel_grid.launch ARGS gui:=false) #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp)
add_rostest(samples/pcl_ros/segmentation/sample_extract_clusters.launch ARGS gui:=false) #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
add_rostest(samples/pcl_ros/surface/sample_convex_hull.launch ARGS gui:=false) #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false)
endif(CATKIN_ENABLE_TESTING) #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}/ install(
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) DIRECTORY include/
DESTINATION include
)
install( install(
TARGETS TARGETS
pcl_ros_tf pcl_ros_tf
pcl_ros_io # pcl_ros_io
pcl_ros_features # pcl_ros_features
pcl_ros_filters # pcl_ros_filters
pcl_ros_surface # pcl_ros_surface
pcl_ros_segmentation # pcl_ros_segmentation
pcd_to_pointcloud # pcd_to_pointcloud
pointcloud_to_pcd # pointcloud_to_pcd
bag_to_pcd # bag_to_pcd
convert_pcd_to_image # convert_pcd_to_image
convert_pointcloud_to_image # convert_pointcloud_to_image
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} EXPORT export_pcl_ros
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION bin
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
INCLUDES DESTINATION include
) )
install(DIRECTORY plugins samples 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()

0
pcl_ros/COLCON_IGNORE
View File

132
pcl_ros/include/pcl_ros/impl/transforms.hpp
View File

@ -37,9 +37,22 @@
#ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_ #ifndef PCL_ROS__IMPL__TRANSFORMS_HPP_
#define PCL_ROS__IMPL__TRANSFORMS_HPP_ #define PCL_ROS__IMPL__TRANSFORMS_HPP_
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/convert.h>
#include <tf2/exceptions.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
#include <Eigen/Dense>
#include <string>
using pcl_conversions::fromPCL; using pcl_conversions::fromPCL;
using pcl_conversions::toPCL; using pcl_conversions::toPCL;
@ -51,16 +64,16 @@ template<typename PointT>
void void
transformPointCloudWithNormals( transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform) pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform)
{ {
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // 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 // 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. // 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 // 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. // 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) 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::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v); // Eigen::Translation3f translation(v);
// Assemble an Eigen Transform // Assemble an Eigen Transform
@ -69,21 +82,33 @@ transformPointCloudWithNormals(
transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation); transformPointCloudWithNormals(cloud_in, cloud_out, origin, rotation);
} }
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const geometry_msgs::msg::TransformStamped & transform)
{
tf2::Transform tf;
tf2::convert(transform.transform, tf);
transformPointCloudWithNormals(cloud_in, cloud_out, tf);
}
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> template<typename PointT>
void void
transformPointCloud( transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const tf::Transform & transform) pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform)
{ {
// Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering // 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 // 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. // 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 // 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. // 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) 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::Vector3f origin(v.x(), v.y(), v.z());
// Eigen::Translation3f translation(v); // Eigen::Translation3f translation(v);
// Assemble an Eigen Transform // Assemble an Eigen Transform
@ -92,6 +117,18 @@ transformPointCloud(
transformPointCloud(cloud_in, cloud_out, origin, rotation); transformPointCloud(cloud_in, cloud_out, origin, rotation);
} }
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
void
transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, const geometry_msgs::msg::TransformStamped & transform)
{
tf2::Transform tf;
tf2::convert(transform.transform, tf);
transformPointCloud(cloud_in, cloud_out, tf);
}
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> template<typename PointT>
bool bool
@ -99,23 +136,24 @@ transformPointCloudWithNormals(
const std::string & target_frame, const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener) const tf2_ros::Buffer & tf_buffer)
{ {
if (cloud_in.header.frame_id == target_frame) { if (cloud_in.header.frame_id == target_frame) {
cloud_out = cloud_in; cloud_out = cloud_in;
return true; return true;
} }
tf::StampedTransform transform; geometry_msgs::msg::TransformStamped transform;
try { try {
tf_listener.lookupTransform( transform =
target_frame, cloud_in.header.frame_id, fromPCL( tf_buffer.lookupTransform(
cloud_in.header).stamp, transform); target_frame, cloud_in.header.frame_id,
} catch (tf::LookupException & e) { fromPCL(cloud_in.header.stamp));
ROS_ERROR("%s", e.what()); } catch (tf2::LookupException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} catch (tf::ExtrapolationException & e) { } catch (tf2::ExtrapolationException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} }
@ -131,25 +169,27 @@ transformPointCloud(
const std::string & target_frame, const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener) const tf2_ros::Buffer & tf_buffer)
{ {
if (cloud_in.header.frame_id == target_frame) { if (cloud_in.header.frame_id == target_frame) {
cloud_out = cloud_in; cloud_out = cloud_in;
return true; return true;
} }
tf::StampedTransform transform; geometry_msgs::msg::TransformStamped transform;
try { try {
tf_listener.lookupTransform( transform =
target_frame, cloud_in.header.frame_id, fromPCL( tf_buffer.lookupTransform(
cloud_in.header).stamp, transform); target_frame, cloud_in.header.frame_id,
} catch (tf::LookupException & e) { fromPCL(cloud_in.header.stamp));
ROS_ERROR("%s", e.what()); } catch (tf2::LookupException & e) {
RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} catch (tf::ExtrapolationException & e) { } catch (tf2::ExtrapolationException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} }
transformPointCloud(cloud_in, cloud_out, transform); transformPointCloud(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
return true; return true;
@ -160,28 +200,28 @@ template<typename PointT>
bool bool
transformPointCloud( transformPointCloud(
const std::string & target_frame, const std::string & target_frame,
const ros::Time & target_time, const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame, const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener) const tf2_ros::Buffer & tf_buffer)
{ {
tf::StampedTransform transform; geometry_msgs::msg::TransformStamped transform;
try { try {
tf_listener.lookupTransform( transform = tf_buffer.lookupTransform(
target_frame, target_time, cloud_in.header.frame_id, target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, fixed_frame, transform); fromPCL(cloud_in.header.stamp), fixed_frame);
} catch (tf::LookupException & e) { } catch (tf2::LookupException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} catch (tf::ExtrapolationException & e) { } catch (tf2::ExtrapolationException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} }
transformPointCloud(cloud_in, cloud_out, transform); transformPointCloud(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
std_msgs::Header header; std_msgs::msg::Header header;
header.stamp = target_time; header.stamp = target_time;
cloud_out.header = toPCL(header); cloud_out.header = toPCL(header);
return true; return true;
@ -192,28 +232,28 @@ template<typename PointT>
bool bool
transformPointCloudWithNormals( transformPointCloudWithNormals(
const std::string & target_frame, const std::string & target_frame,
const ros::Time & target_time, const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame, const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & cloud_out,
const tf::TransformListener & tf_listener) const tf2_ros::Buffer & tf_buffer)
{ {
tf::StampedTransform transform; geometry_msgs::msg::TransformStamped transform;
try { try {
tf_listener.lookupTransform( transform = tf_buffer.lookupTransform(
target_frame, target_time, cloud_in.header.frame_id, target_frame, target_time, cloud_in.header.frame_id,
fromPCL(cloud_in.header).stamp, fixed_frame, transform); fromPCL(cloud_in.header.stamp), fixed_frame);
} catch (tf::LookupException & e) { } catch (tf2::LookupException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} catch (tf::ExtrapolationException & e) { } catch (tf2::ExtrapolationException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} }
transformPointCloudWithNormals(cloud_in, cloud_out, transform); transformPointCloudWithNormals(cloud_in, cloud_out, transform);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
std_msgs::Header header; std_msgs::msg::Header header;
header.stamp = target_time; header.stamp = target_time;
cloud_out.header = toPCL(header); cloud_out.header = toPCL(header);
return true; return true;

101
pcl_ros/include/pcl_ros/transforms.hpp
View File

@ -37,10 +37,14 @@
#ifndef PCL_ROS__TRANSFORMS_HPP_ #ifndef PCL_ROS__TRANSFORMS_HPP_
#define PCL_ROS__TRANSFORMS_HPP_ #define PCL_ROS__TRANSFORMS_HPP_
#include <sensor_msgs/PointCloud2.h>
#include <pcl/common/transforms.h> #include <pcl/common/transforms.h>
#include <tf/transform_datatypes.h> #include <tf2/LinearMath/Transform.h>
#include <tf/transform_listener.h> #include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <rclcpp/time.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <Eigen/Dense>
#include <string> #include <string>
namespace pcl_ros namespace pcl_ros
@ -56,13 +60,26 @@ void
transformPointCloudWithNormals( transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & 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<typename PointT>
void
transformPointCloudWithNormals(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const geometry_msgs::msg::TransformStamped & transform);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener /** \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 target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param cloud_out 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<typename PointT> template<typename PointT>
bool bool
@ -70,7 +87,7 @@ transformPointCloudWithNormals(
const std::string & target_frame, const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & 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 /** \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 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 cloud_in the input point cloud
* \param fixed_frame fixed TF frame * \param fixed_frame fixed TF frame
* \param cloud_out 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<typename PointT> template<typename PointT>
bool bool
transformPointCloudWithNormals( transformPointCloudWithNormals(
const std::string & target_frame, const std::string & target_frame,
const ros::Time & target_time, const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame, const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & 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 /** \brief Apply a rigid transform defined by a 3D offset and a quaternion
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
@ -101,13 +118,26 @@ void
transformPointCloud( transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & 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<typename PointT>
void
transformPointCloud(
const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out,
const geometry_msgs::msg::TransformStamped & transform);
/** \brief Transforms a point cloud in a given target TF frame using a TransformListener /** \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 target_frame the target TF frame the point cloud should be transformed to
* \param cloud_in the input point cloud * \param cloud_in the input point cloud
* \param cloud_out 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<typename PointT> template<typename PointT>
bool bool
@ -115,7 +145,7 @@ transformPointCloud(
const std::string & target_frame, const std::string & target_frame,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & 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 /** \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 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 cloud_in the input point cloud
* \param fixed_frame fixed TF frame * \param fixed_frame fixed TF frame
* \param cloud_out 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<typename PointT> template<typename PointT>
bool bool
transformPointCloud( transformPointCloud(
const std::string & target_frame, const ros::Time & target_time, const std::string & target_frame,
const rclcpp::Time & target_time,
const pcl::PointCloud<PointT> & cloud_in, const pcl::PointCloud<PointT> & cloud_in,
const std::string & fixed_frame, const std::string & fixed_frame,
pcl::PointCloud<PointT> & cloud_out, pcl::PointCloud<PointT> & 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. /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame * \param target_frame the target TF frame
* \param in the input PointCloud2 dataset * \param in the input PointCloud2 dataset
* \param out the resultant transformed PointCloud2 dataset * \param out the resultant transformed PointCloud2 dataset
* \param tf_listener a TF listener object * \param tf_buffer a TF buffer object
*/ */
bool bool
transformPointCloud( transformPointCloud(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::PointCloud2 & in, const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::PointCloud2 & out, sensor_msgs::msg::PointCloud2 & 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. /** \brief Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame.
* \param target_frame the target TF frame * \param target_frame the target TF frame
@ -156,9 +187,22 @@ transformPointCloud(
void void
transformPointCloud( transformPointCloud(
const std::string & target_frame, const std::string & target_frame,
const tf::Transform & net_transform, const tf2::Transform & net_transform,
const sensor_msgs::PointCloud2 & in, const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::PointCloud2 & out); 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. /** \brief Transform a sensor_msgs::PointCloud2 dataset using an Eigen 4x4 matrix.
* \param transform the transformation to use on the points * \param transform the transformation to use on the points
@ -168,15 +212,22 @@ transformPointCloud(
void void
transformPointCloud( transformPointCloud(
const Eigen::Matrix4f & transform, const Eigen::Matrix4f & transform,
const sensor_msgs::PointCloud2 & in, const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::PointCloud2 & out); sensor_msgs::msg::PointCloud2 & out);
/** \brief Obtain the transformation matrix from TF into an Eigen form /** \brief Obtain the transformation matrix from TF into an Eigen form
* \param bt the TF transformation * \param bt the TF transformation
* \param out_mat the Eigen transformation * \param out_mat the Eigen transformation
*/ */
void 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 } // namespace pcl_ros
#endif // PCL_ROS__TRANSFORMS_HPP_ #endif // PCL_ROS__TRANSFORMS_HPP_

41
pcl_ros/package.xml
View File

@ -1,5 +1,6 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pcl_ros</name> <name>pcl_ros</name>
<version>2.2.0</version> <version>2.2.0</version>
<description> <description>
@ -10,10 +11,6 @@
</description> </description>
<author>Open Perception</author>
<author email="julius@kammerl.de">Julius Kammerl</author>
<author email="william@osrfoundation.org">William Woodall</author>
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer> <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer> <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer> <maintainer email="www.kentaro.wada@gmail.com">Kentaro Wada</maintainer>
@ -24,29 +21,26 @@
<url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url> <url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
<url type="repository">https://github.com/ros-perception/perception_pcl</url> <url type="repository">https://github.com/ros-perception/perception_pcl</url>
<buildtool_depend>catkin</buildtool_depend> <author>Open Perception</author>
<build_depend>cmake_modules</build_depend> <author email="julius@kammerl.de">Julius Kammerl</author>
<build_depend>rosconsole</build_depend> <author email="william@osrfoundation.org">William Woodall</author>
<build_depend>roslib</build_depend>
<depend>dynamic_reconfigure</depend> <buildtool_depend>ament_cmake</buildtool_depend>
<depend>eigen</depend> <depend>eigen</depend>
<depend>message_filters</depend>
<depend>nodelet</depend>
<depend>nodelet_topic_tools</depend>
<depend>libpcl-all-dev</depend> <depend>libpcl-all-dev</depend>
<depend>pcl_conversions</depend> <depend>pcl_conversions</depend>
<depend>pcl_msgs</depend> <depend>rclcpp</depend>
<depend>pluginlib</depend>
<depend>rosbag</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
<depend>std_msgs</depend> <depend>geometry_msgs</depend>
<depend>tf</depend> <depend>tf2</depend>
<depend>tf2_eigen</depend> <depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<test_depend>rostest</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<!--
<export> <export>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_features.xml"/> <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_features.xml"/>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_filters.xml"/> <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_filters.xml"/>
@ -54,5 +48,10 @@
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_segmentation.xml"/> <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_segmentation.xml"/>
<nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_surface.xml"/> <nodelet plugin="${prefix}/plugins/nodelet/libpcl_ros_surface.xml"/>
</export> </export>
-->
<export>
<build_type>ament_cmake</build_type>
</export>
</package> </package>

250
pcl_ros/src/transforms.cpp
View File

@ -34,22 +34,34 @@
* *
*/ */
#include <sensor_msgs/PointCloud2.h>
#include <pcl/common/io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <limits>
#include <string>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
#include "pcl_ros/impl/transforms.hpp" #include "pcl_ros/impl/transforms.hpp"
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/convert.h>
#include <tf2/exceptions.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
#include <Eigen/Dense>
#include <cmath>
#include <limits>
#include <string>
namespace pcl_ros namespace pcl_ros
{ {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool bool
transformPointCloud( transformPointCloud(
const std::string & target_frame, const sensor_msgs::PointCloud2 & in, const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::PointCloud2 & out, const tf::TransformListener & tf_listener) sensor_msgs::msg::PointCloud2 & out, const tf2_ros::Buffer & tf_buffer)
{ {
if (in.header.frame_id == target_frame) { if (in.header.frame_id == target_frame) {
out = in; out = in;
@ -57,14 +69,17 @@ transformPointCloud(
} }
// Get the TF transform // Get the TF transform
tf::StampedTransform transform; geometry_msgs::msg::TransformStamped transform;
try { try {
tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform); transform =
} catch (tf::LookupException & e) { tf_buffer.lookupTransform(
ROS_ERROR("%s", e.what()); 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; return false;
} catch (tf::ExtrapolationException & e) { } catch (tf2::ExtrapolationException & e) {
ROS_ERROR("%s", e.what()); RCLCPP_ERROR(rclcpp::get_logger("pcl_ros"), "%s", e.what());
return false; return false;
} }
@ -81,8 +96,8 @@ transformPointCloud(
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
transformPointCloud( transformPointCloud(
const std::string & target_frame, const tf::Transform & net_transform, const std::string & target_frame, const tf2::Transform & net_transform,
const sensor_msgs::PointCloud2 & in, sensor_msgs::PointCloud2 & out) const sensor_msgs::msg::PointCloud2 & in, sensor_msgs::msg::PointCloud2 & out)
{ {
if (in.header.frame_id == target_frame) { if (in.header.frame_id == target_frame) {
out = in; out = in;
@ -98,11 +113,21 @@ transformPointCloud(
out.header.frame_id = target_frame; 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 void
transformPointCloud( transformPointCloud(
const Eigen::Matrix4f & transform, const sensor_msgs::PointCloud2 & in, const Eigen::Matrix4f & transform, const sensor_msgs::msg::PointCloud2 & in,
sensor_msgs::PointCloud2 & out) sensor_msgs::msg::PointCloud2 & out)
{ {
// Get X-Y-Z indices // Get X-Y-Z indices
int x_idx = pcl::getFieldIndex(in, "x"); int x_idx = pcl::getFieldIndex(in, "x");
@ -110,15 +135,19 @@ transformPointCloud(
int z_idx = pcl::getFieldIndex(in, "z"); int z_idx = pcl::getFieldIndex(in, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1) { 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; return;
} }
if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 || if (in.fields[x_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 ||
in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 || in.fields[y_idx].datatype != sensor_msgs::msg::PointField::FLOAT32 ||
in.fields[z_idx].datatype != sensor_msgs::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; return;
} }
@ -144,15 +173,15 @@ transformPointCloud(
in.fields[z_idx].offset, 0); in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i) { for (size_t i = 0; i < in.width * in.height; ++i) {
Eigen::Vector4f pt(*reinterpret_cast<float *>(&in.data[xyz_offset[0]]), Eigen::Vector4f pt(*reinterpret_cast<const float *>(&in.data[xyz_offset[0]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[1]]), *reinterpret_cast<const float *>(&in.data[xyz_offset[1]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[2]], 1)); *reinterpret_cast<const float *>(&in.data[xyz_offset[2]]), 1);
Eigen::Vector4f pt_out; Eigen::Vector4f pt_out;
bool max_range_point = false; bool max_range_point = false;
int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset; int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset;
float * distance_ptr = const float * distance_ptr = (dist_idx < 0 ?
(dist_idx < 0 ? NULL : reinterpret_cast<float *>(&in.data[distance_ptr_offset])); NULL : reinterpret_cast<const float *>(&in.data[distance_ptr_offset]));
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) { if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
pt_out = pt; pt_out = pt;
@ -160,8 +189,6 @@ transformPointCloud(
pt[0] = *distance_ptr; // Replace x with the x value saved in distance pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt; pt_out = transform * pt;
max_range_point = true; max_range_point = true;
// std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<",
// "<<pt_out[1]<<","<<pt_out[2]<<"\n";
} }
} else { } else {
pt_out = transform * pt; pt_out = transform * pt;
@ -201,12 +228,12 @@ transformPointCloud(
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat) transformAsMatrix(const tf2::Transform & bt, Eigen::Matrix4f & out_mat)
{ {
double mv[12]; double mv[12];
bt.getBasis().getOpenGLSubMatrix(mv); bt.getBasis().getOpenGLSubMatrix(mv);
tf::Vector3 origin = bt.getOrigin(); tf2::Vector3 origin = bt.getOrigin();
out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8]; out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8];
out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9]; out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9];
@ -218,176 +245,231 @@ transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat)
out_mat(2, 3) = origin.z(); out_mat(2, 3) = origin.z();
} }
void
transformAsMatrix(const geometry_msgs::msg::TransformStamped & bt, Eigen::Matrix4f & out_mat)
{
tf2::Transform transform;
tf2::convert(bt.transform, transform);
transformAsMatrix(transform, out_mat);
}
} // namespace pcl_ros } // namespace pcl_ros
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>( template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>( template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>( template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::Transform &); const tf2::Transform &);
//////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const geometry_msgs::msg::TransformStamped &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>( template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>( template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>( template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>( template bool pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointNormal> &, const std::string &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointNormal> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>( template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZRGBNormal>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>( template bool pcl_ros::transformPointCloudWithNormals<pcl::PointXYZINormal>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZINormal> &, const tf2_ros::Buffer &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloud<pcl::PointXYZ>( template void pcl_ros::transformPointCloud<pcl::PointXYZ>(
const pcl::PointCloud<pcl::PointXYZ> &, const pcl::PointCloud<pcl::PointXYZ> &,
pcl::PointCloud<pcl::PointXYZ> &, pcl::PointCloud<pcl::PointXYZ> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZI>( template void pcl_ros::transformPointCloud<pcl::PointXYZI>(
const pcl::PointCloud<pcl::PointXYZI> &, const pcl::PointCloud<pcl::PointXYZI> &,
pcl::PointCloud<pcl::PointXYZI> &, pcl::PointCloud<pcl::PointXYZI> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>( template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &, const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB>( template void pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &, const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::InterestPoint>( template void pcl_ros::transformPointCloud<pcl::InterestPoint>(
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &, const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointNormal>( template void pcl_ros::transformPointCloud<pcl::PointNormal>(
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>( template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal>( template void pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithRange>( template void pcl_ros::transformPointCloud<pcl::PointWithRange>(
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &, const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
const tf::Transform &); const tf2::Transform &);
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint>( template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &, const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
const tf::Transform &); const tf2::Transform &);
//////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloud<pcl::PointXYZ>(
const pcl::PointCloud<pcl::PointXYZ> &,
pcl::PointCloud<pcl::PointXYZ> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointXYZI>(
const pcl::PointCloud<pcl::PointXYZI> &,
pcl::PointCloud<pcl::PointXYZI> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::InterestPoint>(
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointNormal>(
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointWithRange>(
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
const geometry_msgs::msg::TransformStamped &);
template void pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
const geometry_msgs::msg::TransformStamped &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloud<pcl::PointXYZ>( template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZ> &, const pcl::PointCloud<pcl::PointXYZ> &,
pcl::PointCloud<pcl::PointXYZ> &, pcl::PointCloud<pcl::PointXYZ> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZI>( template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZI> &, const pcl::PointCloud<pcl::PointXYZI> &,
pcl::PointCloud<pcl::PointXYZI> &, pcl::PointCloud<pcl::PointXYZI> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>( template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &, const pcl::PointCloud<pcl::PointXYZRGBA> &, pcl::PointCloud<pcl::PointXYZRGBA> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>( template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &, const pcl::PointCloud<pcl::PointXYZRGB> &, pcl::PointCloud<pcl::PointXYZRGB> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::InterestPoint>( template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &, const pcl::PointCloud<pcl::InterestPoint> &, pcl::PointCloud<pcl::InterestPoint> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal>( template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &, const pcl::PointCloud<pcl::PointNormal> &, pcl::PointCloud<pcl::PointNormal> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>( template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, pcl::PointCloud<pcl::PointXYZRGBNormal> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>( template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &, const pcl::PointCloud<pcl::PointXYZINormal> &, pcl::PointCloud<pcl::PointXYZINormal> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointWithRange>( template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &, const pcl::PointCloud<pcl::PointWithRange> &, pcl::PointCloud<pcl::PointWithRange> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>( template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const std::string &, const std::string &,
const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &, const pcl::PointCloud<pcl::PointWithViewpoint> &, pcl::PointCloud<pcl::PointWithViewpoint> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
template bool pcl_ros::transformPointCloud<pcl::PointXYZ>( template bool pcl_ros::transformPointCloud<pcl::PointXYZ>(
const std::string &, const ros::Time &, const std::string &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZ> &, const pcl::PointCloud<pcl::PointXYZ> &,
const std::string &, const std::string &,
pcl::PointCloud<pcl::PointXYZ> &, pcl::PointCloud<pcl::PointXYZ> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZI>( template bool pcl_ros::transformPointCloud<pcl::PointXYZI>(
const std::string &, const ros::Time &, const std::string &,
const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZI> &, const pcl::PointCloud<pcl::PointXYZI> &,
const std::string &, const std::string &,
pcl::PointCloud<pcl::PointXYZI> &, pcl::PointCloud<pcl::PointXYZI> &,
const tf::TransformListener &); const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>( template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBA>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZRGBA> &, const std::string &, const pcl::PointCloud<pcl::PointXYZRGBA> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGBA> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZRGBA> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>( template bool pcl_ros::transformPointCloud<pcl::PointXYZRGB>(
const std::string &, const ros::Time &, const std::string &,
const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZRGB> &, const std::string &, const pcl::PointCloud<pcl::PointXYZRGB> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGB> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZRGB> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::InterestPoint>( template bool pcl_ros::transformPointCloud<pcl::InterestPoint>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::InterestPoint> &, const std::string &, const pcl::PointCloud<pcl::InterestPoint> &, const std::string &,
pcl::PointCloud<pcl::InterestPoint> &, const tf::TransformListener &); pcl::PointCloud<pcl::InterestPoint> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointNormal>( template bool pcl_ros::transformPointCloud<pcl::PointNormal>(
const std::string &, const ros::Time &, const std::string &,
const rclcpp::Time &,
const pcl::PointCloud<pcl::PointNormal> &, const std::string &, const pcl::PointCloud<pcl::PointNormal> &, const std::string &,
pcl::PointCloud<pcl::PointNormal> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointNormal> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>( template bool pcl_ros::transformPointCloud<pcl::PointXYZRGBNormal>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &, const pcl::PointCloud<pcl::PointXYZRGBNormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZRGBNormal> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>( template bool pcl_ros::transformPointCloud<pcl::PointXYZINormal>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &, const pcl::PointCloud<pcl::PointXYZINormal> &, const std::string &,
pcl::PointCloud<pcl::PointXYZINormal> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointXYZINormal> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointWithRange>( template bool pcl_ros::transformPointCloud<pcl::PointWithRange>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointWithRange> &, const std::string &, const pcl::PointCloud<pcl::PointWithRange> &, const std::string &,
pcl::PointCloud<pcl::PointWithRange> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointWithRange> &, const tf2_ros::Buffer &);
template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>( template bool pcl_ros::transformPointCloud<pcl::PointWithViewpoint>(
const std::string &, const std::string &,
const ros::Time &, const rclcpp::Time &,
const pcl::PointCloud<pcl::PointWithViewpoint> &, const std::string &, const pcl::PointCloud<pcl::PointWithViewpoint> &, const std::string &,
pcl::PointCloud<pcl::PointWithViewpoint> &, const tf::TransformListener &); pcl::PointCloud<pcl::PointWithViewpoint> &, const tf2_ros::Buffer &);

2
perception_pcl/package.xml
View File

@ -27,7 +27,7 @@
<exec_depend>pcl_conversions</exec_depend> <exec_depend>pcl_conversions</exec_depend>
<exec_depend>pcl_msgs</exec_depend> <exec_depend>pcl_msgs</exec_depend>
<!--<exec_depend>pcl_ros</exec_depend>--> <exec_depend>pcl_ros</exec_depend>
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>