Sensor.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <iostream>
19 
20 #include <OpenKarto/StringHelper.h>
22 #include <OpenKarto/Sensor.h>
23 #include <OpenKarto/SensorData.h>
25 #include <OpenKarto/Logger.h>
26 
27 namespace karto
28 {
29 
33 
35  {
36  MetaEnum::Register<LaserRangeFinderType>("LaserRangeFinderType")
37  .Value("Custom", LaserRangeFinder_Custom)
38  .Value("Sick_LMS100", LaserRangeFinder_Sick_LMS100)
39  .Value("Sick_LMS200", LaserRangeFinder_Sick_LMS200)
40  .Value("Sick_LMS291", LaserRangeFinder_Sick_LMS291)
41  .Value("Hokuyo_UTM_30LX", LaserRangeFinder_Hokuyo_UTM_30LX)
42  .Value("Hokuyo_URG_04LX", LaserRangeFinder_Hokuyo_URG_04LX);
43  }
44 
48 
49  Sensor::Sensor(const Identifier& rIdentifier)
50  : Object(rIdentifier)
51  {
52  m_pOffsetPose = new Parameter< karto::Pose2 >(GetParameterSet(), "OffsetPose", "Offset", "", karto::Pose2());
53 
55  }
56 
57  Sensor::~Sensor()
58  {
60  }
61 
65 
67  : Sensor(rName)
68  , m_NumberOfRangeReadings(0)
69  {
70  m_pMinimumRange = new Parameter<kt_double>(GetParameterSet(), "MinimumRange", "Minimum Range", "", 0.0);
71  m_pMaximumRange = new Parameter<kt_double>(GetParameterSet(), "MaximumRange", "Maximum Range", "", 80.0);
72 
73  m_pMinimumAngle = new Parameter<kt_double>(GetParameterSet(), "MinimumAngle", "Minimum Angle", "", -KT_PI_2);
74  m_pMaximumAngle = new Parameter<kt_double>(GetParameterSet(), "MaximumAngle", "Minimum Angle", "", KT_PI_2);
75 
76  m_pAngularResolution = new Parameter<kt_double>(GetParameterSet(), "AngularResolution", "Angular Resolution", "", math::DegreesToRadians(1));
77 
78  m_pRangeThreshold = new Parameter<kt_double>(GetParameterSet(), "RangeThreshold", "Range Threshold", "", 12.0);
79 
81  const MetaEnum& rMetaEnum = karto::GetMetaEnumByType<karto::LaserRangeFinderType>();
82  for (kt_size_t i = 0; i < rMetaEnum.GetSize(); i++)
83  {
84  const EnumPair& rEnumPair = rMetaEnum.GetPair(i);
85  m_pType->DefineEnumValue(rEnumPair.name, rEnumPair.value);
86  }
87  }
88 
89  LaserRangeFinder::~LaserRangeFinder()
90  {
91  }
92 
93  const Vector2dList LaserRangeFinder::GetPointReadings(LocalizedLaserScan* pLocalizedLaserScan, CoordinateConverter* pCoordinateConverter, kt_bool ignoreThresholdPoints, kt_bool flipY) const
94  {
95  Vector2dList pointReadings;
96 
97  Vector2d scanPosition = pLocalizedLaserScan->GetSensorPose().GetPosition();
98 
99  // compute point readings
100  const Vector2dList& rPoints = pLocalizedLaserScan->GetPointReadings(ignoreThresholdPoints);
101  for (kt_int32u i = 0; i < rPoints.Size(); i++)
102  {
103  Vector2d point = rPoints[i];
104 
105  kt_double range = scanPosition.Distance(point);
106  kt_double clippedRange = math::Clip(range, GetMinimumRange(), GetRangeThreshold());
107  if (karto::math::DoubleEqual(range, clippedRange) == false)
108  {
109  kt_double ratio = clippedRange / range;
110  point.SetX(scanPosition.GetX() + ratio * (point.GetX() - scanPosition.GetX()));
111  point.SetY(scanPosition.GetY() + ratio * (point.GetY() - scanPosition.GetY()));
112  }
113 
114  if (pCoordinateConverter != NULL)
115  {
116  Vector2i gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
117  point.SetX(gridPoint.GetX());
118  point.SetY(gridPoint.GetY());
119  }
120 
121  pointReadings.Add(point);
122  }
123 
124  return pointReadings;
125  }
126 
128  {
129  // make sure rangeThreshold is within laser range finder range
131 
132  if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
133  {
134  Log(LOG_INFORMATION, "Info: clipped range threshold to be within minimum and maximum range!");
135  }
136  }
137 
139  {
141  {
142  m_pAngularResolution->SetValue(angularResolution);
143  }
145  {
146  if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
147  {
149  }
150  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
151  {
153  }
154  else
155  {
156  String errorMessage;
157  errorMessage.Append("Invalid resolution for Sick LMS100: ");
158  errorMessage.Append(karto::StringHelper::ToString(angularResolution));
159  throw Exception(errorMessage);
160  }
161  }
163  {
164  if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
165  {
167  }
168  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
169  {
171  }
172  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
173  {
175  }
176  else
177  {
178  String errorMessage;
179  errorMessage.Append("Invalid resolution for Sick LMS291: ");
180  errorMessage.Append(karto::StringHelper::ToString(angularResolution));
181  throw Exception(errorMessage);
182  }
183  }
184  else
185  {
186  throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
187  }
188 
189  Update();
190  }
191 
193  {
194  Update();
195 
196  // check if min < max range!
197  if (GetMinimumRange() >= GetMaximumRange())
198  {
199  assert(false);
200  throw Exception("LaserRangeFinder::Validate() - MinimumRange must be less than MaximumRange. Please set MinimumRange and MaximumRange to valid values.");
201  }
202 
203  // set range threshold to valid value
205  {
207  Log(LOG_INFORMATION, String("Updating RangeThreshold from ") + StringHelper::ToString(GetRangeThreshold()) + " to " + StringHelper::ToString(newValue));
208  SetRangeThreshold(newValue);
209  }
210  }
211 
213  {
214  LocalizedRangeScan* pScan = dynamic_cast<LocalizedRangeScan*>(pSensorData);
215 
216  // verify number of range readings in laser scan matches the number of expected range
217  // readings; validation only valid with LocalizedRangeScan (LocalizedPointScan may have
218  // variable number of readings)
219  if (pScan != NULL && (pScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()))
220  {
221  StringBuilder errorMessage;
222  errorMessage << "LaserRangeFinder::Validate() - LocalizedRangeScan contains " << pScan->GetNumberOfRangeReadings() << " range readings, expected " << GetNumberOfRangeReadings();
223 
224  // assert(false);
225  throw Exception(errorMessage.ToString());
226  }
227  }
228 
230  {
231  LaserRangeFinder* pLrf = NULL;
232 
233  switch(type)
234  {
235  // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
236  // set range threshold to 18m
238  {
239  pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 100");
240 
241  // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
242  pLrf->m_pMinimumRange->SetValue(0.0);
243  pLrf->m_pMaximumRange->SetValue(20.0);
244 
245  // 270 degree range, 50 Hz
248 
249  // 0.25 degree angular resolution
251 
252  pLrf->m_NumberOfRangeReadings = 1081;
253  }
254  break;
255 
256  // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
257  // set range threshold to 10m
259  {
260  pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 200");
261 
262  // Sensing range is 80 meters
263  pLrf->m_pMinimumRange->SetValue(0.0);
264  pLrf->m_pMaximumRange->SetValue(80.0);
265 
266  // 180 degree range, 75 Hz
269 
270  // 0.5 degree angular resolution
272 
273  pLrf->m_NumberOfRangeReadings = 361;
274  }
275  break;
276 
277  // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
278  // set range threshold to 30m
280  {
281  pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 291");
282 
283  // Sensing range is 80 meters
284  pLrf->m_pMinimumRange->SetValue(0.0);
285  pLrf->m_pMaximumRange->SetValue(80.0);
286 
287  // 180 degree range, 75 Hz
290 
291  // 0.5 degree angular resolution
293 
294  pLrf->m_NumberOfRangeReadings = 361;
295  }
296  break;
297 
298  // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
299  // set range threshold to 30m
301  {
302  pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Hokuyo UTM-30LX");
303 
304  // Sensing range is 30 meters
305  pLrf->m_pMinimumRange->SetValue(0.1);
306  pLrf->m_pMaximumRange->SetValue(30.0);
307 
308  // 270 degree range, 40 Hz
311 
312  // 0.25 degree angular resolution
314 
315  pLrf->m_NumberOfRangeReadings = 1081;
316  }
317  break;
318 
319  // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
320  // set range threshold to 3.5m
322  {
323  pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Hokuyo URG-04LX");
324 
325  // Sensing range is 4 meters. It has detection problems when scanning absorptive surfaces (such as black trimming).
326  pLrf->m_pMinimumRange->SetValue(0.02);
327  pLrf->m_pMaximumRange->SetValue(4.0);
328 
329  // 240 degree range, 10 Hz
332 
333  // 0.352 degree angular resolution
335 
336  pLrf->m_NumberOfRangeReadings = 751;
337  }
338  break;
339 
341  {
342  pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "User-Defined LaserRangeFinder");
343 
344  // Sensing range is 80 meters.
345  pLrf->m_pMinimumRange->SetValue(0.0);
346  pLrf->m_pMaximumRange->SetValue(80.0);
347 
348  // 180 degree range
351 
352  // 1.0 degree angular resolution
354 
355  pLrf->m_NumberOfRangeReadings = 181;
356  }
357  break;
358  }
359 
360  if (pLrf != NULL)
361  {
362  pLrf->m_pType->SetValue(type);
363 
364  Pose2 defaultOffset;
365  pLrf->SetOffsetPose(defaultOffset);
366  }
367 
368  return pLrf;
369  }
370 
371 }
372 
void Append(const String &rString)
Definition: String.cpp:82
virtual kt_size_t Size() const
Definition: List.h:214
kt_double Distance(const Vector2 &rOther) const
Definition: Geometry.h:443
bool kt_bool
Definition: Types.h:145
kt_int32u GetNumberOfRangeReadings() const
Definition: Sensor.h:383
std::size_t kt_size_t
Definition: Types.h:138
const EnumPair & GetPair(kt_size_t index) const
Definition: MetaEnum.cpp:107
virtual const T & GetValue() const
Definition: Parameter.h:483
Sensor(const Identifier &rName)
Definition: Sensor.cpp:49
LaserRangeFinder(const Identifier &rName)
Definition: Sensor.cpp:66
void SetRangeThreshold(kt_double rangeThreshold)
Definition: Sensor.cpp:127
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Identifier &rName)
Definition: Sensor.cpp:229
kt_size_t GetSize() const
Definition: MetaEnum.cpp:92
const Vector2dList GetPointReadings(LocalizedLaserScan *pLocalizedLaserScan, CoordinateConverter *pCoordinateConverter, kt_bool ignoreThresholdPoints=true, kt_bool flipY=false) const
Definition: Sensor.cpp:93
kt_int32u m_NumberOfRangeReadings
Definition: Sensor.h:451
const T & GetY() const
Definition: Geometry.h:369
void SetX(const T &x)
Definition: Geometry.h:360
Parameter< kt_double > * m_pMinimumAngle
Definition: Sensor.h:439
void RegisterSensor(Sensor *pSensor)
virtual void Add(const T &rValue)
Definition: List.h:111
LaserRangeFinderType
Definition: Sensor.h:193
Parameter< Pose2 > * m_pOffsetPose
Definition: Sensor.h:103
Pose2 GetSensorPose() const
Definition: SensorData.h:627
void SetOffsetPose(const Pose2 &rPose)
Definition: Sensor.h:78
Parameter< kt_double > * m_pAngularResolution
Definition: Sensor.h:442
void UnregisterSensor(Sensor *pSensor)
const kt_double KT_PI_2
Definition: Math.h:53
const T & GetX() const
Definition: Geometry.h:351
virtual void SetValue(const T &rValue)
Definition: Parameter.h:492
void SetAngularResolution(kt_double angularResolution)
Definition: Sensor.cpp:138
void Log(LogLevel level, const karto::String &rMessage)
Definition: Logger.cpp:183
uint32_t kt_int32u
Definition: Types.h:111
karto::String name
Definition: MetaEnum.h:58
static String ToString(const char *value)
Parameter< kt_double > * m_pRangeThreshold
Definition: Sensor.h:447
Vector2i WorldToGrid(const Vector2d &rWorld, kt_bool flipY=false) const
Parameter< kt_double > * m_pMinimumRange
Definition: Sensor.h:444
const String & ToString() const
Definition: Identifier.cpp:93
const String & ToString() const
Parameter< kt_double > * m_pMaximumRange
Definition: Sensor.h:445
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:151
void DefineEnumValue(const String &rName, kt_int64s value)
Definition: Parameter.cpp:219
kt_int64s value
Definition: MetaEnum.h:63
ParameterEnum * m_pType
Definition: Sensor.h:449
static SensorRegistry * GetInstance()
double kt_double
Definition: Types.h:160
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:83
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:203
Parameter< kt_double > * m_pMaximumAngle
Definition: Sensor.h:440
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:162
signed long long kt_int64s
Definition: Types.h:127
ParameterSet * GetParameterSet()
Definition: Object.h:144
virtual void Validate()
Definition: Sensor.cpp:192
Definition: Any.cpp:20
KARTO_EXPORT void RegisterLaserRangeFinderType()
Definition: Sensor.cpp:34
kt_size_t GetNumberOfRangeReadings() const
Definition: SensorData.h:697
kt_double GetMinimumRange() const
Definition: Sensor.h:264
void SetY(const T &y)
Definition: Geometry.h:378
kt_double GetRangeThreshold() const
Definition: Sensor.h:304
kt_double GetMaximumRange() const
Definition: Sensor.h:284
const Vector2dList & GetPointReadings(kt_bool wantFiltered=false) const
Definition: SensorData.cpp:152


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24