LaserProc.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* 
00031  * Author: Chad Rockey
00032  */
00033 
00034 #include <laser_proc/LaserProc.h>
00035 
00036 using namespace laser_proc;
00037 
00038 sensor_msgs::LaserScanPtr LaserProc::getFirstScan(const sensor_msgs::MultiEchoLaserScan& msg){
00039   sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
00040   fillLaserScan(msg, *out);
00041   out->ranges.resize(msg.ranges.size());
00042   if(msg.ranges.size() == msg.intensities.size()){
00043     out->intensities.resize(msg.intensities.size());
00044   }
00045 
00046   for(size_t i = 0; i < out->ranges.size(); i++){
00047     size_t index = getFirstValue(msg.ranges[i], out->ranges[i]);
00048     if(out->intensities.size() > 0){
00049       if(msg.intensities[i].echoes.size() > 0){
00050         out->intensities[i] = msg.intensities[i].echoes[index];
00051       } else {
00052         out->intensities[i] = 0.0;
00053       }
00054     }
00055   }
00056   return out;
00057 }
00058 
00059 sensor_msgs::LaserScanPtr LaserProc::getLastScan(const sensor_msgs::MultiEchoLaserScan& msg){
00060   sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
00061   fillLaserScan(msg, *out);
00062   out->ranges.resize(msg.ranges.size());
00063   if(msg.ranges.size() == msg.intensities.size()){
00064     out->intensities.resize(msg.intensities.size());
00065   }
00066   for(size_t i = 0; i < out->ranges.size(); i++){
00067     size_t index = getLastValue(msg.ranges[i], out->ranges[i]);
00068     if(out->intensities.size() > 0){
00069       if(msg.intensities[i].echoes.size() > 0){
00070         out->intensities[i] = msg.intensities[i].echoes[index];
00071       } else {
00072         out->intensities[i] = 0.0;
00073       }
00074     }
00075   }
00076   return out;
00077 }
00078 
00079 sensor_msgs::LaserScanPtr LaserProc::getMostIntenseScan(const sensor_msgs::MultiEchoLaserScan& msg){
00080   sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
00081   fillLaserScan(msg, *out);
00082   if(msg.ranges.size() == msg.intensities.size()){
00083     out->ranges.resize(msg.ranges.size());
00084     out->intensities.resize(msg.intensities.size());
00085   } else {
00086     std::stringstream ss;
00087     ss << "getMostIntenseScan::Size of ranges does not equal size of intensities, cannot create scan.";
00088     throw std::runtime_error(ss.str());
00089   }
00090   for(size_t i = 0; i < out->intensities.size(); i++){
00091     getMostIntenseValue(msg.ranges[i], msg.intensities[i], out->ranges[i], out->intensities[i]);
00092   }
00093   return out;
00094 }
00095 
00096 void LaserProc::fillLaserScan(const sensor_msgs::MultiEchoLaserScan& msg, sensor_msgs::LaserScan& out){
00097   out.header = msg.header;
00098   out.angle_min = msg.angle_min;
00099   out.angle_max = msg.angle_max;
00100   out.angle_increment = msg.angle_increment;
00101   out.time_increment = msg.time_increment;
00102   out.scan_time = msg.scan_time;
00103   out.range_min = msg.range_min;
00104   out.range_max = msg.range_max;
00105 }
00106 
00108 size_t LaserProc::getFirstValue(const sensor_msgs::LaserEcho& ranges, float& range){
00109   if(ranges.echoes.size() > 0){
00110     size_t index = 0;
00111     range = ranges.echoes[index];
00112     return index;
00113   }
00114 
00115   range = std::numeric_limits<float>::quiet_NaN();
00116   return 0; // Value doesn't matter
00117 }
00118 
00120 size_t LaserProc::getLastValue(const sensor_msgs::LaserEcho& ranges, float& range){
00121   if(ranges.echoes.size() > 0){
00122     size_t index = ranges.echoes.size()-1;
00123     range = ranges.echoes[index];
00124     return index;
00125   }
00126 
00127   range = std::numeric_limits<float>::quiet_NaN();
00128   return 0; // Value doesn't matter
00129 }
00130 
00131 void LaserProc::getMostIntenseValue(const sensor_msgs::LaserEcho& ranges, const sensor_msgs::LaserEcho& intensities, float& range, float& intensity){
00132   if(intensities.echoes.size() == 0){
00133     range = std::numeric_limits<float>::quiet_NaN();
00134     intensity = 0.0;
00135     return;
00136   }
00137 
00138   std::vector<float>::const_iterator max_iter = std::max_element(intensities.echoes.begin(), intensities.echoes.end());
00139   size_t index = std::distance(intensities.echoes.begin(), max_iter);
00140 
00141   if(ranges.echoes.size() > 0){
00142     range = ranges.echoes[index];
00143     intensity = *max_iter;
00144   } else {
00145     range = std::numeric_limits<float>::quiet_NaN();
00146     intensity = 0.0;
00147     return;
00148   }
00149 }


laser_proc
Author(s): Chad Rockey
autogenerated on Thu Aug 27 2015 13:47:25