LaserProc.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Chad Rockey
32  */
33 
34 #include <laser_proc/LaserProc.h>
35 
36 using namespace laser_proc;
37 
38 sensor_msgs::LaserScanPtr LaserProc::getFirstScan(const sensor_msgs::MultiEchoLaserScan& msg){
39  sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
40  fillLaserScan(msg, *out);
41  out->ranges.resize(msg.ranges.size());
42  if(msg.ranges.size() == msg.intensities.size()){
43  out->intensities.resize(msg.intensities.size());
44  }
45 
46  for(size_t i = 0; i < out->ranges.size(); i++){
47  size_t index = getFirstValue(msg.ranges[i], out->ranges[i]);
48  if(out->intensities.size() > 0){
49  if(msg.intensities[i].echoes.size() > 0){
50  out->intensities[i] = msg.intensities[i].echoes[index];
51  } else {
52  out->intensities[i] = 0.0;
53  }
54  }
55  }
56  return out;
57 }
58 
59 sensor_msgs::LaserScanPtr LaserProc::getLastScan(const sensor_msgs::MultiEchoLaserScan& msg){
60  sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
61  fillLaserScan(msg, *out);
62  out->ranges.resize(msg.ranges.size());
63  if(msg.ranges.size() == msg.intensities.size()){
64  out->intensities.resize(msg.intensities.size());
65  }
66  for(size_t i = 0; i < out->ranges.size(); i++){
67  size_t index = getLastValue(msg.ranges[i], out->ranges[i]);
68  if(out->intensities.size() > 0){
69  if(msg.intensities[i].echoes.size() > 0){
70  out->intensities[i] = msg.intensities[i].echoes[index];
71  } else {
72  out->intensities[i] = 0.0;
73  }
74  }
75  }
76  return out;
77 }
78 
79 sensor_msgs::LaserScanPtr LaserProc::getMostIntenseScan(const sensor_msgs::MultiEchoLaserScan& msg){
80  sensor_msgs::LaserScanPtr out(new sensor_msgs::LaserScan());
81  fillLaserScan(msg, *out);
82  if(msg.ranges.size() == msg.intensities.size()){
83  out->ranges.resize(msg.ranges.size());
84  out->intensities.resize(msg.intensities.size());
85  } else {
86  std::stringstream ss;
87  ss << "getMostIntenseScan::Size of ranges does not equal size of intensities, cannot create scan.";
88  throw std::runtime_error(ss.str());
89  }
90  for(size_t i = 0; i < out->intensities.size(); i++){
91  getMostIntenseValue(msg.ranges[i], msg.intensities[i], out->ranges[i], out->intensities[i]);
92  }
93  return out;
94 }
95 
96 void LaserProc::fillLaserScan(const sensor_msgs::MultiEchoLaserScan& msg, sensor_msgs::LaserScan& out){
97  out.header = msg.header;
98  out.angle_min = msg.angle_min;
99  out.angle_max = msg.angle_max;
100  out.angle_increment = msg.angle_increment;
101  out.time_increment = msg.time_increment;
102  out.scan_time = msg.scan_time;
103  out.range_min = msg.range_min;
104  out.range_max = msg.range_max;
105 }
106 
108 size_t LaserProc::getFirstValue(const sensor_msgs::LaserEcho& ranges, float& range){
109  if(ranges.echoes.size() > 0){
110  size_t index = 0;
111  range = ranges.echoes[index];
112  return index;
113  }
114 
115  range = std::numeric_limits<float>::quiet_NaN();
116  return 0; // Value doesn't matter
117 }
118 
120 size_t LaserProc::getLastValue(const sensor_msgs::LaserEcho& ranges, float& range){
121  if(ranges.echoes.size() > 0){
122  size_t index = ranges.echoes.size()-1;
123  range = ranges.echoes[index];
124  return index;
125  }
126 
127  range = std::numeric_limits<float>::quiet_NaN();
128  return 0; // Value doesn't matter
129 }
130 
131 void LaserProc::getMostIntenseValue(const sensor_msgs::LaserEcho& ranges, const sensor_msgs::LaserEcho& intensities, float& range, float& intensity){
132  if(intensities.echoes.size() == 0){
133  range = std::numeric_limits<float>::quiet_NaN();
134  intensity = 0.0;
135  return;
136  }
137 
138  std::vector<float>::const_iterator max_iter = std::max_element(intensities.echoes.begin(), intensities.echoes.end());
139  size_t index = std::distance(intensities.echoes.begin(), max_iter);
140 
141  if(ranges.echoes.size() > 0){
142  range = ranges.echoes[index];
143  intensity = *max_iter;
144  } else {
145  range = std::numeric_limits<float>::quiet_NaN();
146  intensity = 0.0;
147  return;
148  }
149 }
static sensor_msgs::LaserScanPtr getFirstScan(const sensor_msgs::MultiEchoLaserScan &msg)
Definition: LaserProc.cpp:38
static sensor_msgs::LaserScanPtr getMostIntenseScan(const sensor_msgs::MultiEchoLaserScan &msg)
Definition: LaserProc.cpp:79
static size_t getLastValue(const sensor_msgs::LaserEcho &ranges, float &range)
Definition: LaserProc.cpp:120
static sensor_msgs::LaserScanPtr getLastScan(const sensor_msgs::MultiEchoLaserScan &msg)
Definition: LaserProc.cpp:59
static void fillLaserScan(const sensor_msgs::MultiEchoLaserScan &msg, sensor_msgs::LaserScan &out)
I&#39;m assuming all laserscanners/drivers output the ranges in order received (shortest to longest)...
Definition: LaserProc.cpp:96
static size_t getFirstValue(const sensor_msgs::LaserEcho &ranges, float &range)
I&#39;m assuming all laserscanners/drivers output the ranges in order received (shortest to longest)...
Definition: LaserProc.cpp:108
static void getMostIntenseValue(const sensor_msgs::LaserEcho &ranges, const sensor_msgs::LaserEcho &intensities, float &range, float &intensity)
Definition: LaserProc.cpp:131


laser_proc
Author(s): Chad Rockey
autogenerated on Thu Jun 6 2019 19:56:23