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 <sean@seankelly.dev> * Feedback from PR
This commit is contained in:
Sean Kelly
GitHub
@@ -37,9 +37,22 @@
|
||||
#ifndef 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/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::toPCL;
|
||||
@@ -51,16 +64,16 @@ template<typename PointT>
|
||||
void
|
||||
transformPointCloudWithNormals(
|
||||
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
|
||||
// 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<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>
|
||||
void
|
||||
transformPointCloud(
|
||||
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
|
||||
// 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<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>
|
||||
bool
|
||||
@@ -99,23 +136,24 @@ transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & 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<PointT> & cloud_in,
|
||||
pcl::PointCloud<PointT> & 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<typename PointT>
|
||||
bool
|
||||
transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const ros::Time & target_time,
|
||||
const rclcpp::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
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 {
|
||||
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<typename PointT>
|
||||
bool
|
||||
transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const ros::Time & target_time,
|
||||
const rclcpp::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
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 {
|
||||
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;
|
||||
|
||||
@@ -37,10 +37,14 @@
|
||||
#ifndef PCL_ROS__TRANSFORMS_HPP_
|
||||
#define PCL_ROS__TRANSFORMS_HPP_
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <tf/transform_datatypes.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <tf2/LinearMath/Transform.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>
|
||||
|
||||
namespace pcl_ros
|
||||
@@ -56,13 +60,26 @@ void
|
||||
transformPointCloudWithNormals(
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
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
|
||||
* \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<typename PointT>
|
||||
bool
|
||||
@@ -70,7 +87,7 @@ transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
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
|
||||
* \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<typename PointT>
|
||||
bool
|
||||
transformPointCloudWithNormals(
|
||||
const std::string & target_frame,
|
||||
const ros::Time & target_time,
|
||||
const rclcpp::Time & target_time,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
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
|
||||
* \param cloud_in the input point cloud
|
||||
@@ -101,13 +118,26 @@ void
|
||||
transformPointCloud(
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
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
|
||||
* \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<typename PointT>
|
||||
bool
|
||||
@@ -115,7 +145,7 @@ transformPointCloud(
|
||||
const std::string & target_frame,
|
||||
const pcl::PointCloud<PointT> & cloud_in,
|
||||
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
|
||||
* \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<typename PointT>
|
||||
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<PointT> & cloud_in,
|
||||
const std::string & fixed_frame,
|
||||
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.
|
||||
* \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_
|
||||
|
||||
Reference in New Issue
Block a user