From 136cf0c3b8b1e31ed596084db8e04f6538010f8a Mon Sep 17 00:00:00 2001 From: Jeremy Leibs Date: Fri, 25 Sep 2009 20:29:41 +0000 Subject: [PATCH] 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 --- CMakeLists.txt | 3 - include/laser_geometry/laser_processor.h | 145 ------------ manifest.xml | 2 +- src/laser_processor.cpp | 288 ----------------------- 4 files changed, 1 insertion(+), 437 deletions(-) delete mode 100644 include/laser_geometry/laser_processor.h delete mode 100644 src/laser_processor.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 843f18b..6b77edd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/include/laser_geometry/laser_processor.h b/include/laser_geometry/laser_processor.h deleted file mode 100644 index 0f26891..0000000 --- a/include/laser_geometry/laser_processor.h +++ /dev/null @@ -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 -#include -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/PointCloud.h" -#include "geometry_msgs/Point.h" - -#include -#include -#include -#include -#include -#include - -#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 - { - 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 clusters_; - sensor_msgs::LaserScan scan_; - - public: - - std::list& 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 diff --git a/manifest.xml b/manifest.xml index 49a2193..229ea15 100644 --- a/manifest.xml +++ b/manifest.xml @@ -17,6 +17,6 @@ scanners. - + diff --git a/src/laser_processor.cpp b/src/laser_processor.cpp deleted file mode 100644 index b788d82..0000000 --- a/src/laser_processor.cpp +++ /dev/null @@ -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 - -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::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::iterator c = clusters_.begin(); - c != clusters_.end(); - c++) - delete (*c); -} - -void -ScanProcessor::removeLessThan(uint32_t num) -{ - list::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 tmp_clusters; - - list::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_queue; - sample_queue.push_back(*s_first); - - (*c_iter)->erase(s_first); - - // Grow until we get to the end of the queue - list::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()); -}