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

@@ -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)
+  LIBRARY DESTINATION lib
-install(DIRECTORY include/laser_geometry/
+  RUNTIME DESTINATION bin
-  DESTINATION include/${PROJECT_NAME}/
+  INCLUDES DESTINATION include
-  FILES_MATCHING PATTERN "*.h")
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+

include/laser_geometry/laser_geometry.hpp

@@ -34,13 +34,9 @@
 #include <iostream>
 #include <sstream>
 
-#include "boost/numeric/ublas/matrix.hpp"
+#include "tf2/buffer_core.h"
-#include "boost/thread/mutex.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
-
+#include "sensor_msgs/msg/point_cloud2.hpp"
-#include <tf2/buffer_core.h>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <sensor_msgs/msg/point_cloud.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<double>& 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<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
       float angle_min_;
       float angle_max_;
       Eigen::ArrayXXd co_sine_map_;
-      boost::mutex guv_mutex_;
     };
 
 }

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 <algorithm>
 #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<double> 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<double> 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<double>& 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 <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
-    std::map<std::string, boost::numeric::ublas::matrix<double>* >::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<double> * tempPtr = new boost::numeric::ublas::matrix<double>(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<std::string, boost::numeric::ublas::matrix<double>*>::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,