43 Sample* Sample::Extract(
int ind,
const sensor_msgs::LaserScan& scan)
48 s->
range = scan.ranges[ind];
49 s->
x = cos(scan.angle_min + ind * scan.angle_increment) * s->
range;
50 s->
y = sin(scan.angle_min + ind * scan.angle_increment) * s->
range;
51 if (s->
range > scan.range_min && s->
range < scan.range_max)
60 void SampleSet::clear()
62 for (SampleSet::iterator i = begin();
68 set<Sample*, CompareSample>::clear();
71 void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud,
int r,
int g,
int b)
75 int rgb = (r << 16) | (g << 8) | b;
76 color_val = *(
float*) & (rgb);
78 for (iterator sample_iter = begin();
82 geometry_msgs::Point32 point;
83 point.x = (*sample_iter)->x;
84 point.y = (*sample_iter)->y;
87 cloud.points.push_back(point);
89 if (cloud.channels[0].name ==
"rgb")
90 cloud.channels[0].values.push_back(color_val);
98 for (iterator i = begin();
103 x_mean += ((*i)->x) / size();
104 y_mean += ((*i)->y) / size();
111 void ScanMask::addScan(sensor_msgs::LaserScan& scan)
115 angle_min = scan.angle_min;
116 angle_max = scan.angle_max;
117 size = scan.ranges.size();
120 else if (angle_min != scan.angle_min ||
121 angle_max != scan.angle_max ||
122 size != scan.ranges.size())
124 throw std::runtime_error(
"laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
127 for (uint32_t i = 0; i < scan.ranges.size(); i++)
129 Sample*
s = Sample::Extract(i, scan);
133 SampleSet::iterator m = mask_.find(s);
135 if (m != mask_.end())
137 if ((*m)->range > s->
range)
157 bool ScanMask::hasSample(
Sample* s,
float thresh)
161 SampleSet::iterator m = mask_.find(s);
162 if (m != mask_.end())
163 if (((*m)->range - thresh) < s->
range)
171 ScanProcessor::ScanProcessor(
const sensor_msgs::LaserScan& scan,
ScanMask& mask_,
float mask_threshold)
177 for (uint32_t i = 0; i < scan.ranges.size(); i++)
179 Sample*
s = Sample::Extract(i, scan);
194 clusters_.push_back(cluster);
198 ScanProcessor::~ScanProcessor()
200 for (list<SampleSet*>::iterator c = clusters_.begin();
201 c != clusters_.end();
207 ScanProcessor::removeLessThan(uint32_t num)
209 list<SampleSet*>::iterator c_iter = clusters_.begin();
210 while (c_iter != clusters_.end())
212 if ((*c_iter)->size() < num)
215 clusters_.erase(c_iter++);
226 ScanProcessor::splitConnected(
float thresh)
228 list<SampleSet*> tmp_clusters;
230 list<SampleSet*>::iterator c_iter = clusters_.begin();
233 while (c_iter != clusters_.end())
236 while ((*c_iter)->size() > 0)
239 SampleSet::iterator s_first = (*c_iter)->begin();
242 list<Sample*> sample_queue;
243 sample_queue.push_back(*s_first);
245 (*c_iter)->erase(s_first);
248 list<Sample*>::iterator s_q = sample_queue.begin();
249 while (s_q != sample_queue.end())
251 int expand = (int)(asin(thresh / (*s_q)->range) / std::abs(scan_.angle_increment));
253 SampleSet::iterator s_rest = (*c_iter)->begin();
255 while ((s_rest != (*c_iter)->end() &&
256 (*s_rest)->index < (*s_q)->index + expand))
258 if ((*s_rest)->range - (*s_q)->range > thresh)
262 else if (sqrt(pow((*s_q)->x - (*s_rest)->x, 2.0f) + pow((*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
264 sample_queue.push_back(*s_rest);
265 (*c_iter)->erase(s_rest++);
278 for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
282 tmp_clusters.push_back(c);
289 clusters_.erase(c_iter++);
292 clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
A namespace containing the laser processor helper classes.
An ordered set of Samples.
A struct representing a single sample from the laser.
bool hasSample(Sample *s, float thresh)
A mask for filtering out Samples based on range.