From 433181e63efe9f130d03b84db935155a7283ba52 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 14 Mar 2018 13:36:50 +0100 Subject: [PATCH] Uncrustify --- include/laser_geometry/laser_geometry.hpp | 265 ++++---- src/laser_geometry.cpp | 774 +++++++++++----------- test/projection_test.cpp | 380 ++++++----- 3 files changed, 730 insertions(+), 689 deletions(-) diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp index eac640c..acf26cf 100644 --- a/include/laser_geometry/laser_geometry.hpp +++ b/include/laser_geometry/laser_geometry.hpp @@ -49,148 +49,153 @@ 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; +// 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. +namespace channel_option +{ +//! Enumerated output channels options. +/*! + * An OR'd set of these options is passed as the final argument of + * the projectLaser and transformLaserScanToPointCloud calls to + * enable generation of the appropriate set of additional channels. + */ +enum ChannelOption +{ + None = 0x00, //!< Enable no channels + Intensity = 0x01, //!< Enable "intensities" channel + Index = 0x02, //!< Enable "index" channel + Distance = 0x04, //!< Enable "distances" channel + Timestamp = 0x08, //!< Enable "stamps" channel + Viewpoint = 0x10, //!< Enable "viewpoint" channel + Default = (Intensity | Index) //!< Enable "intensities" and "index" channels +}; +} + +//! \brief A Class to Project Laser Scan +/*! + * This class will project laser scans into point clouds. It caches + * unit vectors between runs (provided the angular resolution of + * your scanner is not changing) to avoid excess computation. + * + * By default all range values less than the scanner min_range, and + * greater than the scanner max_range are removed from the generated + * point cloud, as these are assumed to be invalid. + * + * If it is important to preserve a mapping between the index of + * range values and points in the cloud, the recommended approach is + * to pre-filter your laser_scan message to meet the requiremnt that all + * ranges are between min and max_range. + * + * The generated PointClouds have a number of channels which can be enabled + * through the use of ChannelOption. + * - channel_option::Intensity - Create a channel named "intensities" with the intensity of the return for each point + * - channel_option::Index - Create a channel named "index" containing the index from the original array for each point + * - channel_option::Distance - Create a channel named "distances" containing the distance from the laser to each point + * - channel_option::Timestamp - Create a channel named "stamps" containing the specific timestamp at which each point was measured + */ +class LaserProjection +{ + +public: + LaserProjection() + : angle_min_(0), angle_max_(0) {} + + //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 /*! - * An OR'd set of these options is passed as the final argument of - * the projectLaser and transformLaserScanToPointCloud calls to - * enable generation of the appropriate set of additional channels. + * 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 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. */ - enum ChannelOption - { - None = 0x00, //!< Enable no channels - Intensity = 0x01, //!< Enable "intensities" channel - Index = 0x02, //!< Enable "index" channel - Distance = 0x04, //!< Enable "distances" channel - Timestamp = 0x08, //!< Enable "stamps" channel - Viewpoint = 0x10, //!< Enable "viewpoint" channel - Default = (Intensity | Index) //!< Enable "intensities" and "index" channels - }; + void projectLaser( + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + double range_cutoff = -1.0, + int channel_options = channel_option::Default) + { + projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); } - //! \brief A Class to Project Laser Scan + //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame /*! - * This class will project laser scans into point clouds. It caches - * unit vectors between runs (provided the angular resolution of - * your scanner is not changing) to avoid excess computation. + * 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. * - * By default all range values less than the scanner min_range, and - * greater than the scanner max_range are removed from the generated - * point cloud, as these are assumed to be invalid. - * - * If it is important to preserve a mapping between the index of - * range values and points in the cloud, the recommended approach is - * to pre-filter your laser_scan message to meet the requiremnt that all - * ranges are between min and max_range. - * - * The generated PointClouds have a number of channels which can be enabled - * through the use of ChannelOption. - * - channel_option::Intensity - Create a channel named "intensities" with the intensity of the return for each point - * - channel_option::Index - Create a channel named "index" containing the index from the original array for each point - * - channel_option::Distance - Create a channel named "distances" containing the distance from the laser to each point - * - channel_option::Timestamp - Create a channel named "stamps" containing the specific timestamp at which each point was measured + * \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. */ - class LaserProjection - { + void transformLaserScanToPointCloud( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::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); + } - public: +private: + //! Internal hidden representation of projectLaser + void projectLaser_( + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + double range_cutoff, + int channel_options); - LaserProjection() : angle_min_(0), angle_max_(0) {} + //! Internal hidden representation of transformLaserScanToPointCloud2 + void transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::BufferCore & tf, + double range_cutoff, + int channel_options); - //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::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 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 projectLaser (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - double range_cutoff = -1.0, - int channel_options = channel_option::Default) - { - projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); - } + //! Function used by the several forms of transformLaserScanToPointCloud_ + void transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::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); - //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::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::msg::LaserScan &scan_in, - sensor_msgs::msg::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); - } - - private: - - //! Internal hidden representation of projectLaser - void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - double range_cutoff, - int channel_options); - - //! Internal hidden representation of transformLaserScanToPointCloud2 - void transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::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::msg::LaserScan &scan_in, - sensor_msgs::msg::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 - float angle_min_; - float angle_max_; - Eigen::ArrayXXd co_sine_map_; - }; + //! Internal map of pointers to stored values + float angle_min_; + float angle_max_; + Eigen::ArrayXXd co_sine_map_; +}; } diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index e682e6b..e18b1b7 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -42,424 +42,418 @@ typedef double tfScalar; namespace laser_geometry { - void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - double range_cutoff, - int channel_options) - { - size_t n_pts = scan_in.ranges.size (); - Eigen::ArrayXXd ranges (n_pts, 2); - Eigen::ArrayXXd output (n_pts, 2); +void LaserProjection::projectLaser_( + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + double range_cutoff, + int channel_options) +{ + size_t n_pts = scan_in.ranges.size(); + Eigen::ArrayXXd ranges(n_pts, 2); + Eigen::ArrayXXd 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 our existing co_sine_map is valid - if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max ) - { - ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one."); - co_sine_map_ = Eigen::ArrayXXd (n_pts, 2); - angle_min_ = scan_in.angle_min; - angle_max_ = scan_in.angle_max; - // 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[0].offset = 0; - cloud_out.fields[0].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[0].count = 1; - cloud_out.fields[1].name = "y"; - cloud_out.fields[1].offset = 4; - cloud_out.fields[1].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[1].count = 1; - cloud_out.fields[2].name = "z"; - cloud_out.fields[2].offset = 8; - cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[2].count = 1; - - // 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 = 12; - 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 = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - 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 = POINT_FIELD::INT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - 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 = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - 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 = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - 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 = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size].offset = offset; - cloud_out.fields[field_size].count = 1; - offset += 4; - - cloud_out.fields[field_size + 1].name = "vp_y"; - cloud_out.fields[field_size + 1].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size + 1].offset = offset; - cloud_out.fields[field_size + 1].count = 1; - offset += 4; - - cloud_out.fields[field_size + 2].name = "vp_z"; - cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32; - cloud_out.fields[field_size + 2].offset = offset; - cloud_out.fields[field_size + 2].count = 1; - 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 = false; - - if (range_cutoff < 0) - range_cutoff = scan_in.range_max; - - unsigned int count = 0; - for (size_t i = 0; i < n_pts; ++i) - { - //check to see if we want to keep the point - const float range = scan_in.ranges[i]; - if (range < range_cutoff && range >= 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) - ((int*)(pstep))[idx_index] = i; - - // Copy distance - if(idx_distance != -1) - pstep[idx_distance] = range; - - // 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); + // 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]; } - void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::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) + // Check if our existing co_sine_map is valid + if (co_sine_map_.rows() != (int)n_pts || angle_min_ != scan_in.angle_min || + angle_max_ != scan_in.angle_max) { - //check if the user has requested the index field - bool requested_index = false; - if ((channel_options & channel_option::Index)) - requested_index = true; + ROS_DEBUG("[projectLaser] No precomputed map given. Computing one."); + co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); + angle_min_ = scan_in.angle_min; + angle_max_ = scan_in.angle_max; + // 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); + } + } - //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; + output = ranges * co_sine_map_; - projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); + // 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[0].offset = 0; + cloud_out.fields[0].datatype = POINT_FIELD::FLOAT32; + cloud_out.fields[0].count = 1; + cloud_out.fields[1].name = "y"; + cloud_out.fields[1].offset = 4; + cloud_out.fields[1].datatype = POINT_FIELD::FLOAT32; + cloud_out.fields[1].count = 1; + cloud_out.fields[2].name = "z"; + cloud_out.fields[2].offset = 8; + cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32; + cloud_out.fields[2].count = 1; - //we'll assume no associated viewpoint by default - bool has_viewpoint = false; - uint32_t vp_x_offset = 0; + // 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; - //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; + //now, we need to check what fields we need to store + int offset = 12; + 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 = POINT_FIELD::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + 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 = POINT_FIELD::INT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + 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 = POINT_FIELD::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + 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 = POINT_FIELD::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + 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 = POINT_FIELD::FLOAT32; + cloud_out.fields[field_size].offset = offset; + cloud_out.fields[field_size].count = 1; + offset += 4; + + cloud_out.fields[field_size + 1].name = "vp_y"; + cloud_out.fields[field_size + 1].datatype = POINT_FIELD::FLOAT32; + cloud_out.fields[field_size + 1].offset = offset; + cloud_out.fields[field_size + 1].count = 1; + offset += 4; + + cloud_out.fields[field_size + 2].name = "vp_z"; + cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32; + cloud_out.fields[field_size + 2].offset = offset; + cloud_out.fields[field_size + 2].count = 1; + 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 = false; + + if (range_cutoff < 0) { + range_cutoff = scan_in.range_max; + } + + unsigned int count = 0; + for (size_t i = 0; i < n_pts; ++i) { + //check to see if we want to keep the point + const float range = scan_in.ranges[i]; + if (range < range_cutoff && range >= 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]; } - //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; + //Copy index + if (idx_index != -1) { + ((int *)(pstep))[idx_index] = i; } + + // Copy distance + if (idx_distance != -1) { + pstep[idx_distance] = range; + } + + // 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; } - ROS_ASSERT(index_offset > 0); - - cloud_out.header.frame_id = target_frame; - - tf2::Transform cur_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) + /* 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) { - // Apply the transform to the current point - float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0]; + 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 - //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)); + 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 + } + } + */ + } - // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms - tfScalar ratio = pt_index * ranges_norm; + //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); +} - //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector) - // Interpolate translation - tf2::Vector3 v (0, 0, 0); - v.setInterpolate3 (origin_start, origin_end, ratio); - cur_transform.setOrigin (v); +void LaserProjection::transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::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) +{ + //check if the user has requested the index field + bool requested_index = false; + if ((channel_options & channel_option::Index)) { + requested_index = true; + } - // Compute the slerp-ed rotation - cur_transform.setRotation (slerp (quat_start, quat_end , ratio)); + //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; - tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]); - tf2::Vector3 point_out = cur_transform * point_in; + projectLaser_(scan_in, cloud_out, range_cutoff, 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; + + tf2::Transform cur_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 + tfScalar 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 + tf2::Vector3 v(0, 0, 0); + v.setInterpolate3(origin_start, origin_end, ratio); + cur_transform.setOrigin(v); + + // Compute the slerp-ed rotation + cur_transform.setRotation(slerp(quat_start, quat_end, ratio)); + + 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(); + 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 = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]); + 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 = tf2::Vector3 (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 (); - } - } - - //if the user didn't request the index field, then we need to copy the PointCloud and drop it - if(!requested_index) - { - sensor_msgs::msg::PointCloud2 cloud_without_index; - - //copy basic meta data - cloud_without_index.header = cloud_out.header; - cloud_without_index.width = cloud_out.width; - cloud_without_index.height = cloud_out.height; - cloud_without_index.is_bigendian = cloud_out.is_bigendian; - cloud_without_index.is_dense = cloud_out.is_dense; - - //copy the fields - cloud_without_index.fields.resize(cloud_out.fields.size()); - unsigned int field_count = 0; - unsigned int offset_shift = 0; - for(unsigned int i = 0; i < cloud_out.fields.size(); ++i) - { - if(cloud_out.fields[i].name != "index") - { - cloud_without_index.fields[field_count] = cloud_out.fields[i]; - cloud_without_index.fields[field_count].offset -= offset_shift; - ++field_count; - } - else - { - //once we hit the index, we'll set the shift - offset_shift = 4; - } - } - - //resize the fields - cloud_without_index.fields.resize(field_count); - - //compute the size of the new data - cloud_without_index.point_step = cloud_out.point_step - offset_shift; - cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; - cloud_without_index.data.resize (cloud_without_index.row_step * cloud_without_index.height); - - uint32_t i = 0; - uint32_t j = 0; - //copy over the data from one cloud to the other - while (i < cloud_out.data.size()) - { - if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4)) - { - cloud_without_index.data[j++] = cloud_out.data[i]; - } - i++; - } - - //make sure to actually set the output - cloud_out = cloud_without_index; + vpstep[0] = point_out.x(); + vpstep[1] = point_out.y(); + vpstep[2] = point_out.z(); } } - void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::msg::LaserScan &scan_in, - sensor_msgs::msg::PointCloud2 &cloud_out, - tf2::BufferCore &tf, - double range_cutoff, - int channel_options) - { - TIME start_time = scan_in.header.stamp; - TIME end_time = scan_in.header.stamp; - // TODO: reconcile all the different time constructs - if (!scan_in.ranges.empty()) - { - end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0); + //if the user didn't request the index field, then we need to copy the PointCloud and drop it + if (!requested_index) { + sensor_msgs::msg::PointCloud2 cloud_without_index; + + //copy basic meta data + cloud_without_index.header = cloud_out.header; + cloud_without_index.width = cloud_out.width; + cloud_without_index.height = cloud_out.height; + cloud_without_index.is_bigendian = cloud_out.is_bigendian; + cloud_without_index.is_dense = cloud_out.is_dense; + + //copy the fields + cloud_without_index.fields.resize(cloud_out.fields.size()); + unsigned int field_count = 0; + unsigned int offset_shift = 0; + for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) { + if (cloud_out.fields[i].name != "index") { + cloud_without_index.fields[field_count] = cloud_out.fields[i]; + cloud_without_index.fields[field_count].offset -= offset_shift; + ++field_count; + } else { + //once we hit the index, we'll set the shift + offset_shift = 4; + } } - std::chrono::milliseconds start(start_time.nanoseconds()); - std::chrono::time_point st(start); - geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, st); - std::chrono::milliseconds end(end_time.nanoseconds()); - std::chrono::time_point e(end); - geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, e); + //resize the fields + cloud_without_index.fields.resize(field_count); - 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); + //compute the size of the new data + cloud_without_index.point_step = cloud_out.point_step - offset_shift; + cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; + cloud_without_index.data.resize(cloud_without_index.row_step * cloud_without_index.height); - 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); + uint32_t i = 0; + uint32_t j = 0; + //copy over the data from one cloud to the other + while (i < cloud_out.data.size()) { + if ((i % cloud_out.point_step) < index_offset || + (i % cloud_out.point_step) >= (index_offset + 4)) + { + cloud_without_index.data[j++] = cloud_out.data[i]; + } + i++; + } + + //make sure to actually set the output + cloud_out = cloud_without_index; } +} + +void LaserProjection::transformLaserScanToPointCloud_( + const std::string & target_frame, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud2 & cloud_out, + tf2::BufferCore & tf, + double range_cutoff, + int channel_options) +{ + TIME start_time = scan_in.header.stamp; + TIME end_time = scan_in.header.stamp; + // TODO: reconcile all the different time constructs + if (!scan_in.ranges.empty()) { + end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0); + } + + std::chrono::milliseconds start(start_time.nanoseconds()); + std::chrono::time_point st(start); + geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(target_frame, + scan_in.header.frame_id, + st); + std::chrono::milliseconds end(end_time.nanoseconds()); + std::chrono::time_point e(end); + geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(target_frame, + scan_in.header.frame_id, + e); + + 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 ab7aedd..b221ff6 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright @@ -13,7 +13,7 @@ * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -37,12 +37,15 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #define PROJECTION_TEST_RANGE_MIN (0.23) -#define PROJECTION_TEST_RANGE_MAX (40.0) +#define PROJECTION_TEST_RANGE_MAX (40.0) -class BuildScanException { }; +class BuildScanException +{ +}; -struct ScanOptions { +struct ScanOptions +{ double range_; double intensity_; double ang_min_; @@ -50,10 +53,11 @@ struct ScanOptions { double ang_increment_; rclcpp::Duration scan_time_; - ScanOptions(double range, double intensity, + ScanOptions( + double range, double intensity, double ang_min, double ang_max, double ang_increment, - rclcpp::Duration scan_time) : - range_(range), + rclcpp::Duration scan_time) + : range_(range), intensity_(intensity), ang_min_(ang_min), ang_max_(ang_max), @@ -63,8 +67,9 @@ struct ScanOptions { sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options) { - if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) + if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) { throw (BuildScanException()); + } sensor_msgs::msg::LaserScan scan; scan.header.stamp = rclcpp::Clock().now(); @@ -76,21 +81,19 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options) scan.range_min = PROJECTION_TEST_RANGE_MIN; scan.range_max = PROJECTION_TEST_RANGE_MAX; uint32_t i = 0; - for(; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++) - { + for (; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++) { scan.ranges.push_back(options.range_); scan.intensities.push_back(options.intensity_); } - scan.time_increment = options.scan_time_.nanoseconds()/(double)i; + scan.time_increment = options.scan_time_.nanoseconds() / (double)i; return scan; -}; +} -TEST(laser_geometry, projectLaser2) -{ +TEST(laser_geometry, projectLaser2) { double tolerance = 1e-12; - laser_geometry::LaserProjection projector; + laser_geometry::LaserProjection projector; std::vector ranges, intensities, min_angles, max_angles, angle_increments; std::vector increment_times, scan_times; @@ -110,34 +113,34 @@ TEST(laser_geometry, projectLaser2) intensities.push_back(5.0); min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); + min_angles.push_back(-M_PI / 1.5); + min_angles.push_back(-M_PI / 2); + min_angles.push_back(-M_PI / 4); + min_angles.push_back(-M_PI / 8); max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); + max_angles.push_back(M_PI / 1.5); + max_angles.push_back(M_PI / 2); + max_angles.push_back(M_PI / 4); + max_angles.push_back(M_PI / 8); // angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree + angle_increments.push_back(M_PI / 180); // one degree + angle_increments.push_back(M_PI / 360); // half degree + angle_increments.push_back(M_PI / 720); // quarter degree - scan_times.push_back(rclcpp::Duration(1/40)); - scan_times.push_back(rclcpp::Duration(1/20)); + scan_times.push_back(rclcpp::Duration(1 / 40)); + scan_times.push_back(rclcpp::Duration(1 / 20)); std::vector options; - for(auto range : ranges) { - for(auto intensity : intensities) { - for(auto min_angle : min_angles) { - for(auto max_angle : max_angles) { - for(auto angle_increment : angle_increments) { - for(auto scan_time : scan_times) { + for (auto range : ranges) { + for (auto intensity : intensities) { + for (auto min_angle : min_angles) { + for (auto max_angle : max_angles) { + for (auto angle_increment : angle_increments) { + for (auto scan_time : scan_times) { options.push_back(ScanOptions( - range, intensity, min_angle, max_angle, angle_increment, scan_time)); + range, intensity, min_angle, max_angle, angle_increment, scan_time)); } } } @@ -145,66 +148,86 @@ TEST(laser_geometry, projectLaser2) } } - for (auto option : options){ - try - { - // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::msg::LaserScan scan = build_constant_scan(option); + for (auto option : options) { + try { + // printf("%f %f %f %f %f %f\n", + // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); + sensor_msgs::msg::LaserScan scan = build_constant_scan(option); - sensor_msgs::msg::PointCloud2 cloud_out; - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + sensor_msgs::msg::PointCloud2 cloud_out; + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); - projector.projectLaser(scan, cloud_out, -1.0); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + projector.projectLaser(scan, cloud_out, -1.0); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + projector.projectLaser(scan, cloud_out, -1.0, + laser_geometry::channel_option::Intensity | + laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); - projector.projectLaser(scan, cloud_out, -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.projectLaser(scan, cloud_out, -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.projectLaser(scan, cloud_out, -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.projectLaser(scan, cloud_out, -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); - unsigned int valid_points = 0; - for (unsigned int i = 0; i < scan.ranges.size(); i++) - if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; + unsigned int valid_points = 0; + for (unsigned int i = 0; i < scan.ranges.size(); i++) { + if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && + scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) + { + valid_points++; + } + } - EXPECT_EQ(valid_points, cloud_out.width); + EXPECT_EQ(valid_points, cloud_out.width); - uint32_t x_offset = 0; - uint32_t y_offset = 0; - uint32_t z_offset = 0; - uint32_t intensity_offset = 0; - uint32_t index_offset = 0; - uint32_t distance_offset = 0; - uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) { - if (f->name == "x") x_offset = f->offset; - if (f->name == "y") y_offset = f->offset; - if (f->name == "z") z_offset = f->offset; - if (f->name == "intensity") intensity_offset = f->offset; - if (f->name == "index") index_offset = f->offset; - if (f->name == "distances") distance_offset = f->offset; - if (f->name == "stamps") stamps_offset = f->offset; - } + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t z_offset = 0; + uint32_t intensity_offset = 0; + uint32_t index_offset = 0; + uint32_t distance_offset = 0; + uint32_t stamps_offset = 0; + for (std::vector::iterator f = cloud_out.fields.begin(); + f != cloud_out.fields.end(); f++) + { + if (f->name == "x") {x_offset = f->offset;} + if (f->name == "y") {y_offset = f->offset;} + if (f->name == "z") {z_offset = f->offset;} + if (f->name == "intensity") {intensity_offset = f->offset;} + if (f->name == "index") {index_offset = f->offset;} + if (f->name == "distances") {distance_offset = f->offset;} + if (f->name == "stamps") {stamps_offset = f->offset;} + } - for (unsigned int i = 0; i < cloud_out.width; i++) { + for (unsigned int i = 0; i < cloud_out.width; i++) { - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps - }; - } - catch (BuildScanException &ex) - { + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset], + (float)((double)(scan.ranges[i]) * + cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset], + (float)((double)(scan.ranges[i]) * + sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance); + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset], + scan.intensities[i], tolerance); // intensity + EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i, + tolerance); // index + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset], + scan.ranges[i], tolerance); // ranges + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset], + (float)i * scan.time_increment, tolerance); // timestamps + } + } catch (BuildScanException & ex) { // make sure it is not a false exception if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { FAIL(); @@ -213,13 +236,12 @@ TEST(laser_geometry, projectLaser2) } } -TEST(laser_geometry, transformLaserScanToPointCloud2) -{ +TEST(laser_geometry, transformLaserScanToPointCloud2) { tf2::BufferCore tf2; double tolerance = 1e-12; - laser_geometry::LaserProjection projector; + laser_geometry::LaserProjection projector; std::vector ranges, intensities, min_angles, max_angles, angle_increments; std::vector increment_times, scan_times; @@ -239,35 +261,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) intensities.push_back(5.0); min_angles.push_back(-M_PI); - min_angles.push_back(-M_PI/1.5); - min_angles.push_back(-M_PI/2); - min_angles.push_back(-M_PI/4); - min_angles.push_back(-M_PI/8); + min_angles.push_back(-M_PI / 1.5); + min_angles.push_back(-M_PI / 2); + min_angles.push_back(-M_PI / 4); + min_angles.push_back(-M_PI / 8); max_angles.push_back(M_PI); - max_angles.push_back(M_PI/1.5); - max_angles.push_back(M_PI/2); - max_angles.push_back(M_PI/4); - max_angles.push_back(M_PI/8); + max_angles.push_back(M_PI / 1.5); + max_angles.push_back(M_PI / 2); + max_angles.push_back(M_PI / 4); + max_angles.push_back(M_PI / 8); - angle_increments.push_back(-M_PI/180); // -one degree - angle_increments.push_back(M_PI/180); // one degree - angle_increments.push_back(M_PI/360); // half degree - angle_increments.push_back(M_PI/720); // quarter degree - - scan_times.push_back(rclcpp::Duration(1/40)); - scan_times.push_back(rclcpp::Duration(1/20)); + angle_increments.push_back(-M_PI / 180); // -one degree + angle_increments.push_back(M_PI / 180); // one degree + angle_increments.push_back(M_PI / 360); // half degree + angle_increments.push_back(M_PI / 720); // quarter degree + scan_times.push_back(rclcpp::Duration(1 / 40)); + scan_times.push_back(rclcpp::Duration(1 / 20)); std::vector options; - for(auto range : ranges) { - for(auto intensity : intensities) { - for(auto min_angle : min_angles) { - for(auto max_angle : max_angles) { - for(auto angle_increment : angle_increments) { - for(auto scan_time : scan_times) { + for (auto range : ranges) { + for (auto intensity : intensities) { + for (auto min_angle : min_angles) { + for (auto max_angle : max_angles) { + for (auto angle_increment : angle_increments) { + for (auto scan_time : scan_times) { options.push_back(ScanOptions( - range, intensity, min_angle, max_angle, angle_increment, scan_time)); + range, intensity, min_angle, max_angle, angle_increment, scan_time)); } } } @@ -275,73 +296,93 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) } } - for (auto option : options){ - try - { - // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); + for (auto option : options) { + try { + // printf("%f %f %f %f %f %f\n", + // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); sensor_msgs::msg::LaserScan scan = build_constant_scan(option); scan.header.frame_id = "laser_frame"; - sensor_msgs::msg::PointCloud2 cloud_out; - 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, 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, tf2, -1.0, laser_geometry::channel_option::Intensity); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + sensor_msgs::msg::PointCloud2 cloud_out; + 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, 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, 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, tf2); - 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, tf2); + 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, 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, 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, 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); + 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); - unsigned int valid_points = 0; - for (unsigned int i = 0; i < scan.ranges.size(); i++) - if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) - valid_points ++; - EXPECT_EQ(valid_points, cloud_out.width); + unsigned int valid_points = 0; + for (unsigned int i = 0; i < scan.ranges.size(); i++) { + if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && + scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) + { + valid_points++; + } + } + EXPECT_EQ(valid_points, cloud_out.width); - uint32_t x_offset = 0; - uint32_t y_offset = 0; - uint32_t z_offset = 0; - uint32_t intensity_offset = 0; - uint32_t index_offset = 0; - uint32_t distance_offset = 0; - uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) - { - if (f->name == "x") x_offset = f->offset; - if (f->name == "y") y_offset = f->offset; - if (f->name == "z") z_offset = f->offset; - if (f->name == "intensity") intensity_offset = f->offset; - if (f->name == "index") index_offset = f->offset; - if (f->name == "distances") distance_offset = f->offset; - if (f->name == "stamps") stamps_offset = f->offset; - } + uint32_t x_offset = 0; + uint32_t y_offset = 0; + uint32_t z_offset = 0; + uint32_t intensity_offset = 0; + uint32_t index_offset = 0; + uint32_t distance_offset = 0; + uint32_t stamps_offset = 0; + for (std::vector::iterator f = cloud_out.fields.begin(); + f != cloud_out.fields.end(); f++) + { + if (f->name == "x") {x_offset = f->offset;} + if (f->name == "y") {y_offset = f->offset;} + if (f->name == "z") {z_offset = f->offset;} + if (f->name == "intensity") {intensity_offset = f->offset;} + if (f->name == "index") {index_offset = f->offset;} + if (f->name == "distances") {distance_offset = f->offset;} + if (f->name == "stamps") {stamps_offset = f->offset;} + } - for (unsigned int i = 0; i < cloud_out.width; i++) - { - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance); - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order - EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges - EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps + for (unsigned int i = 0; i < cloud_out.width; i++) { + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset], + (float)((double)(scan.ranges[i]) * + cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset], + (float)((double)(scan.ranges[i]) * + sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance); + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset], + scan.intensities[i], tolerance); // intensity + EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i, + tolerance); // index + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset], + scan.ranges[i], tolerance); // ranges + EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset], + (float)i * scan.time_increment, tolerance); // timestamps - }; - } - catch (BuildScanException &ex) - { + } + } catch (BuildScanException & ex) { // make sure it is not a false exception if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { FAIL(); @@ -351,7 +392,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }