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()
|
||||
|
||||
#todo integrate this
|
||||
rospack_add_library(laser_processor src/laser_processor.cpp)
|
||||
|
||||
rospack_add_library(laser_geometry src/laser_geometry.cpp )
|
||||
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="angles"/>
|
||||
<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>
|
||||
</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