From 8fb9c51bf888c4b28ee23024908266f4ef37fb80 Mon Sep 17 00:00:00 2001 From: Alessandro Bottero Date: Tue, 13 Mar 2018 12:45:30 +0100 Subject: [PATCH] Make it compile, remove PointCloud support, and remove boost - Compiles on Windows with VS2015/VS2017 - Compiles on Mac with clang - Compiles on Linux with gcc - Removed PointCloud support as this is deprecated and might not be needed in ROS 2 - Remove boost as per ROS 2 development guidelines --- CMakeLists.txt | 27 ++- .../{laser_geometry.h => laser_geometry.hpp} | 126 +------------- src/laser_geometry.cpp | 154 +----------------- 3 files changed, 23 insertions(+), 284 deletions(-) rename include/laser_geometry/{laser_geometry.h => laser_geometry.hpp} (58%) diff --git a/CMakeLists.txt b/CMakeLists.txt index fd5332d..e96c438 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(laser_geometry) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) @@ -10,9 +14,6 @@ find_package(sensor_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(Eigen3 REQUIRED) -set(Boost_USE_STATIC_LIBS ON) -find_package(Boost REQUIRED) - # TODO(dhood): enable python support once ported to ROS 2 # catkin_python_setup() @@ -21,7 +22,6 @@ include_directories(include ${rclcpp_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) @@ -34,12 +34,21 @@ target_link_libraries(laser_geometry ) ament_export_include_directories(include) +ament_export_interfaces(laser_geometry) ament_export_libraries(laser_geometry) ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake) -install(TARGETS laser_geometry +install( + TARGETS laser_geometry + EXPORT laser_geometry ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib) -install(DIRECTORY include/laser_geometry/ - DESTINATION include/${PROJECT_NAME}/ - FILES_MATCHING PATTERN "*.h") + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.hpp similarity index 58% rename from include/laser_geometry/laser_geometry.h rename to include/laser_geometry/laser_geometry.hpp index fee95ce..eac640c 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.hpp @@ -34,13 +34,9 @@ #include #include -#include "boost/numeric/ublas/matrix.hpp" -#include "boost/thread/mutex.hpp" - -#include -#include -#include -#include +#include "tf2/buffer_core.h" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #ifndef ROS_DEBUG #define ROS_DEBUG(...) @@ -107,33 +103,6 @@ namespace laser_geometry LaserProjection() : angle_min_(0), angle_max_(0) {} - //! Destructor to deallocate stored unit vectors - ~LaserProjection(); - - //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::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::msg::LaserScan& scan_in, - sensor_msgs::msg::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::msg::LaserScan into a sensor_msgs::msg::PointCloud2 /*! * Project a single laser scan from a linear array into a 3D @@ -158,65 +127,6 @@ namespace laser_geometry projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); } - //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::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::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - tf2::BufferCore &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::msg::LaserScan into a sensor_msgs::msg::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::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - tf2::BufferCore &tf, - int channel_options = channel_option::Default) - { - return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, 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 @@ -249,42 +159,14 @@ namespace laser_geometry 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::msg::LaserScan& scan_in, - sensor_msgs::msg::PointCloud& cloud_out, - double range_cutoff, - bool preservative, - int channel_options); - //! 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 transformLaserScanToPointCloud - void transformLaserScanToPointCloud_ (const std::string& target_frame, - sensor_msgs::msg::PointCloud& cloud_out, - const sensor_msgs::msg::LaserScan& scan_in, - tf2::BufferCore &tf, - 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, @@ -305,11 +187,9 @@ namespace laser_geometry 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_; }; } diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index e6d4f9a..e682e6b 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -27,7 +27,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "laser_geometry/laser_geometry.h" +#include "laser_geometry/laser_geometry.hpp" + #include #include "rclcpp/time.hpp" #define TIME rclcpp::Time @@ -41,157 +42,6 @@ typedef double tfScalar; namespace laser_geometry { - - void - LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::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::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::PointCloud2 &cloud_out, double range_cutoff,