DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

provide support for tf2

Browse Source
This commit is contained in:
Vincent Rabaud 2014-09-28 16:18:23 +02:00
parent 04c06208d9
commit 8d916f93d5
4 changed files with 151 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -8,6 +8,7 @@ find_package(catkin REQUIRED
roscpp roscpp
sensor_msgs sensor_msgs
tf tf
tf2
) )
find_package(Boost REQUIRED COMPONENTS system thread) find_package(Boost REQUIRED COMPONENTS system thread)

54
include/laser_geometry/laser_geometry.h
View File

@ -37,7 +37,8 @@
#include "boost/numeric/ublas/matrix.hpp" #include "boost/numeric/ublas/matrix.hpp"
#include "boost/thread/mutex.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/LaserScan.h"
#include "sensor_msgs/PointCloud.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); 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: protected:
//! Internal protected representation of getUnitVectors //! Internal protected representation of getUnitVectors
@ -289,6 +322,25 @@ namespace laser_geometry
double range_cutoff, double range_cutoff,
int channel_options); 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 //! Internal map of pointers to stored values
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_; std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
float angle_min_; float angle_min_;

102
src/laser_geometry.cpp
View File

@ -30,6 +30,7 @@
#include "laser_geometry/laser_geometry.h" #include "laser_geometry/laser_geometry.h"
#include <algorithm> #include <algorithm>
#include <ros/assert.h> #include <ros/assert.h>
#include <tf2/LinearMath/Transform.h>
namespace laser_geometry namespace laser_geometry
{ {
@ -491,10 +492,13 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.data.resize (cloud_out.row_step * cloud_out.height); cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
} }
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, 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, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -540,14 +544,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
// Extract transforms for the beginning and end of the laser scan tf2::Transform cur_transform ;
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);
double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0); 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) //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
// Interpolate translation // Interpolate translation
tf::Vector3 v (0, 0, 0); tf2::Vector3 v (0, 0, 0);
v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio); v.setInterpolate3 (origin_start, origin_end, ratio);
cur_transform.setOrigin (v); 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 // 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]); tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
tf::Vector3 point_out = cur_transform * point_in; tf2::Vector3 point_out = cur_transform * point_in;
// Copy transformed point into cloud // Copy transformed point into cloud
pstep[0] = point_out.x (); pstep[0] = point_out.x ();
@ -590,7 +582,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
if(has_viewpoint) if(has_viewpoint)
{ {
float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset]; 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; point_out = cur_transform * point_in;
// Copy transformed point into cloud // 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 } //laser_geometry

17
test/projection_test.cpp
View File

@ -540,7 +540,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
{ {
tf::Transformer tf; tf::Transformer tf;
tf2::BufferCore tf2;
double tolerance = 1e-12; double tolerance = 1e-12;
laser_geometry::LaserProjection projector; laser_geometry::LaserProjection projector;
@ -613,21 +614,35 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
sensor_msgs::PointCloud2 cloud_out; sensor_msgs::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None); 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); 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); 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); 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); 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); 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); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); 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); 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); 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); 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); 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); 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); 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); EXPECT_EQ(cloud_out.is_dense, false);