laser_processor.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #ifndef FETCH_AUTO_DOCK_LASER_PROCESSOR_H
36 #define FETCH_AUTO_DOCK_LASER_PROCESSOR_H
37 
38 #include <unistd.h>
39 #include <math.h>
40 #include "sensor_msgs/LaserScan.h"
41 #include "sensor_msgs/PointCloud.h"
42 #include "geometry_msgs/Point.h"
43 
44 #include <list>
45 #include <set>
46 #include <vector>
47 #include <map>
48 #include <utility>
49 #include <algorithm>
50 
51 #include "tf/transform_datatypes.h"
52 
53 namespace laser_processor
54 {
56 class Sample
57 {
58 public:
59  int index;
60  float range;
61  float intensity;
62  float x;
63  float y;
64 
65  static Sample* Extract(int ind, const sensor_msgs::LaserScan& scan);
66 
67 private:
68  Sample() {};
69 };
70 
73 {
75 
76  inline bool operator()(const Sample* a, const Sample* b)
77  {
78  return (a->index < b->index);
79  }
80 };
81 
82 
84 class SampleSet : public std::set<Sample*, CompareSample>
85 {
86 public:
87  SampleSet() {}
88 
90  {
91  clear();
92  }
93 
94  void clear();
95 
96  void appendToCloud(sensor_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0);
97 
98  tf::Point center();
99  std_msgs::Header header;
100 };
101 
103 class ScanMask
104 {
106 
107  bool filled;
108  float angle_min;
109  float angle_max;
110  uint32_t size;
111 
112 public:
113  ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { }
114 
115  inline void clear()
116  {
117  mask_.clear();
118  filled = false;
119  }
120 
121  void addScan(sensor_msgs::LaserScan& scan);
122 
123  bool hasSample(Sample* s, float thresh);
124 };
125 
126 
129 
131 {
132  std::list<SampleSetConstPtr> clusters_;
133  sensor_msgs::LaserScan scan_;
134 
135 public:
136  std::list<SampleSetConstPtr>& getClusters()
137  {
138  return clusters_;
139  }
140 
141  ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03);
142 
143  ~ScanProcessor();
144 
145  void removeLessThan(uint32_t num);
146 
147  void splitConnected(float thresh);
148 };
149 }; // namespace laser_processor
150 
151 #endif // FETCH_AUTO_DOCK_LASER_PROCESSOR_H
std::list< SampleSetConstPtr > clusters_
XmlRpcServer s
SampleSet * SampleSetConstPtr
static Sample * Extract(int ind, const sensor_msgs::LaserScan &scan)
sensor_msgs::LaserScan scan_
An ordered set of Samples.
A struct representing a single sample from the laser.
bool operator()(const Sample *a, const Sample *b)
std::list< SampleSetConstPtr > & getClusters()
The comparator allowing the creation of an ordered "SampleSet".
A mask for filtering out Samples based on range.
SampleSet * SampleSetPtr


caster_app
Author(s): Ye Tian
autogenerated on Wed Dec 18 2019 03:34:44