provide support for tf2
This commit is contained in:
Vincent Rabaud
parent
04c06208d9
commit
8d916f93d5
|
|
@ -8,6 +8,7 @@ find_package(catkin REQUIRED
|
|||
roscpp
|
||||
sensor_msgs
|
||||
tf
|
||||
tf2
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED COMPONENTS system thread)
|
||||
|
|
|
|||
|
|
@ -37,7 +37,8 @@
|
|||
#include "boost/numeric/ublas/matrix.hpp"
|
||||
#include "boost/thread/mutex.hpp"
|
||||
|
||||
#include "tf/tf.h"
|
||||
#include <tf/tf.h>
|
||||
#include <tf2/buffer_core.h>
|
||||
|
||||
#include "sensor_msgs/LaserScan.h"
|
||||
#include "sensor_msgs/PointCloud.h"
|
||||
|
|
@ -245,6 +246,38 @@ namespace laser_geometry
|
|||
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
|
||||
}
|
||||
|
||||
//! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame
|
||||
/*!
|
||||
* Transform a single laser scan from a linear array into a 3D
|
||||
* point cloud, accounting for movement of the laser over the
|
||||
* course of the scan. In order for this transform to be
|
||||
* meaningful at a single point in time, the target_frame must
|
||||
* be a fixed reference frame. See the tf documentation for
|
||||
* more information on fixed frames.
|
||||
*
|
||||
* \param target_frame The frame of the resulting point cloud
|
||||
* \param scan_in The input laser scan
|
||||
* \param cloud_out The output point cloud
|
||||
* \param tf a tf2::BufferCore object to use to perform the
|
||||
* transform
|
||||
* \param range_cutoff An additional range cutoff which can be
|
||||
* applied to discard everything above it.
|
||||
* Defaults to -1.0, which means the laser scan max range.
|
||||
* \param channel_option An OR'd set of channels to include.
|
||||
* Options include: channel_option::Default,
|
||||
* channel_option::Intensity, channel_option::Index,
|
||||
* channel_option::Distance, channel_option::Timestamp.
|
||||
*/
|
||||
void transformLaserScanToPointCloud(const std::string &target_frame,
|
||||
const sensor_msgs::LaserScan &scan_in,
|
||||
sensor_msgs::PointCloud2 &cloud_out,
|
||||
tf2::BufferCore &tf,
|
||||
double range_cutoff = -1.0,
|
||||
int channel_options = channel_option::Default)
|
||||
{
|
||||
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
//! Internal protected representation of getUnitVectors
|
||||
|
|
@ -289,6 +322,25 @@ namespace laser_geometry
|
|||
double range_cutoff,
|
||||
int channel_options);
|
||||
|
||||
//! Internal hidden representation of transformLaserScanToPointCloud2
|
||||
void transformLaserScanToPointCloud_ (const std::string &target_frame,
|
||||
const sensor_msgs::LaserScan &scan_in,
|
||||
sensor_msgs::PointCloud2 &cloud_out,
|
||||
tf2::BufferCore &tf,
|
||||
double range_cutoff,
|
||||
int channel_options);
|
||||
|
||||
//! Function used by the several forms of transformLaserScanToPointCloud_
|
||||
void transformLaserScanToPointCloud_ (const std::string &target_frame,
|
||||
const sensor_msgs::LaserScan &scan_in,
|
||||
sensor_msgs::PointCloud2 &cloud_out,
|
||||
tf2::Quaternion quat_start,
|
||||
tf2::Vector3 origin_start,
|
||||
tf2::Quaternion quat_end,
|
||||
tf2::Vector3 origin_end,
|
||||
double range_cutoff,
|
||||
int channel_options);
|
||||
|
||||
//! Internal map of pointers to stored values
|
||||
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
|
||||
float angle_min_;
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
#include "laser_geometry/laser_geometry.h"
|
||||
#include <algorithm>
|
||||
#include <ros/assert.h>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
|
||||
namespace laser_geometry
|
||||
{
|
||||
|
|
@ -494,7 +495,10 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame,
|
||||
const sensor_msgs::LaserScan &scan_in,
|
||||
sensor_msgs::PointCloud2 &cloud_out,
|
||||
tf::Transformer &tf,
|
||||
tf2::Quaternion quat_start,
|
||||
tf2::Vector3 origin_start,
|
||||
tf2::Quaternion quat_end,
|
||||
tf2::Vector3 origin_end,
|
||||
double range_cutoff,
|
||||
int channel_options)
|
||||
{
|
||||
|
|
@ -540,14 +544,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
|
||||
// Extract transforms for the beginning and end of the laser scan
|
||||
ros::Time start_time = scan_in.header.stamp;
|
||||
ros::Time end_time = scan_in.header.stamp + ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
|
||||
|
||||
tf::StampedTransform start_transform, end_transform, cur_transform ;
|
||||
|
||||
tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
|
||||
tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
|
||||
tf2::Transform cur_transform ;
|
||||
|
||||
double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
|
||||
|
||||
|
|
@ -566,20 +563,15 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
|
||||
//! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
|
||||
// Interpolate translation
|
||||
tf::Vector3 v (0, 0, 0);
|
||||
v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio);
|
||||
tf2::Vector3 v (0, 0, 0);
|
||||
v.setInterpolate3 (origin_start, origin_end, ratio);
|
||||
cur_transform.setOrigin (v);
|
||||
|
||||
// Interpolate rotation
|
||||
tf::Quaternion q1, q2;
|
||||
start_transform.getBasis ().getRotation (q1);
|
||||
end_transform.getBasis ().getRotation (q2);
|
||||
|
||||
// Compute the slerp-ed rotation
|
||||
cur_transform.setRotation (slerp (q1, q2 , ratio));
|
||||
cur_transform.setRotation (slerp (quat_start, quat_end , ratio));
|
||||
|
||||
tf::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
|
||||
tf::Vector3 point_out = cur_transform * point_in;
|
||||
tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
|
||||
tf2::Vector3 point_out = cur_transform * point_in;
|
||||
|
||||
// Copy transformed point into cloud
|
||||
pstep[0] = point_out.x ();
|
||||
|
|
@ -590,7 +582,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
if(has_viewpoint)
|
||||
{
|
||||
float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
|
||||
point_in = tf::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
|
||||
point_in = tf2::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
|
||||
point_out = cur_transform * point_in;
|
||||
|
||||
// Copy transformed point into cloud
|
||||
|
|
@ -656,5 +648,73 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
}
|
||||
}
|
||||
|
||||
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
|
||||
const sensor_msgs::LaserScan &scan_in,
|
||||
sensor_msgs::PointCloud2 &cloud_out,
|
||||
tf::Transformer &tf,
|
||||
double range_cutoff,
|
||||
int channel_options)
|
||||
{
|
||||
ros::Time start_time = scan_in.header.stamp;
|
||||
ros::Time end_time = scan_in.header.stamp + ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
|
||||
|
||||
tf::StampedTransform start_transform, end_transform ;
|
||||
|
||||
tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
|
||||
tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
|
||||
|
||||
tf::Quaternion q;
|
||||
start_transform.getBasis().getRotation(q);
|
||||
tf2::Quaternion quat_start(q.getX(), q.getY(), q.getZ(), q.getW());
|
||||
end_transform.getBasis().getRotation(q);
|
||||
tf2::Quaternion quat_end(q.getX(), q.getY(), q.getZ(), q.getW());
|
||||
|
||||
tf2::Vector3 origin_start(start_transform.getOrigin().getX(),
|
||||
start_transform.getOrigin().getY(),
|
||||
start_transform.getOrigin().getZ());
|
||||
tf2::Vector3 origin_end(end_transform.getOrigin().getX(),
|
||||
end_transform.getOrigin().getY(),
|
||||
end_transform.getOrigin().getZ());
|
||||
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out,
|
||||
quat_start, origin_start,
|
||||
quat_end, origin_end,
|
||||
range_cutoff,
|
||||
channel_options);
|
||||
}
|
||||
|
||||
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
|
||||
const sensor_msgs::LaserScan &scan_in,
|
||||
sensor_msgs::PointCloud2 &cloud_out,
|
||||
tf2::BufferCore &tf,
|
||||
double range_cutoff,
|
||||
int channel_options)
|
||||
{
|
||||
ros::Time start_time = scan_in.header.stamp;
|
||||
ros::Time end_time = scan_in.header.stamp + ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
|
||||
|
||||
geometry_msgs::TransformStamped start_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time);
|
||||
geometry_msgs::TransformStamped end_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time);
|
||||
|
||||
tf2::Quaternion quat_start(start_transform.transform.rotation.x,
|
||||
start_transform.transform.rotation.y,
|
||||
start_transform.transform.rotation.z,
|
||||
start_transform.transform.rotation.w);
|
||||
tf2::Quaternion quat_end(end_transform.transform.rotation.x,
|
||||
end_transform.transform.rotation.y,
|
||||
end_transform.transform.rotation.z,
|
||||
end_transform.transform.rotation.w);
|
||||
|
||||
tf2::Vector3 origin_start(start_transform.transform.translation.x,
|
||||
start_transform.transform.translation.y,
|
||||
start_transform.transform.translation.z);
|
||||
tf2::Vector3 origin_end(end_transform.transform.translation.x,
|
||||
end_transform.transform.translation.y,
|
||||
end_transform.transform.translation.z);
|
||||
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out,
|
||||
quat_start, origin_start,
|
||||
quat_end, origin_end,
|
||||
range_cutoff,
|
||||
channel_options);
|
||||
}
|
||||
|
||||
} //laser_geometry
|
||||
|
|
|
|||
|
|
@ -540,6 +540,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
|
|||
{
|
||||
|
||||
tf::Transformer tf;
|
||||
tf2::BufferCore tf2;
|
||||
|
||||
double tolerance = 1e-12;
|
||||
laser_geometry::LaserProjection projector;
|
||||
|
|
@ -613,21 +614,35 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
|
|||
sensor_msgs::PointCloud2 cloud_out;
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
|
||||
|
||||
EXPECT_EQ(cloud_out.is_dense, false);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user