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
committed by
Martin Idel
Martin Idel
parent
7ad6e21d3d
commit
8fb9c51bf8
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user