diff --git a/CMakeLists.txt b/CMakeLists.txt index 67a2739..28ab530 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED roscpp sensor_msgs tf + tf2 ) find_package(Boost REQUIRED COMPONENTS system thread) diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index 55fb4e7..9731898 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -37,7 +37,8 @@ #include "boost/numeric/ublas/matrix.hpp" #include "boost/thread/mutex.hpp" -#include "tf/tf.h" +#include +#include #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* > unit_vector_map_; float angle_min_; diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 56d2ecd..1393ce5 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -30,6 +30,7 @@ #include "laser_geometry/laser_geometry.h" #include #include +#include namespace laser_geometry { @@ -491,10 +492,13 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do 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, 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& 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& 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& 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& 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 diff --git a/test/projection_test.cpp b/test/projection_test.cpp index 09cadb3..c0132a0 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -540,7 +540,8 @@ 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);