From 3f5ba8926aaf18b8917111058946c548056388f4 Mon Sep 17 00:00:00 2001 From: Eitan Marder-Eppstein Date: Fri, 14 Jan 2011 08:05:45 +0000 Subject: [PATCH] Adding support for PointCloud2 to the laser projector, removing a bunch of deprecated functions, and fixing a ton of warnings git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35237 eb33c2ac-9c88-4c90-87e0-44a10359b0c3 --- CMakeLists.txt | 2 +- include/laser_geometry/laser_geometry.h | 178 +++++++--- manifest.xml | 1 + src/laser_geometry.cpp | 413 +++++++++++++++++++++--- src/laser_scan_geometry.cpp | 12 +- 5 files changed, 509 insertions(+), 97 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d680387..75303c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,7 @@ rosbuild_add_boost_directories() rosbuild_add_library(laser_geometry src/laser_geometry.cpp ) rosbuild_link_boost(laser_geometry thread) -rosbuild_add_library (laser_scan_geometry src/laser_scan_geometry.cpp) +#rosbuild_add_library (laser_scan_geometry src/laser_scan_geometry.cpp) rosbuild_add_gtest(test/projection_test test/projection_test.cpp) target_link_libraries (test/projection_test laser_geometry) diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index 8610915..e2cdc4a 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -43,14 +43,16 @@ #include "sensor_msgs/PointCloud.h" #include "sensor_msgs/PointCloud.h" -#if defined(__GNUC__) -#define DEPRECATED __attribute__((deprecated)) -#else -#define DEPRECATED -#endif +#include +#include namespace laser_geometry { + // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) + const float LASER_SCAN_INVALID = -1.0; + const float LASER_SCAN_MIN_RANGE = -2.0; + const float LASER_SCAN_MAX_RANGE = -3.0; + namespace channel_option { //! Enumerated output channels options. @@ -65,17 +67,11 @@ namespace laser_geometry Index = 0x02, //!< Enable "index" channel Distance = 0x04, //!< Enable "distances" channel Timestamp = 0x08, //!< Enable "stamps" channel + Viewpoint = 0x16, //!< Enable "viewpoint" channel Default = (Intensity | Index) //!< Enable "intensities" and "stamps" channels }; } - DEPRECATED const int MASK_INTENSITY = channel_option::Intensity; - DEPRECATED const int MASK_INDEX = channel_option::Index; - DEPRECATED const int MASK_DISTANCE = channel_option::Distance; - DEPRECATED const int MASK_TIMESTAMP = channel_option::Timestamp; - DEPRECATED const int DEFAULT_MASK = (channel_option::Intensity | channel_option::Index); - - //! \brief A Class to Project Laser Scan /*! * This class will project laser scans into point clouds. It caches @@ -131,6 +127,61 @@ namespace laser_geometry return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options); } + //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 + /*! + * Project a single laser scan from a linear array into a 3D + * point cloud. The generated cloud will be in the same frame + * as the original laser scan. + * + * \param scan_in The input laser scan + * \param cloud_out The output point cloud + * \param range_cutoff An additional range cutoff which can be + * applied which is more limiting than max_range in the scan. + * \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 projectLaser (const sensor_msgs::LaserScan& scan_in, + sensor_msgs::PointCloud2 &cloud_out, + double range_cutoff = -1.0, + int channel_options = channel_option::Default) + { + projectLaser_(scan_in, cloud_out, co_sine_map_, range_cutoff, channel_options); + } + + + //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud 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 tf::Transformer object to use to perform the + * transform + * \param range_cutoff An additional range cutoff which can be + * applied which is more limiting than max_range in the scan. + * \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::PointCloud& cloud_out, + tf::Transformer& tf, + double range_cutoff, + int channel_options = channel_option::Default) + { + return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options, false); + } + //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame /*! * Transform a single laser scan from a linear array into a 3D @@ -156,51 +207,68 @@ namespace laser_geometry tf::Transformer& tf, int channel_options = channel_option::Default) { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, channel_options, false); + return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options, false); } - //! Deprecated version of projectLaser + //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame /*! - * - The preservative argument has been removed. + * 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 tf::Transformer object to use to perform the + * transform + * \param range_cutoff An additional range cutoff which can be + * applied which is more limiting than max_range in the scan. + * \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. */ - DEPRECATED - void projectLaser (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud & cloud_out, - double range_cutoff, - bool preservative, - int channel_options = channel_option::Default) + void transformLaserScanToPointCloud2(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 = channel_option::Default) { - return projectLaser_ (scan_in, cloud_out, range_cutoff, preservative, channel_options); + transformLaserScanToPointCloud2_(target_frame, scan_in, cloud_out, tf, co_sine_map_, range_cutoff, channel_options); } - //! Deprecated version of projectLaser + //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame /*! - * - The preservative argument has been removed. - * - The ordering of cloud_out and scan_in have been reversed + * 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 tf::Transformer object to use to perform the + * transform + * \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. */ - DEPRECATED - void transformLaserScanToPointCloud (const std::string& target_frame, - sensor_msgs::PointCloud & cloud_out, - const sensor_msgs::LaserScan& scan_in, - tf::Transformer & tf, - int channel_options = channel_option::Default, - bool preservative = false) + void transformLaserScanToPointCloud2(const std::string &target_frame, + const sensor_msgs::LaserScan &scan_in, + sensor_msgs::PointCloud2 &cloud_out, + tf::Transformer &tf, + int channel_options = channel_option::Default) { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, channel_options, preservative); + transformLaserScanToPointCloud2(target_frame, scan_in, cloud_out, tf, -1.0, channel_options); } - //! Deprecated version of getUnitVectors - /*! - * - This is now assumed to be an internal, protected API. Do not call externally. - */ - DEPRECATED - const boost::numeric::ublas::matrix& getUnitVectors(float angle_min, - float angle_max, - float angle_increment, - unsigned int length) - { - return getUnitVectors_(angle_min, angle_max, angle_increment, length); - } protected: @@ -224,22 +292,38 @@ namespace laser_geometry bool preservative, int channel_options); + //! Internal hidden representation of projectLaser + void projectLaser_ (const sensor_msgs::LaserScan& scan_in, + sensor_msgs::PointCloud2 &cloud_out, + Eigen3::ArrayXXd &co_sine_map, + double range_cutoff, + int channel_options); + //! Internal hidden representation of transformLaserScanToPointCloud void transformLaserScanToPointCloud_ (const std::string& target_frame, sensor_msgs::PointCloud& cloud_out, const sensor_msgs::LaserScan& scan_in, tf::Transformer & tf, + double range_cutoff, int channel_options, bool preservative); + //! Internal hidden representation of transformLaserScanToPointCloud2 + void transformLaserScanToPointCloud2_ (const std::string &target_frame, + const sensor_msgs::LaserScan &scan_in, + sensor_msgs::PointCloud2 &cloud_out, + tf::Transformer &tf, + Eigen3::ArrayXXd &co_sine_map, + double range_cutoff, + int channel_options); + //! Internal map of pointers to stored values std::map* > unit_vector_map_; + Eigen3::ArrayXXd co_sine_map_; boost::mutex guv_mutex_; }; } -#undef DEPRECATED - #endif //LASER_SCAN_UTILS_LASERSCAN_H diff --git a/manifest.xml b/manifest.xml index 5d24946..f68cf03 100644 --- a/manifest.xml +++ b/manifest.xml @@ -15,6 +15,7 @@ for the skew resulting from moving robots or tilting laser scanners. + diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index ab60bb5..ae34eaf 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -37,10 +37,10 @@ namespace laser_geometry LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, bool preservative, int mask) { - boost::numeric::ublas::matrix ranges(2, scan_in.get_ranges_size()); + boost::numeric::ublas::matrix ranges(2, scan_in.ranges.size()); // Fill the ranges matrix - for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++) + for (unsigned int index = 0; index < scan_in.ranges.size(); index++) { ranges(0,index) = (double) scan_in.ranges[index]; ranges(1,index) = (double) scan_in.ranges[index]; @@ -49,53 +49,53 @@ namespace laser_geometry //Do the projection // NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment)); - boost::numeric::ublas::matrix output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.get_ranges_size())); + boost::numeric::ublas::matrix output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size())); //Stuff the output cloud cloud_out.header = scan_in.header; - cloud_out.set_points_size (scan_in.get_ranges_size ()); + cloud_out.points.resize (scan_in.ranges.size()); // Define 4 indices in the channel array for each possible value type int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1; - cloud_out.set_channels_size(0); + cloud_out.channels.resize(0); // Check if the intensity bit is set - if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0) + if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[0].name = "intensities"; - cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ()); + cloud_out.channels[0].values.resize (scan_in.intensities.size()); idx_intensity = 0; } // Check if the index bit is set if (mask & channel_option::Index) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size +1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size +1); cloud_out.channels[chan_size].name = "index"; - cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ()); + cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_index = chan_size; } // Check if the distance bit is set if (mask & channel_option::Distance) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "distances"; - cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ()); + cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_distance = chan_size; } if (mask & channel_option::Timestamp) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "stamps"; - cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ()); + cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_timestamp = chan_size; } @@ -105,7 +105,7 @@ namespace laser_geometry range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); unsigned int count = 0; - for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++) + for (unsigned int index = 0; index< scan_in.ranges.size(); index++) { if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative { @@ -129,7 +129,7 @@ namespace laser_geometry cloud_out.channels[idx_distance].values[count] = ranges (0, index); // Save intensities channel - if (scan_in.get_intensities_size() >= index) + if (scan_in.intensities.size() >= index) { /// \todo optimize and catch length difference better if (idx_intensity != -1) cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index]; @@ -145,9 +145,9 @@ namespace laser_geometry //downsize if necessary - cloud_out.set_points_size (count); - for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++) - cloud_out.channels[d].set_values_size(count); + cloud_out.points.resize (count); + for (unsigned int d = 0; d < cloud_out.channels.size(); d++) + cloud_out.channels[d].values.resize(count); }; const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length) @@ -189,54 +189,54 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, - tf::Transformer& tf, int mask, bool preservative) + tf::Transformer& tf, double range_cutoff, int mask, bool preservative) { cloud_out.header = scan_in.header; cloud_out.header.frame_id = target_frame; - cloud_out.set_points_size (scan_in.get_ranges_size()); + cloud_out.points.resize (scan_in.ranges.size()); // Define 4 indices in the channel array for each possible value type int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1; - cloud_out.set_channels_size(0); + cloud_out.channels.resize(0); // Check if the intensity bit is set - if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0) + if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[0].name = "intensities"; - cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ()); + cloud_out.channels[0].values.resize (scan_in.intensities.size()); idx_intensity = 0; } // Check if the index bit is set if (mask & channel_option::Index) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "index"; - cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ()); + cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_index = chan_size; } // Check if the distance bit is set if (mask & channel_option::Distance) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "distances"; - cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ()); + cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_distance = chan_size; } if (mask & channel_option::Timestamp) { - int chan_size = cloud_out.get_channels_size (); - cloud_out.set_channels_size (chan_size + 1); + int chan_size = cloud_out.channels.size(); + cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "stamps"; - cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ()); + cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_timestamp = chan_size; } @@ -252,7 +252,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do // 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.get_ranges_size()*scan_in.time_increment) ; + ros::Time end_time = scan_in.header.stamp + ros::Duration().fromSec(scan_in.ranges.size()*scan_in.time_increment) ; tf::StampedTransform start_transform ; tf::StampedTransform end_transform ; @@ -261,10 +261,15 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do 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) ; + if (range_cutoff < 0) + range_cutoff = scan_in.range_max; + else + range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); + unsigned int count = 0; - for (unsigned int i = 0; i < scan_in.get_ranges_size(); i++) + for (unsigned int i = 0; i < scan_in.ranges.size(); i++) { - if (preservative || (scan_in.ranges[i] < scan_in.range_max && scan_in.ranges[i] > scan_in.range_min)) //only when valid + if (preservative || (scan_in.ranges[i] < range_cutoff && scan_in.ranges[i] > scan_in.range_min)) //only when valid { // Looking up transforms in tree is too expensive. Need more optimized way /* @@ -275,7 +280,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do */ // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms - btScalar ratio = i / ( (double) scan_in.get_ranges_size() - 1.0) ; + btScalar ratio = i / ( (double) scan_in.ranges.size() - 1.0) ; //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector) @@ -309,7 +314,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do if (idx_distance != -1) cloud_out.channels[idx_distance].values[count] = scan_in.ranges[i]; - if (scan_in.get_intensities_size() >= i) + if (scan_in.intensities.size() >= i) { /// \todo optimize and catch length difference better if (idx_intensity != -1) cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[i]; @@ -323,9 +328,323 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do } //downsize if necessary - cloud_out.set_points_size (count); - for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++) - cloud_out.channels[d].set_values_size (count); + cloud_out.points.resize (count); + for (unsigned int d = 0; d < cloud_out.channels.size(); d++) + cloud_out.channels[d].values.resize (count); + } + + void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, + sensor_msgs::PointCloud2 &cloud_out, + Eigen3::ArrayXXd &co_sine_map, + double range_cutoff, + int channel_options) + { + size_t n_pts = scan_in.ranges.size (); + + Eigen3::ArrayXXd ranges (n_pts, 2), output (n_pts, 2); + + // Get the ranges into Eigen format + for (size_t i = 0; i < n_pts; ++i) + { + ranges (i, 0) = (double) scan_in.ranges[i]; + ranges (i, 1) = (double) scan_in.ranges[i]; + } + + // Check if we have a precomputed co_sine_map + if (co_sine_map.rows () != (int)n_pts) + { + ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one."); + co_sine_map = Eigen3::ArrayXXd (n_pts, 2); + // Spherical->Cartesian projection + for (size_t i = 0; i < n_pts; ++i) + { + co_sine_map (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment); + co_sine_map (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment); + } + } + + output = ranges * co_sine_map; + + // Set the output cloud accordingly + cloud_out.header = scan_in.header; + cloud_out.height = 1; + cloud_out.width = scan_in.ranges.size (); + cloud_out.fields.resize (3); + cloud_out.fields[0].name = "x"; + cloud_out.fields[1].name = "y"; + cloud_out.fields[2].name = "z"; + + // Define 4 indices in the channel array for each possible value type + int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1; + + //now, we need to check what fields we need to store + int offset = 0; + if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) + { + int field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "intensity"; + cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + offset += 4; + idx_intensity = field_size; + } + + if ((channel_options & channel_option::Index)) + { + int field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "index"; + cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32; + cloud_out.fields[field_size].offset = offset; + offset += 4; + idx_index = field_size; + } + + if ((channel_options & channel_option::Distance)) + { + int field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "distances"; + cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + offset += 4; + idx_distance = field_size; + } + + if ((channel_options & channel_option::Timestamp)) + { + int field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 1); + cloud_out.fields[field_size].name = "stamps"; + cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + offset += 4; + idx_timestamp = field_size; + } + + if ((channel_options & channel_option::Viewpoint)) + { + int field_size = cloud_out.fields.size(); + cloud_out.fields.resize(field_size + 3); + + cloud_out.fields[field_size].name = "vp_x"; + cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].offset = offset; + offset += 4; + + cloud_out.fields[field_size + 1].name = "vp_y"; + cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size + 1].offset = offset; + offset += 4; + + cloud_out.fields[field_size + 2].name = "vp_z"; + cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size + 2].offset = offset; + offset += 4; + + idx_vpx = field_size; + idx_vpy = field_size + 1; + idx_vpz = field_size + 2; + } + + cloud_out.point_step = offset; + cloud_out.row_step = cloud_out.point_step * cloud_out.width; + cloud_out.data.resize (cloud_out.row_step * cloud_out.height); + cloud_out.is_dense = true; + + //TODO: Find out why this was needed + //float bad_point = std::numeric_limits::quiet_NaN (); + + unsigned int count = 0; + for (size_t i = 0; i < n_pts; ++i) + { + if (range_cutoff < 0) + range_cutoff = scan_in.range_max; + else + range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); + + //check to see if we want to keep the point + if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) + { + + float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step]; + + // Copy XYZ + pstep[0] = output (i, 0); + pstep[1] = output (i, 1); + pstep[2] = 0; + + // Copy intensity + if(idx_intensity != -1) + pstep[idx_intensity] = scan_in.intensities[i]; + + //Copy index + if(idx_index != -1) + pstep[idx_index] = i; + + // Copy distance + if(idx_distance != -1) + pstep[idx_distance] = scan_in.ranges[i]; + + // Copy timestamp + if(idx_timestamp != -1) + pstep[idx_timestamp] = i * scan_in.time_increment; + + // Copy viewpoint (0, 0, 0) + if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) + { + pstep[idx_vpx] = 0; + pstep[idx_vpy] = 0; + pstep[idx_vpz] = 0; + } + + //make sure to increment count + ++count; + } + + /* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values + * why can't you just leave them out? + * + // Invalid measurement? + if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) + { + if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE) + { + for (size_t s = 0; s < cloud_out.fields.size (); ++s) + pstep[s] = bad_point; + } + else + { + // Kind of nasty thing: + // We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid. + // Since we still might need the x value we store it in the distance field + pstep[0] = bad_point; // X -> NAN to mark a bad point + pstep[1] = co_sine_map (i, 1); // Y + pstep[2] = 0; // Z + + if (store_intensity) + { + pstep[3] = bad_point; // Intensity -> NAN to mark a bad point + pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X + } + else + pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X + } + } + */ + } + + //resize if necessary + cloud_out.width = count; + cloud_out.row_step = cloud_out.point_step * cloud_out.width; + cloud_out.data.resize (cloud_out.row_step * cloud_out.height); + } + + void LaserProjection::transformLaserScanToPointCloud2_ (const std::string &target_frame, + const sensor_msgs::LaserScan &scan_in, + sensor_msgs::PointCloud2 &cloud_out, + tf::Transformer &tf, + Eigen3::ArrayXXd &co_sine_map, + double range_cutoff, + int channel_options) + { + //we'll enforce that we get index values for the laser scan so that we + //ensure that we use the correct timestamps + channel_options |= channel_option::Index; + + projectLaser_(scan_in, cloud_out, co_sine_map, -1.0, channel_options); + + //we'll assume no associated viewpoint by default + bool has_viewpoint = false; + uint32_t vp_x_offset = 0; + + //we need to find the offset of the intensity field in the point cloud + //we also know that the index field is guaranteed to exist since we + //set the channel option above. To be really safe, it might be worth + //putting in a check at some point, but I'm just going to put in an + //assert for now + uint32_t index_offset = 0; + for(unsigned int i = 0; i < cloud_out.fields.size(); ++i) + { + if(cloud_out.fields[i].name == "index") + { + index_offset = cloud_out.fields[i].offset; + } + + //we want to check if the cloud has a viewpoint associated with it + //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all + //get put in together + if(cloud_out.fields[i].name == "vp_x") + { + has_viewpoint = true; + vp_x_offset = cloud_out.fields[i].offset; + } + } + + ROS_ASSERT(index_offset > 0); + + 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 () * 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); + + //we want to loop through all the points in the cloud + for(size_t i = 0; i < cloud_out.width; ++i) + { + // Apply the transform to the current point + float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0]; + + //find the index of the point + uint32_t pt_index; + memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); + + // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms + btScalar ratio = pt_index * ranges_norm; + + //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector) + // Interpolate translation + btVector3 v (0, 0, 0); + v.setInterpolate3 (start_transform.getOrigin (), end_transform.getOrigin (), ratio); + cur_transform.setOrigin (v); + + // Interpolate rotation + btQuaternion q1, q2; + start_transform.getBasis ().getRotation (q1); + end_transform.getBasis ().getRotation (q2); + + // Compute the slerp-ed rotation + cur_transform.setRotation (slerp (q1, q2 , ratio)); + + btVector3 point_in (pstep[0], pstep[1], pstep[2]); + btVector3 point_out = cur_transform * point_in; + + // Copy transformed point into cloud + pstep[0] = point_out.x (); + pstep[1] = point_out.y (); + pstep[2] = point_out.z (); + + // Convert the viewpoint as well + if(has_viewpoint) + { + float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset]; + point_in = btVector3 (vpstep[0], vpstep[1], vpstep[2]); + point_out = cur_transform * point_in; + + // Copy transformed point into cloud + vpstep[0] = point_out.x (); + vpstep[1] = point_out.y (); + vpstep[2] = point_out.z (); + } + } } diff --git a/src/laser_scan_geometry.cpp b/src/laser_scan_geometry.cpp index 1af8d00..38331ca 100644 --- a/src/laser_scan_geometry.cpp +++ b/src/laser_scan_geometry.cpp @@ -255,14 +255,22 @@ void } void - laser_scan_geometry::LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out) + laser_scan_geometry::LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, + sensor_msgs::PointCloud2 &cloud_out, + double range_cutoff = -1.0, + int channel_options = channel_option::Default) { boost::mutex::scoped_lock guv_lock (guv_mutex_); laser_scan_geometry::projectLaser (scan_in, cloud_out, co_sine_map_); } void - laser_scan_geometry::LaserProjection::transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, tf::Transformer &tf, sensor_msgs::PointCloud2 &cloud_out) + laser_scan_geometry::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 = channel_option::Default) { laser_scan_geometry::transformLaserScanToPointCloud (target_frame, scan_in, tf, cloud_out, co_sine_map_); }