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
This commit is contained in:
Alessandro Bottero
Martin Idel
parent
7ad6e21d3d
commit
8fb9c51bf8
|
|
@ -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
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -34,13 +34,9 @@
|
|||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
#include "boost/numeric/ublas/matrix.hpp"
|
||||
#include "boost/thread/mutex.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>
|
||||
#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<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_;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user