Pulling laser processing out of laser geomtry for 0.9.0 release.
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24513 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs
parent
72f5987e54
commit
136cf0c3b8
|
|
@ -5,9 +5,6 @@ rospack(laser_geometry)
|
||||||
|
|
||||||
rospack_add_boost_directories()
|
rospack_add_boost_directories()
|
||||||
|
|
||||||
#todo integrate this
|
|
||||||
rospack_add_library(laser_processor src/laser_processor.cpp)
|
|
||||||
|
|
||||||
rospack_add_library(laser_geometry src/laser_geometry.cpp )
|
rospack_add_library(laser_geometry src/laser_geometry.cpp )
|
||||||
rospack_link_boost(laser_geometry thread)
|
rospack_link_boost(laser_geometry thread)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,145 +0,0 @@
|
||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* 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 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.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
//! A namespace containing the laser processor helper classes
|
|
||||||
|
|
||||||
|
|
||||||
#ifndef LASER_SCAN_LASERPROCESSOR_HH
|
|
||||||
#define LASER_SCAN_LASERPROCESSOR_HH
|
|
||||||
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include "sensor_msgs/LaserScan.h"
|
|
||||||
#include "sensor_msgs/PointCloud.h"
|
|
||||||
#include "geometry_msgs/Point.h"
|
|
||||||
|
|
||||||
#include <list>
|
|
||||||
#include <set>
|
|
||||||
#include <vector>
|
|
||||||
#include <map>
|
|
||||||
#include <utility>
|
|
||||||
#include <algorithm>
|
|
||||||
|
|
||||||
#include "tf/transform_datatypes.h"
|
|
||||||
|
|
||||||
#warning laser_geometry/laser_processor.h has been deprecated
|
|
||||||
|
|
||||||
namespace laser_geometry
|
|
||||||
{
|
|
||||||
//! A struct representing a single sample from the laser.
|
|
||||||
class Sample
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
int index;
|
|
||||||
float range;
|
|
||||||
float intensity;
|
|
||||||
float x;
|
|
||||||
float y;
|
|
||||||
|
|
||||||
__attribute__ ((deprecated)) static Sample* Extract(int ind, const sensor_msgs::LaserScan& scan);
|
|
||||||
|
|
||||||
private:
|
|
||||||
__attribute__ ((deprecated)) Sample() {};
|
|
||||||
};
|
|
||||||
|
|
||||||
//! The comparator allowing the creation of an ordered "SampleSet"
|
|
||||||
struct CompareSample
|
|
||||||
{
|
|
||||||
__attribute__ ((deprecated)) CompareSample() {}
|
|
||||||
|
|
||||||
inline bool operator() (const Sample* a, const Sample* b)
|
|
||||||
{
|
|
||||||
return (a->index < b->index);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
//! An ordered set of Samples
|
|
||||||
class SampleSet : public std::set<Sample*, CompareSample>
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
__attribute__ ((deprecated)) SampleSet() {}
|
|
||||||
|
|
||||||
~SampleSet() { clear(); }
|
|
||||||
|
|
||||||
void clear();
|
|
||||||
|
|
||||||
void appendToCloud(sensor_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0);
|
|
||||||
|
|
||||||
tf::Point center();
|
|
||||||
};
|
|
||||||
|
|
||||||
//! A mask for filtering out Samples based on range
|
|
||||||
class ScanMask
|
|
||||||
{
|
|
||||||
SampleSet mask_;
|
|
||||||
|
|
||||||
bool filled;
|
|
||||||
float angle_min;
|
|
||||||
float angle_max;
|
|
||||||
uint32_t size;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
__attribute__ ((deprecated)) ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { }
|
|
||||||
|
|
||||||
inline void clear() { mask_.clear(); filled = false; }
|
|
||||||
|
|
||||||
void addScan(sensor_msgs::LaserScan& scan);
|
|
||||||
|
|
||||||
bool hasSample(Sample* s, float thresh);
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class ScanProcessor
|
|
||||||
{
|
|
||||||
std::list<SampleSet*> clusters_;
|
|
||||||
sensor_msgs::LaserScan scan_;
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
std::list<SampleSet*>& getClusters() { return clusters_; }
|
|
||||||
|
|
||||||
__attribute__ ((deprecated)) ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03);
|
|
||||||
|
|
||||||
~ScanProcessor();
|
|
||||||
|
|
||||||
void removeLessThan(uint32_t num);
|
|
||||||
|
|
||||||
void splitConnected(float thresh);
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
@ -17,6 +17,6 @@ scanners.
|
||||||
<depend package="tf"/>
|
<depend package="tf"/>
|
||||||
<depend package="angles"/>
|
<depend package="angles"/>
|
||||||
<export>
|
<export>
|
||||||
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_geometry -llaser_processor"/>
|
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_geometry"/>
|
||||||
</export>
|
</export>
|
||||||
</package>
|
</package>
|
||||||
|
|
|
||||||
|
|
@ -1,288 +0,0 @@
|
||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* 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 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_processor.h"
|
|
||||||
|
|
||||||
#include <stdexcept>
|
|
||||||
|
|
||||||
using namespace ros;
|
|
||||||
using namespace std;
|
|
||||||
using namespace laser_geometry;
|
|
||||||
|
|
||||||
Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan)
|
|
||||||
{
|
|
||||||
Sample* s = new Sample;
|
|
||||||
|
|
||||||
s->index = ind;
|
|
||||||
s->range = scan.ranges[ind];
|
|
||||||
s->intensity = scan.intensities[ind];
|
|
||||||
s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
|
|
||||||
s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
|
|
||||||
if (s->range > scan.range_min && s->range < scan.range_max)
|
|
||||||
return s;
|
|
||||||
else
|
|
||||||
{
|
|
||||||
delete s;
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void SampleSet::clear()
|
|
||||||
{
|
|
||||||
for (SampleSet::iterator i = begin();
|
|
||||||
i != end();
|
|
||||||
i++)
|
|
||||||
{
|
|
||||||
delete (*i);
|
|
||||||
}
|
|
||||||
set<Sample*, CompareSample>::clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int b)
|
|
||||||
{
|
|
||||||
float color_val = 0;
|
|
||||||
|
|
||||||
int rgb = (r << 16) | (g << 8) | b;
|
|
||||||
color_val = *(float*)&(rgb);
|
|
||||||
|
|
||||||
for (iterator sample_iter = begin();
|
|
||||||
sample_iter != end();
|
|
||||||
sample_iter++)
|
|
||||||
{
|
|
||||||
geometry_msgs::Point32 point;
|
|
||||||
point.x = (*sample_iter)->x;
|
|
||||||
point.y = (*sample_iter)->y;
|
|
||||||
point.z = 0;
|
|
||||||
|
|
||||||
cloud.points.push_back(point);
|
|
||||||
|
|
||||||
if (cloud.channels[0].name == "rgb")
|
|
||||||
cloud.channels[0].values.push_back(color_val);
|
|
||||||
|
|
||||||
if (cloud.channels[0].name == "intensity")
|
|
||||||
cloud.channels[0].values.push_back((*sample_iter)->intensity);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
tf::Point SampleSet::center()
|
|
||||||
{
|
|
||||||
float x_mean = 0.0;
|
|
||||||
float y_mean = 0.0;
|
|
||||||
for (iterator i = begin();
|
|
||||||
i != end();
|
|
||||||
i++)
|
|
||||||
|
|
||||||
{
|
|
||||||
x_mean += ((*i)->x)/size();
|
|
||||||
y_mean += ((*i)->y)/size();
|
|
||||||
}
|
|
||||||
|
|
||||||
return tf::Point (x_mean, y_mean, 0.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void ScanMask::addScan(sensor_msgs::LaserScan& scan)
|
|
||||||
{
|
|
||||||
if (!filled)
|
|
||||||
{
|
|
||||||
angle_min = scan.angle_min;
|
|
||||||
angle_max = scan.angle_max;
|
|
||||||
size = scan.ranges.size();
|
|
||||||
filled = true;
|
|
||||||
} else if (angle_min != scan.angle_min ||
|
|
||||||
angle_max != scan.angle_max ||
|
|
||||||
size != scan.ranges.size())
|
|
||||||
{
|
|
||||||
throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
|
|
||||||
}
|
|
||||||
|
|
||||||
for (uint32_t i = 0; i < scan.ranges.size(); i++)
|
|
||||||
{
|
|
||||||
Sample* s = Sample::Extract(i, scan);
|
|
||||||
|
|
||||||
if (s != NULL)
|
|
||||||
{
|
|
||||||
SampleSet::iterator m = mask_.find(s);
|
|
||||||
|
|
||||||
if (m != mask_.end())
|
|
||||||
{
|
|
||||||
if ((*m)->range > s->range)
|
|
||||||
{
|
|
||||||
delete (*m);
|
|
||||||
mask_.erase(m);
|
|
||||||
mask_.insert(s);
|
|
||||||
} else {
|
|
||||||
delete s;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
mask_.insert(s);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
bool ScanMask::hasSample(Sample* s, float thresh)
|
|
||||||
{
|
|
||||||
if (s != NULL)
|
|
||||||
{
|
|
||||||
SampleSet::iterator m = mask_.find(s);
|
|
||||||
if ( m != mask_.end())
|
|
||||||
if (((*m)->range - thresh) < s->range)
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold)
|
|
||||||
{
|
|
||||||
scan_ = scan;
|
|
||||||
|
|
||||||
SampleSet* cluster = new SampleSet;
|
|
||||||
|
|
||||||
for (uint32_t i = 0; i < scan.ranges.size(); i++)
|
|
||||||
{
|
|
||||||
Sample* s = Sample::Extract(i, scan);
|
|
||||||
|
|
||||||
if (s != NULL)
|
|
||||||
{
|
|
||||||
if (!mask_.hasSample(s, mask_threshold))
|
|
||||||
{
|
|
||||||
cluster->insert(s);
|
|
||||||
} else {
|
|
||||||
delete s;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
clusters_.push_back(cluster);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
ScanProcessor::~ScanProcessor()
|
|
||||||
{
|
|
||||||
for ( list<SampleSet*>::iterator c = clusters_.begin();
|
|
||||||
c != clusters_.end();
|
|
||||||
c++)
|
|
||||||
delete (*c);
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
ScanProcessor::removeLessThan(uint32_t num)
|
|
||||||
{
|
|
||||||
list<SampleSet*>::iterator c_iter = clusters_.begin();
|
|
||||||
while (c_iter != clusters_.end())
|
|
||||||
{
|
|
||||||
if ( (*c_iter)->size() < num )
|
|
||||||
{
|
|
||||||
delete (*c_iter);
|
|
||||||
clusters_.erase(c_iter++);
|
|
||||||
} else {
|
|
||||||
++c_iter;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void
|
|
||||||
ScanProcessor::splitConnected(float thresh)
|
|
||||||
{
|
|
||||||
list<SampleSet*> tmp_clusters;
|
|
||||||
|
|
||||||
list<SampleSet*>::iterator c_iter = clusters_.begin();
|
|
||||||
|
|
||||||
// For each cluster
|
|
||||||
while (c_iter != clusters_.end())
|
|
||||||
{
|
|
||||||
// Go through the entire list
|
|
||||||
while ((*c_iter)->size() > 0 )
|
|
||||||
{
|
|
||||||
// Take the first element
|
|
||||||
SampleSet::iterator s_first = (*c_iter)->begin();
|
|
||||||
|
|
||||||
// Start a new queue
|
|
||||||
list<Sample*> sample_queue;
|
|
||||||
sample_queue.push_back(*s_first);
|
|
||||||
|
|
||||||
(*c_iter)->erase(s_first);
|
|
||||||
|
|
||||||
// Grow until we get to the end of the queue
|
|
||||||
list<Sample*>::iterator s_q = sample_queue.begin();
|
|
||||||
while (s_q != sample_queue.end())
|
|
||||||
{
|
|
||||||
int expand = (int)(asin( thresh / (*s_q)->range ) / scan_.angle_increment);
|
|
||||||
|
|
||||||
SampleSet::iterator s_rest = (*c_iter)->begin();
|
|
||||||
|
|
||||||
while ( (s_rest != (*c_iter)->end() &&
|
|
||||||
(*s_rest)->index < (*s_q)->index + expand ) )
|
|
||||||
{
|
|
||||||
if ( (*s_rest)->range - (*s_q)->range > thresh)
|
|
||||||
{
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
else if (sqrt( pow( (*s_q)->x - (*s_rest)->x, 2.0f) + pow( (*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
|
|
||||||
{
|
|
||||||
sample_queue.push_back(*s_rest);
|
|
||||||
(*c_iter)->erase(s_rest++);
|
|
||||||
break;
|
|
||||||
} else {
|
|
||||||
++s_rest;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
s_q++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Move all the samples into the new cluster
|
|
||||||
SampleSet* c = new SampleSet;
|
|
||||||
for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
|
|
||||||
c->insert(*s_q);
|
|
||||||
|
|
||||||
// Store the temporary clusters
|
|
||||||
tmp_clusters.push_back(c);
|
|
||||||
}
|
|
||||||
|
|
||||||
//Now that c_iter is empty, we can delete
|
|
||||||
delete (*c_iter);
|
|
||||||
|
|
||||||
//And remove from the map
|
|
||||||
clusters_.erase(c_iter++);
|
|
||||||
}
|
|
||||||
|
|
||||||
clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue
Block a user