Karto.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010 SRI International
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 <math.h>
19 #include <assert.h>
20 #include <float.h>
21 #include <limits.h>
22 #include <stdlib.h>
23 #include <string.h>
24 #include <stdio.h>
25 
26 #include "open_karto/Karto.h"
27 
28 namespace karto
29 {
30 
34 
36  {
37  static Singleton<SensorManager> sInstance;
38  return sInstance.Get();
39  }
40 
44 
46  : m_pParameterManager(new ParameterManager())
47  {
48  }
49 
50  Object::Object(const Name& rName)
51  : m_Name(rName)
53  {
54  }
55 
57  {
58  delete m_pParameterManager;
59  m_pParameterManager = NULL;
60  }
61 
65 
66  Module::Module(const std::string& rName)
67  : Object(rName)
68  {
69  }
70 
72  {
73  }
74 
78 
79  Sensor::Sensor(const Name& rName)
80  : Object(rName)
81  {
82  m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
83  }
84 
86  {
87  }
88 
92 
93  SensorData::SensorData(const Name& rSensorName)
94  : Object()
95  , m_StateId(-1)
96  , m_UniqueId(-1)
97  , m_SensorName(rSensorName)
98  , m_Time(0.0)
99  {
100  }
101 
103  {
105  {
106  delete *iter;
107  }
108 
109  m_CustomData.clear();
110  }
111 
115 
117  {
118  kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
119  kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
120  kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
121 
122  m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
123  }
124 
128 
129  std::ostream& operator << (std::ostream& rStream, Exception& rException)
130  {
131  rStream << "Error detect: " << std::endl;
132  rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
133  rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;
134 
135  return rStream;
136  }
137 
141 
143  CoordinateConverter* pCoordinateConverter,
144  kt_bool ignoreThresholdPoints,
145  kt_bool flipY) const
146  {
147  PointVectorDouble pointReadings;
148 
149  Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();
150 
151  // compute point readings
152  kt_int32u beamNum = 0;
153  kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
154  for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
155  {
156  kt_double rangeReading = pRangeReadings[i];
157 
158  if (ignoreThresholdPoints)
159  {
160  if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
161  {
162  continue;
163  }
164  }
165  else
166  {
167  rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
168  }
169 
170  kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();
171 
172  Vector2<kt_double> point;
173 
174  point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
175  point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
176 
177  if (pCoordinateConverter != NULL)
178  {
179  Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
180  point.SetX(gridPoint.GetX());
181  point.SetY(gridPoint.GetY());
182  }
183 
184  pointReadings.push_back(point);
185  }
186 
187  return pointReadings;
188  }
189 
191  {
192  LaserRangeScan* pLaserRangeScan = dynamic_cast<LaserRangeScan*>(pSensorData);
193 
194  // verify number of range readings in LaserRangeScan matches the number of expected range readings
195  if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
196  {
197  std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
198  << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
199  return false;
200  }
201 
202  return true;
203  }
204 
208 
210  {
211  forEach(karto::ParameterVector, &m_Parameters)
212  {
213  delete *iter;
214  }
215 
216  m_Parameters.clear();
217 
218  m_ParameterLookup.clear();
219  }
220 
222  {
223  if (pParameter != NULL && pParameter->GetName() != "")
224  {
225  if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
226  {
227  m_Parameters.push_back(pParameter);
228 
229  m_ParameterLookup[pParameter->GetName()] = pParameter;
230  }
231  else
232  {
233  m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());
234 
235  assert(false);
236  }
237  }
238  }
239 
243 
244  /*
245  std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
246  {
247  "Custom",
248 
249  "Sick_LMS100",
250  "Sick_LMS200",
251  "Sick_LMS291",
252 
253  "Hokuyo_UTM_30LX",
254  "Hokuyo_URG_04LX"
255  };
256  */
257 } // namespace karto
virtual kt_bool Validate()
Definition: Karto.h:3926
kt_double GetHeading() const
Definition: Karto.h:2064
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1204
const PointVectorDouble GetPointReadings(LocalizedRangeScan *pLocalizedRangeScan, CoordinateConverter *pCoordinateConverter, kt_bool ignoreThresholdPoints=true, kt_bool flipY=false) const
Definition: Karto.cpp:142
const T & GetY() const
Definition: Karto.h:975
uint8_t kt_int8u
Definition: Types.h:46
kt_double GetX() const
Definition: Karto.h:2010
void SetX(const T &x)
Definition: Karto.h:966
const std::string & GetName() const
Definition: Karto.h:3029
Sensor(const Name &rName)
Definition: Karto.cpp:79
virtual ~SensorData()
Definition: Karto.cpp:102
Parameter< Pose2 > * m_pOffsetPose
Definition: Karto.h:3493
#define forEach(listtype, list)
Definition: Macros.h:66
SensorData(const Name &rSensorName)
Definition: Karto.cpp:93
virtual ~Sensor()
Definition: Karto.cpp:85
const T & GetX() const
Definition: Karto.h:957
Pose2 GetSensorPose() const
Definition: Karto.h:5267
const std::string & GetErrorMessage() const
Definition: Karto.h:131
virtual ~Object()
Definition: Karto.cpp:56
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:635
Module(const std::string &rName)
Definition: Karto.cpp:66
std::vector< CustomData * > CustomDataVector
Definition: Karto.h:4802
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:124
virtual void operator()(kt_int32u index)
Definition: Karto.cpp:116
ParameterManager * m_pParameterManager
Definition: Karto.h:673
virtual ~Module()
Definition: Karto.cpp:71
virtual const std::string GetValueAsString() const =0
kt_int32s GetErrorCode()
Definition: Karto.h:140
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
static SensorManager * GetInstance()
Definition: Karto.cpp:35
virtual void SetValueFromString(const std::string &rStringValue)=0
bool kt_bool
Definition: Types.h:64
CustomDataVector m_CustomData
Definition: Karto.h:4944
kt_double GetY() const
Definition: Karto.h:2028
double kt_double
Definition: Types.h:67
Definition: Karto.h:73
std::ostream & operator<<(std::ostream &rStream, Exception &rException)
Definition: Karto.cpp:129
std::vector< AbstractParameter * > ParameterVector
Definition: Karto.h:258
kt_double * GetRangeReadings() const
Definition: Karto.h:5006
void SetY(const T &y)
Definition: Karto.h:984
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5072
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4218
uint32_t kt_int32u
Definition: Types.h:52
Name m_Name
Definition: Karto.h:672
void Add(AbstractParameter *pParameter)
Definition: Karto.cpp:221


open_karto
Author(s):
autogenerated on Mon Jun 10 2019 14:02:18