pax_global_header00006660000000000000000000000064125602110270014506gustar00rootroot0000000000000052 comment=5fe8669f398c262167584750ede1fc730bd4837f ros-laser-geometry-1.6.4/000077500000000000000000000000001256021102700152565ustar00rootroot00000000000000ros-laser-geometry-1.6.4/.gitignore000066400000000000000000000000111256021102700172360ustar00rootroot00000000000000*~ *.pyc ros-laser-geometry-1.6.4/CHANGELOG.rst000066400000000000000000000044311256021102700173010ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package laser_geometry ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.6.4 (2015-05-18) ------------------ * Fix segfault when laserscan ranges[] is empty * Contributors: Timm Linder, Vincent Rabaud 1.6.3 (2015-03-07) ------------------ * provide support for tf2 * Contributors: Vincent Rabaud 1.6.2 (2014-06-08) ------------------ * adds python port (only simple projection) * allows to have range_cutoff > range_max NOTE this is required if we need to keep the range_max readings in the point cloud. An example application is an obstacle_layer in a costmap. * Contributors: Vincent Rabaud, enriquefernandez 1.6.1 (2014-02-23) ------------------ * Added dependency on cmake_modules * Contributors: William Woodall 1.6.0 (2014-02-21) ------------------ * Adding William Woodall as a co-maintainer * Contributors: Vincent Rabaud, William Woodall 1.5.15 (2013-12-02) ------------------- * Fix mistake in end_time calculation for scan transformation in #6 1.5.14 (2013-11-04) ------------------- * Treat max_range as invalid measurement * Properly propagate range_cutoff * check for CATKIN_ENABLE_TESTING 1.5.13 (2013-10-06) ------------------- * fixes `#3 `_ 1.5.12 (2013-09-14) ------------------- * fix case of Eigen find_package name 1.5.11 (2013-07-01) ------------------- * added missing run deps 1.5.10 (2013-06-28 15:09) ------------------------- * [bugfix] export boost and eigen via DEPENDS 1.5.9 (2013-06-28 11:38) ------------------------ * [bugfix] export boost and eigen include dirs 1.5.8 (2012-12-14 13:54) ------------------------ * Added buildtool_depend on catkin 1.5.7 (2012-12-14 13:48) ------------------------ * CMake clean up 1.5.6 (2012-12-10) ------------------ * Removed vestigial manifest.xml 1.5.5 (2012-11-15) ------------------ * Added .count field (of 1) to every PointCloud2 field description. This fixes the bug referred to here: http://dev.pointclouds.org/issues/821 which is useful because that fix in PCL seems not to be released yet. Also this way is more correct, as far as I can tell. * Tidied up CMakeLists.txt based on Dirk's recommendations. 1.5.4 (2012-10-10) ------------------ * added install rules to CMakeLists.txt needed for catkinization. * catkinized ros-laser-geometry-1.6.4/CMakeLists.txt000066400000000000000000000021261256021102700200170ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(laser_geometry) find_package(catkin REQUIRED COMPONENTS angles cmake_modules roscpp sensor_msgs tf tf2 ) find_package(Boost REQUIRED COMPONENTS system thread) find_package(Eigen REQUIRED) catkin_python_setup() catkin_package( INCLUDE_DIRS include LIBRARIES laser_geometry DEPENDS Boost Eigen ) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ) add_library(laser_geometry src/laser_geometry.cpp) target_link_libraries(laser_geometry ${Boost_LIBRARIES} ${tf_LIBRARIES}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(projection_test test/projection_test.cpp) target_link_libraries(projection_test laser_geometry) catkin_add_nosetests(test/projection_test.py) endif() install(TARGETS laser_geometry ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/laser_geometry/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") ros-laser-geometry-1.6.4/include/000077500000000000000000000000001256021102700167015ustar00rootroot00000000000000ros-laser-geometry-1.6.4/include/laser_geometry/000077500000000000000000000000001256021102700217225ustar00rootroot00000000000000ros-laser-geometry-1.6.4/include/laser_geometry/laser_geometry.h000066400000000000000000000420111256021102700251120ustar00rootroot00000000000000/* * 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 * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * 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 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef LASER_SCAN_UTILS_LASERSCAN_H #define LASER_SCAN_UTILS_LASERSCAN_H #include #include #include #include "boost/numeric/ublas/matrix.hpp" #include "boost/thread/mutex.hpp" #include #include #include "sensor_msgs/LaserScan.h" #include "sensor_msgs/PointCloud.h" #include "sensor_msgs/PointCloud.h" #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. /*! * 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) {} //! Destructor to deallocate stored unit vectors ~LaserProjection(); //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud /*! * 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::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out, double range_cutoff = -1.0, int channel_options = channel_option::Default) { 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 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::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff = -1.0, int channel_options = channel_option::Default) { projectLaser_(scan_in, cloud_out, 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 to discard everything above it. * \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); } //! 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 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, int channel_options = channel_option::Default) { return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options); } //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame /*! * Transform a single laser scan from a linear array into a 3D * point cloud, accounting for movement of the laser over the * course of the scan. In order for this transform to be * meaningful at a single point in time, the target_frame must * be a fixed reference frame. See the tf documentation for * more information on fixed frames. * * \param target_frame The frame of the resulting point cloud * \param scan_in The input laser scan * \param cloud_out The output point cloud * \param tf a tf::Transformer object to use to perform the * transform * \param range_cutoff An additional range cutoff which can be * applied to discard everything above it. * Defaults to -1.0, which means the laser scan max range. * \param channel_option An OR'd set of channels to include. * Options include: channel_option::Default, * channel_option::Intensity, channel_option::Index, * channel_option::Distance, channel_option::Timestamp. */ void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff = -1.0, int channel_options = channel_option::Default) { transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); } //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame /*! * Transform a single laser scan from a linear array into a 3D * point cloud, accounting for movement of the laser over the * course of the scan. In order for this transform to be * meaningful at a single point in time, the target_frame must * be a fixed reference frame. See the tf documentation for * more information on fixed frames. * * \param target_frame The frame of the resulting point cloud * \param scan_in The input laser scan * \param cloud_out The output point cloud * \param tf a tf2::BufferCore object to use to perform the * transform * \param range_cutoff An additional range cutoff which can be * applied to discard everything above it. * Defaults to -1.0, which means the laser scan max range. * \param channel_option An OR'd set of channels to include. * Options include: channel_option::Default, * channel_option::Intensity, channel_option::Index, * channel_option::Distance, channel_option::Timestamp. */ void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf2::BufferCore &tf, double range_cutoff = -1.0, int channel_options = channel_option::Default) { transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); } protected: //! Internal protected representation of getUnitVectors /*! * This function should not be used by external users, however, * it is left protected so that test code can evaluate it * appropriately. */ const boost::numeric::ublas::matrix& getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length); private: //! Internal hidden representation of projectLaser void projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out, double range_cutoff, bool preservative, int channel_options); //! Internal hidden representation of projectLaser void projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, 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); //! Internal hidden representation of transformLaserScanToPointCloud2 void 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); //! Internal hidden representation of transformLaserScanToPointCloud2 void transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf2::BufferCore &tf, double range_cutoff, int channel_options); //! Function used by the several forms of transformLaserScanToPointCloud_ void transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf2::Quaternion quat_start, tf2::Vector3 origin_start, tf2::Quaternion quat_end, tf2::Vector3 origin_end, double range_cutoff, int channel_options); //! Internal map of pointers to stored values std::map* > unit_vector_map_; float angle_min_; float angle_max_; Eigen::ArrayXXd co_sine_map_; boost::mutex guv_mutex_; }; } #endif //LASER_SCAN_UTILS_LASERSCAN_H ros-laser-geometry-1.6.4/package.xml000066400000000000000000000025131256021102700173740ustar00rootroot00000000000000 laser_geometry 1.6.4 This package contains a class for converting from a 2D laser scan as defined by sensor_msgs/LaserScan into a point cloud as defined by sensor_msgs/PointCloud or sensor_msgs/PointCloud2. In particular, it contains functionality to account for the skew resulting from moving robots or tilting laser scanners. Dave Hershberger William Woodall BSD Tully Foote Radu Bogdan Rusu http://ros.org/wiki/laser_geometry catkin angles boost cmake_modules eigen roscpp sensor_msgs tf angles boost eigen python-numpy roscpp sensor_msgs tf rosunit ros-laser-geometry-1.6.4/setup.py000066400000000000000000000003371256021102700167730ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['laser_geometry'], package_dir={'': 'src'} ) setup(**d) ros-laser-geometry-1.6.4/src/000077500000000000000000000000001256021102700160455ustar00rootroot00000000000000ros-laser-geometry-1.6.4/src/laser_geometry.cpp000066400000000000000000000703421256021102700216000ustar00rootroot00000000000000/* * 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 * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * 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 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "laser_geometry/laser_geometry.h" #include #include #include namespace laser_geometry { void 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.ranges.size()); // Fill the ranges matrix 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]; } //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.ranges.size())); //Stuff the output cloud cloud_out.header = scan_in.header; 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.channels.resize(0); // Check if the intensity bit is set if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) { int chan_size = cloud_out.channels.size(); cloud_out.channels.resize (chan_size + 1); cloud_out.channels[0].name = "intensities"; 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.channels.size(); cloud_out.channels.resize (chan_size +1); cloud_out.channels[chan_size].name = "index"; 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.channels.size(); cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "distances"; 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.channels.size(); cloud_out.channels.resize (chan_size + 1); cloud_out.channels[chan_size].name = "stamps"; cloud_out.channels[chan_size].values.resize (scan_in.ranges.size()); idx_timestamp = chan_size; } if (range_cutoff < 0) range_cutoff = scan_in.range_max; unsigned int count = 0; for (unsigned int index = 0; index< scan_in.ranges.size(); index++) { const float range = ranges(0, index); if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min))) //if valid or preservative { cloud_out.points[count].x = output(0,index); cloud_out.points[count].y = output(1,index); cloud_out.points[count].z = 0.0; //double x = cloud_out.points[count].x; //double y = cloud_out.points[count].y; //if(x*x + y*y < scan_in.range_min * scan_in.range_min){ // ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y); //} // Save the original point index if (idx_index != -1) cloud_out.channels[idx_index].values[count] = index; // Save the original point distance if (idx_distance != -1) cloud_out.channels[idx_distance].values[count] = range; // Save intensities channel 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]; } // Save timestamps to seperate channel if asked for if( idx_timestamp != -1) cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment; count++; } } //downsize if necessary 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) { boost::mutex::scoped_lock guv_lock(this->guv_mutex_); //construct string for lookup in the map std::stringstream anglestring; anglestring <* >::iterator it; it = unit_vector_map_.find(anglestring.str()); //check the map for presense if (it != unit_vector_map_.end()) return *((*it).second); //if present return boost::numeric::ublas::matrix * tempPtr = new boost::numeric::ublas::matrix(2,length); for (unsigned int index = 0;index < length; index++) { (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment); (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment); } //store unit_vector_map_[anglestring.str()] = tempPtr; //and return return *tempPtr; }; LaserProjection::~LaserProjection() { std::map*>::iterator it; it = unit_vector_map_.begin(); while (it != unit_vector_map_.end()) { delete (*it).second; it++; } }; void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, tf::Transformer& tf, double range_cutoff, int mask) { cloud_out.header = scan_in.header; tf::Stamped pointIn; tf::Stamped pointOut; //check if the user has requested the index field bool requested_index = false; if ((mask & channel_option::Index)) requested_index = true; //we need to make sure that we include the index in our mask //in order to guarantee that we get our timestamps right mask |= channel_option::Index; pointIn.frame_id_ = scan_in.header.frame_id; projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask); 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 ; if(!scan_in.ranges.empty()) end_time += ros::Duration().fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment); tf::StampedTransform start_transform ; tf::StampedTransform end_transform ; tf::StampedTransform 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) ; //we need to find the index of the index channel int index_channel_idx = -1; for(unsigned int i = 0; i < cloud_out.channels.size(); ++i) { if(cloud_out.channels[i].name == "index") { index_channel_idx = i; break; } } //check just in case ROS_ASSERT(index_channel_idx >= 0); for(unsigned int i = 0; i < cloud_out.points.size(); ++i) { //get the index for this point uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i]; // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms tfScalar ratio = pt_index / ( (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) //Interpolate translation tf::Vector3 v (0, 0, 0); v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ; cur_transform.setOrigin(v) ; //Interpolate rotation tf::Quaternion q1, q2 ; start_transform.getBasis().getRotation(q1) ; end_transform.getBasis().getRotation(q2) ; // Compute the slerp-ed rotation cur_transform.setRotation( slerp( q1, q2 , ratio) ) ; // Apply the transform to the current point tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ; tf::Vector3 pointOut = cur_transform * pointIn ; // Copy transformed point into cloud cloud_out.points[i].x = pointOut.x(); cloud_out.points[i].y = pointOut.y(); cloud_out.points[i].z = pointOut.z(); } //if the user didn't request the index, we want to remove it from the channels if(!requested_index) cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx); } void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::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 = sensor_msgs::PointField::FLOAT32; cloud_out.fields[0].count = 1; cloud_out.fields[1].name = "y"; cloud_out.fields[1].offset = 4; cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[1].count = 1; cloud_out.fields[2].name = "z"; cloud_out.fields[2].offset = 8; cloud_out.fields[2].datatype = sensor_msgs::PointField::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 = sensor_msgs::PointField::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 = sensor_msgs::PointField::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 = sensor_msgs::PointField::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 = sensor_msgs::PointField::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 = sensor_msgs::PointField::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 = sensor_msgs::PointField::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 = sensor_msgs::PointField::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); } void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf2::Quaternion quat_start, tf2::Vector3 origin_start, tf2::Quaternion quat_end, tf2::Vector3 origin_end, double range_cutoff, int channel_options) { //check if the user has requested the index field bool requested_index = false; if ((channel_options & channel_option::Index)) requested_index = true; //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, 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 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::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; } } void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options) { ros::Time start_time = scan_in.header.stamp; ros::Time end_time = scan_in.header.stamp; if(!scan_in.ranges.empty()) end_time += ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment); tf::StampedTransform start_transform, end_transform ; tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform); tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform); tf::Quaternion q; start_transform.getBasis().getRotation(q); tf2::Quaternion quat_start(q.getX(), q.getY(), q.getZ(), q.getW()); end_transform.getBasis().getRotation(q); tf2::Quaternion quat_end(q.getX(), q.getY(), q.getZ(), q.getW()); tf2::Vector3 origin_start(start_transform.getOrigin().getX(), start_transform.getOrigin().getY(), start_transform.getOrigin().getZ()); tf2::Vector3 origin_end(end_transform.getOrigin().getX(), end_transform.getOrigin().getY(), end_transform.getOrigin().getZ()); transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, quat_start, origin_start, quat_end, origin_end, range_cutoff, channel_options); } void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf2::BufferCore &tf, double range_cutoff, int channel_options) { ros::Time start_time = scan_in.header.stamp; ros::Time end_time = scan_in.header.stamp; if(!scan_in.ranges.empty()) end_time += ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment); geometry_msgs::TransformStamped start_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time); geometry_msgs::TransformStamped end_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time); tf2::Quaternion quat_start(start_transform.transform.rotation.x, start_transform.transform.rotation.y, start_transform.transform.rotation.z, start_transform.transform.rotation.w); tf2::Quaternion quat_end(end_transform.transform.rotation.x, end_transform.transform.rotation.y, end_transform.transform.rotation.z, end_transform.transform.rotation.w); tf2::Vector3 origin_start(start_transform.transform.translation.x, start_transform.transform.translation.y, start_transform.transform.translation.z); tf2::Vector3 origin_end(end_transform.transform.translation.x, end_transform.transform.translation.y, end_transform.transform.translation.z); transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, quat_start, origin_start, quat_end, origin_end, range_cutoff, channel_options); } } //laser_geometry ros-laser-geometry-1.6.4/src/laser_geometry/000077500000000000000000000000001256021102700210665ustar00rootroot00000000000000ros-laser-geometry-1.6.4/src/laser_geometry/__init__.py000066400000000000000000000000541256021102700231760ustar00rootroot00000000000000from .laser_geometry import LaserProjection ros-laser-geometry-1.6.4/src/laser_geometry/laser_geometry.py000066400000000000000000000225071256021102700244670ustar00rootroot00000000000000""" Copyright (c) 2014, Enrique Fernandez 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 notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * 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 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. """ import rospy from sensor_msgs.msg import PointCloud2 import sensor_msgs.point_cloud2 as pc2 import numpy as np class LaserProjection: """ A class to Project Laser Scan This calls 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 thatn the scanner min_range, 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 requirement that all ranges are between min and max_range. The generate PointClouds have a number of channels which can be enabled through the use of ChannelOption. - ChannelOption.INTENSITY - Create a channel named "intensities" with the intensity of the return for each point. - ChannelOption.INDEX - Create a channel named "index" containing the index from the original array for each point. - ChannelOption.DISTANCE - Create a channel named "distance" containing the distance from the laser to each point. - ChannelOption.TIMESTAMP - Create a channel named "stamps" containing the specific timestamp at which each point was measured. """ LASER_SCAN_INVALID = -1.0 LASER_SCAN_MIN_RANGE = -2.0 LASER_SCAN_MAX_RANGE = -3.0 class 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) def __init__(self): self.__angle_min = 0.0 self.__angle_max = 0.0 self.__cos_sin_map = np.array([[]]) def projectLaser(self, scan_in, range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): """ 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. Keyword arguments: scan_in -- The input laser scan. range_cutoff -- An additional range cutoff which can be applied which is more limiting than max_range in the scan (default -1.0). channel_options -- An OR'd set of channels to include. """ return self.__projectLaser(scan_in, range_cutoff, channel_options) def __projectLaser(self, scan_in, range_cutoff, channel_options): N = len(scan_in.ranges) ranges = np.array(scan_in.ranges) ranges = np.array([ranges, ranges]) if (self.__cos_sin_map.shape[1] != N or self.__angle_min != scan_in.angle_min or self.__angle_max != scan_in.angle_max): rospy.logdebug("No precomputed map given. Computing one.") self.__angle_min = scan_in.angle_min self.__angle_max = scan_in.angle_max cos_map = [np.cos(scan_in.angle_min + i * scan_in.angle_increment) for i in range(N)] sin_map = [np.sin(scan_in.angle_min + i * scan_in.angle_increment) for i in range(N)] self.__cos_sin_map = np.array([cos_map, sin_map]) output = ranges * self.__cos_sin_map # Set the output cloud accordingly cloud_out = PointCloud2() fields = [pc2.PointField() for _ in range(3)] fields[0].name = "x" fields[0].offset = 0 fields[0].datatype = pc2.PointField.FLOAT32 fields[0].count = 1 fields[1].name = "y" fields[1].offset = 4 fields[1].datatype = pc2.PointField.FLOAT32 fields[1].count = 1 fields[2].name = "z" fields[2].offset = 8 fields[2].datatype = pc2.PointField.FLOAT32 fields[2].count = 1 idx_intensity = idx_index = idx_distance = idx_timestamp = -1 idx_vpx = idx_vpy = idx_vpz = -1 offset = 12 if (channel_options & self.ChannelOption.INTENSITY and len(scan_in.intensities) > 0): field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "intensity" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_intensity = field_size if channel_options & self.ChannelOption.INDEX: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "index" fields[field_size].datatype = pc2.PointField.INT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_index = field_size if channel_options & self.ChannelOption.DISTANCE: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "distances" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_distance = field_size if channel_options & self.ChannelOption.TIMESTAMP: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "stamps" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_timestamp = field_size if channel_options & self.ChannelOption.VIEWPOINT: field_size = len(fields) fields.extend([pc2.PointField() for _ in range(3)]) fields[field_size].name = "vp_x" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpx = field_size field_size += 1 fields[field_size].name = "vp_y" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpy = field_size field_size += 1 fields[field_size].name = "vp_z" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpz = field_size if range_cutoff < 0: range_cutoff = scan_in.range_max else: range_cutoff = min(range_cutoff, scan_in.range_max) points = [] for i in range(N): ri = scan_in.ranges[i] if ri < range_cutoff and ri >= scan_in.range_min: point = output[:, i].tolist() point.append(0) if idx_intensity != -1: point.append(scan_in.intensities[i]) if idx_index != -1: point.append(i) if idx_distance != -1: point.append(scan_in.ranges[i]) if idx_timestamp != -1: point.append(i * scan_in.time_increment) if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1: point.extend([0 for _ in range(3)]) points.append(point) cloud_out = pc2.create_cloud(scan_in.header, fields, points) return cloud_out ros-laser-geometry-1.6.4/test/000077500000000000000000000000001256021102700162355ustar00rootroot00000000000000ros-laser-geometry-1.6.4/test/projection_test.cpp000066400000000000000000000700461256021102700221630ustar00rootroot00000000000000/* * 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 * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * 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 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "laser_geometry/laser_geometry.h" #include "sensor_msgs/PointCloud.h" #include #include #include "rostest/permuter.h" #define PROJECTION_TEST_RANGE_MIN (0.23) #define PROJECTION_TEST_RANGE_MAX (40.0) class BuildScanException { }; sensor_msgs::LaserScan build_constant_scan(double range, double intensity, double ang_min, double ang_max, double ang_increment, ros::Duration scan_time) { if (((ang_max - ang_min) / ang_increment) < 0) throw (BuildScanException()); sensor_msgs::LaserScan scan; scan.header.stamp = ros::Time::now(); scan.header.frame_id = "laser_frame"; scan.angle_min = ang_min; scan.angle_max = ang_max; scan.angle_increment = ang_increment; scan.scan_time = scan_time.toSec(); scan.range_min = PROJECTION_TEST_RANGE_MIN; scan.range_max = PROJECTION_TEST_RANGE_MAX; uint32_t i = 0; for(; ang_min + i * ang_increment < ang_max; i++) { scan.ranges.push_back(range); scan.intensities.push_back(intensity); } scan.time_increment = scan_time.toSec()/(double)i; return scan; }; class TestProjection : public laser_geometry::LaserProjection { public: const boost::numeric::ublas::matrix& getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length) { return getUnitVectors_(angle_min, angle_max, angle_increment, length); } }; void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length) { double tolerance = 1e-12; TestProjection projector; const boost::numeric::ublas::matrix & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length); for (unsigned int i = 0; i < mat.size2(); i++) { EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)), angle_min + i * angle_increment), 0, tolerance); // check expected angle EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized } } #if 0 TEST(laser_geometry, getUnitVectors) { double min_angle = -M_PI/2; double max_angle = M_PI/2; double angle_increment = M_PI/180; std::vector min_angles, max_angles, angle_increments; rostest::Permuter permuter; 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); 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); permuter.addOptionSet(min_angles, &min_angle); 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); 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); permuter.addOptionSet(max_angles, &max_angle); 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 permuter.addOptionSet(angle_increments, &angle_increment); while (permuter.step()) { if ((max_angle - min_angle) / angle_increment > 0.0) { unsigned int length = round((max_angle - min_angle)/ angle_increment); try { test_getUnitVectors(min_angle, max_angle, angle_increment, length); test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment); test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1); } catch (BuildScanException &ex) { if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception FAIL(); } } //else //printf("%f\n", (max_angle - min_angle) / angle_increment); } } TEST(laser_geometry, projectLaser) { double tolerance = 1e-12; laser_geometry::LaserProjection projector; double min_angle = -M_PI/2; double max_angle = M_PI/2; double angle_increment = M_PI/180; double range = 1.0; double intensity = 1.0; ros::Duration scan_time = ros::Duration(1/40); ros::Duration increment_time = ros::Duration(1/400); std::vector ranges, intensities, min_angles, max_angles, angle_increments; std::vector increment_times, scan_times; rostest::Permuter permuter; ranges.push_back(-1.0); ranges.push_back(1.0); ranges.push_back(2.0); ranges.push_back(3.0); ranges.push_back(4.0); ranges.push_back(5.0); ranges.push_back(100.0); permuter.addOptionSet(ranges, &range); intensities.push_back(1.0); intensities.push_back(2.0); intensities.push_back(3.0); intensities.push_back(4.0); intensities.push_back(5.0); permuter.addOptionSet(intensities, &intensity); 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); permuter.addOptionSet(min_angles, &min_angle); 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); permuter.addOptionSet(max_angles, &max_angle); // 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 permuter.addOptionSet(angle_increments, &angle_increment); scan_times.push_back(ros::Duration(1/40)); scan_times.push_back(ros::Duration(1/20)); permuter.addOptionSet(scan_times, &scan_time); while (permuter.step()) { try { // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); sensor_msgs::PointCloud cloud_out; projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); projector.projectLaser(scan, cloud_out, -1.0); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); 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.channels.size(), (unsigned int)3); 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.channels.size(), (unsigned int)4); 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.points.size()); for (unsigned int i = 0; i < cloud_out.points.size(); i++) { EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance); EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps }; } catch (BuildScanException &ex) { if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception FAIL(); } } } #endif TEST(laser_geometry, projectLaser2) { double tolerance = 1e-12; laser_geometry::LaserProjection projector; double min_angle = -M_PI/2; double max_angle = M_PI/2; double angle_increment = M_PI/180; double range = 1.0; double intensity = 1.0; ros::Duration scan_time = ros::Duration(1/40); ros::Duration increment_time = ros::Duration(1/400); std::vector ranges, intensities, min_angles, max_angles, angle_increments; std::vector increment_times, scan_times; rostest::Permuter permuter; ranges.push_back(-1.0); ranges.push_back(1.0); ranges.push_back(2.0); ranges.push_back(3.0); ranges.push_back(4.0); ranges.push_back(5.0); ranges.push_back(100.0); permuter.addOptionSet(ranges, &range); intensities.push_back(1.0); intensities.push_back(2.0); intensities.push_back(3.0); intensities.push_back(4.0); intensities.push_back(5.0); permuter.addOptionSet(intensities, &intensity); 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); permuter.addOptionSet(min_angles, &min_angle); 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); permuter.addOptionSet(max_angles, &max_angle); // 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 permuter.addOptionSet(angle_increments, &angle_increment); scan_times.push_back(ros::Duration(1/40)); scan_times.push_back(ros::Duration(1/20)); permuter.addOptionSet(scan_times, &scan_time); while (permuter.step()) { try { // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); sensor_msgs::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, 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); 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; } 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) { if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception FAIL(); } } } TEST(laser_geometry, transformLaserScanToPointCloud) { tf::Transformer tf; double tolerance = 1e-12; laser_geometry::LaserProjection projector; double min_angle = -M_PI/2; double max_angle = M_PI/2; double angle_increment = M_PI/180; double range = 1.0; double intensity = 1.0; ros::Duration scan_time = ros::Duration(1/40); ros::Duration increment_time = ros::Duration(1/400); std::vector ranges, intensities, min_angles, max_angles, angle_increments; std::vector increment_times, scan_times; rostest::Permuter permuter; ranges.push_back(-1.0); ranges.push_back(1.0); ranges.push_back(2.0); ranges.push_back(3.0); ranges.push_back(4.0); ranges.push_back(5.0); ranges.push_back(100.0); permuter.addOptionSet(ranges, &range); intensities.push_back(1.0); intensities.push_back(2.0); intensities.push_back(3.0); intensities.push_back(4.0); intensities.push_back(5.0); permuter.addOptionSet(intensities, &intensity); 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); 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); permuter.addOptionSet(min_angles, &min_angle); permuter.addOptionSet(max_angles, &max_angle); 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 permuter.addOptionSet(angle_increments, &angle_increment); scan_times.push_back(ros::Duration(1/40)); scan_times.push_back(ros::Duration(1/20)); permuter.addOptionSet(scan_times, &scan_time); while (permuter.step()) { try { //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); scan.header.frame_id = "laser_frame"; sensor_msgs::PointCloud cloud_out; projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4); 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.points.size()); for (unsigned int i = 0; i < cloud_out.points.size(); i++) { EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance); EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps }; } catch (BuildScanException &ex) { if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception FAIL(); } } } TEST(laser_geometry, transformLaserScanToPointCloud2) { tf::Transformer tf; tf2::BufferCore tf2; double tolerance = 1e-12; laser_geometry::LaserProjection projector; double min_angle = -M_PI/2; double max_angle = M_PI/2; double angle_increment = M_PI/180; double range = 1.0; double intensity = 1.0; ros::Duration scan_time = ros::Duration(1/40); ros::Duration increment_time = ros::Duration(1/400); std::vector ranges, intensities, min_angles, max_angles, angle_increments; std::vector increment_times, scan_times; rostest::Permuter permuter; ranges.push_back(-1.0); ranges.push_back(1.0); ranges.push_back(2.0); ranges.push_back(3.0); ranges.push_back(4.0); ranges.push_back(5.0); ranges.push_back(100.0); permuter.addOptionSet(ranges, &range); intensities.push_back(1.0); intensities.push_back(2.0); intensities.push_back(3.0); intensities.push_back(4.0); intensities.push_back(5.0); permuter.addOptionSet(intensities, &intensity); 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); 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); permuter.addOptionSet(min_angles, &min_angle); permuter.addOptionSet(max_angles, &max_angle); 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 permuter.addOptionSet(angle_increments, &angle_increment); scan_times.push_back(ros::Duration(1/40)); scan_times.push_back(ros::Duration(1/20)); permuter.addOptionSet(scan_times, &scan_time); while (permuter.step()) { try { //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); scan.header.frame_id = "laser_frame"; sensor_msgs::PointCloud2 cloud_out; projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); EXPECT_EQ(cloud_out.is_dense, false); 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; } 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) { if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception FAIL(); } } } int main(int argc, char **argv){ ros::Time::init(); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ros-laser-geometry-1.6.4/test/projection_test.py000077500000000000000000000141471256021102700220340ustar00rootroot00000000000000#!/usr/bin/env python PKG='laser_geometry' import rospy import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import LaserScan from laser_geometry import LaserProjection import numpy as np from itertools import product import unittest PROJECTION_TEST_RANGE_MIN = 0.23 PROJECTION_TEST_RANGE_MAX = 40.00 class BuildScanException: pass def build_constant_scan( range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time): count = np.uint(np.ceil((angle_max - angle_min) / angle_increment)) if count < 0: raise BuildScanException scan = LaserScan() scan.header.stamp = rospy.rostime.Time.from_sec(10.10) scan.header.frame_id = "laser_frame" scan.angle_min = angle_min scan.angle_max = angle_max scan.angle_increment = angle_increment scan.scan_time = scan_time.to_sec() scan.range_min = PROJECTION_TEST_RANGE_MIN scan.range_max = PROJECTION_TEST_RANGE_MAX scan.ranges = [range_val for _ in range(count)] scan.intensities = [intensity_val for _ in range(count)] scan.time_increment = scan_time.to_sec()/count return scan class ProjectionTest(unittest.TestCase): def test_project_laser(self): tolerance = 6 # decimal places projector = LaserProjection() ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0] intensities = np.arange(1.0, 6.0).tolist() min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0]) max_angles = -min_angles angle_increments = np.pi / np.array([180., 360., 720.]) scan_times = [rospy.Duration(1./i) for i in [40, 20]] for range_val, intensity_val, \ angle_min, angle_max, angle_increment, scan_time in \ product(ranges, intensities, min_angles, max_angles, angle_increments, scan_times): try: scan = build_constant_scan( range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time) except BuildScanException: if (angle_max - angle_min)/angle_increment > 0: self.fail() cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INDEX) self.assertEquals(len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 4") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY) self.assertEquals(len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 4") cloud_out = projector.projectLaser(scan, -1.0) self.assertEquals(len(cloud_out.fields), 5, "PointCloud2 with channel INDEX: fields size != 5") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX) self.assertEquals(len(cloud_out.fields), 5, "PointCloud2 with channel INDEX: fields size != 5") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.DISTANCE) self.assertEquals(len(cloud_out.fields), 6, "PointCloud2 with channel INDEX: fields size != 6") cloud_out = projector.projectLaser(scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.DISTANCE | LaserProjection.ChannelOption.TIMESTAMP) self.assertEquals(len(cloud_out.fields), 7, "PointCloud2 with channel INDEX: fields size != 7") valid_points = 0 for i in range(len(scan.ranges)): ri = scan.ranges[i] if (PROJECTION_TEST_RANGE_MIN <= ri and ri <= PROJECTION_TEST_RANGE_MAX): valid_points += 1 self.assertEqual(valid_points, cloud_out.width, "Valid points != PointCloud2 width") idx_x = idx_y = idx_z = 0 idx_intensity = idx_index = 0 idx_distance = idx_stamps = 0 i = 0 for f in cloud_out.fields: if f.name == "x": idx_x = i elif f.name == "y": idx_y = i elif f.name == "z": idx_z = i elif f.name == "intensity": idx_intensity = i elif f.name == "index": idx_index = i elif f.name == "distances": idx_distance = i elif f.name == "stamps": idx_stamps = i i += 1 i = 0 for point in pc2.read_points(cloud_out): ri = scan.ranges[i] ai = scan.angle_min + i * scan.angle_increment self.assertAlmostEqual(point[idx_x], ri * np.cos(ai), tolerance, "x not equal") self.assertAlmostEqual(point[idx_y], ri * np.sin(ai), tolerance, "y not equal") self.assertAlmostEqual(point[idx_z], 0, tolerance, "z not equal") self.assertAlmostEqual(point[idx_intensity], scan.intensities[i], tolerance, "Intensity not equal") self.assertAlmostEqual(point[idx_index], i, tolerance, "Index not equal") self.assertAlmostEqual(point[idx_distance], ri, tolerance, "Distance not equal") self.assertAlmostEqual(point[idx_stamps], i * scan.time_increment, tolerance, "Timestamp not equal") i += 1 if __name__ == '__main__': import rosunit rosunit.unitrun(PKG, 'projection_test', ProjectionTest)