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 #include "karto_sdk/Karto.h"
26 #include <boost/serialization/export.hpp>
50 namespace karto
51 {
52 
56 
58  {
59  static Singleton<SensorManager> sInstance;
60  return sInstance.Get();
61  }
62 
66 
68  : m_pParameterManager(new ParameterManager())
69  {
70  }
71 
72  Object::Object(const Name& rName)
73  : m_Name(rName)
75  {
76  }
77 
79  {
80  delete m_pParameterManager;
82  }
83 
87 
88  Module::Module(const std::string& rName)
89  : Object(rName)
90  {
91  }
92 
94  {
95  }
96 
100 
101  Sensor::Sensor(const Name& rName)
102  : Object(rName)
103  {
104  m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
105  }
106 
108  {
109  }
110 
114 
115  SensorData::SensorData(const Name& rSensorName)
116  : Object()
117  , m_StateId(-1)
118  , m_UniqueId(-1)
119  , m_SensorName(rSensorName)
120  , m_Time(0.0)
121  {
122  }
123 
125  {
127  {
128  delete *iter;
129  }
130 
131  m_CustomData.clear();
132  }
133 
137 
139  {
140  kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
141  kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
142  kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
143 
144  m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
145  }
146 
150 
151  std::ostream& operator << (std::ostream& rStream, Exception& rException)
152  {
153  rStream << "Error detect: " << std::endl;
154  rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
155  rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;
156 
157  return rStream;
158  }
159 
163 
165  CoordinateConverter* pCoordinateConverter,
166  kt_bool ignoreThresholdPoints,
167  kt_bool flipY) const
168  {
169  PointVectorDouble pointReadings;
170 
171  Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();
172 
173  // compute point readings
174  kt_int32u beamNum = 0;
175  kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
176  for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
177  {
178  kt_double rangeReading = pRangeReadings[i];
179 
180  if (ignoreThresholdPoints)
181  {
182  if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
183  {
184  continue;
185  }
186  }
187  else
188  {
189  rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
190  }
191 
192  kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();
193 
194  Vector2<kt_double> point;
195 
196  point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
197  point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
198 
199  if (pCoordinateConverter != NULL)
200  {
201  Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
202  point.SetX(gridPoint.GetX());
203  point.SetY(gridPoint.GetY());
204  }
205 
206  pointReadings.push_back(point);
207  }
208 
209  return pointReadings;
210  }
211 
213  {
214  LaserRangeScan* pLaserRangeScan = dynamic_cast<LaserRangeScan*>(pSensorData);
215 
216  // verify number of range readings in LaserRangeScan matches the number of expected range readings
217  if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
218  {
219  std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
220  << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
221  return false;
222  }
223 
224  return true;
225  }
226 
230 
232  {
233  forEach(karto::ParameterVector, &m_Parameters)
234  {
235  delete *iter;
236  }
237 
238  m_Parameters.clear();
239 
240  m_ParameterLookup.clear();
241  }
242 
244  {
245  if (pParameter != NULL && pParameter->GetName() != "")
246  {
247  if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
248  {
249  m_Parameters.push_back(pParameter);
250 
251  m_ParameterLookup[pParameter->GetName()] = pParameter;
252  }
253  else
254  {
255  m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());
256 
257  assert(false);
258  }
259  }
260  }
261 
265 
266  /*
267  std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
268  {
269  "Custom",
270 
271  "Sick_LMS100",
272  "Sick_LMS200",
273  "Sick_LMS291",
274 
275  "Hokuyo_UTM_30LX",
276  "Hokuyo_URG_04LX"
277  };
278  */
279 } // namespace karto
#define NULL
kt_double * GetRangeReadings() const
Definition: Karto.h:5338
virtual kt_bool Validate()
Definition: Karto.h:4150
Pose2 GetSensorPose() const
Definition: Karto.h:5621
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1281
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5404
uint8_t kt_int8u
Definition: Types.h:46
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4486
void SetX(const T &x)
Definition: Karto.h:1035
virtual ~SensorData()
Definition: Karto.cpp:124
Parameter< Pose2 > * m_pOffsetPose
Definition: Karto.h:3672
#define forEach(listtype, list)
Definition: Macros.h:66
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
virtual ~Sensor()
Definition: Karto.cpp:107
virtual ~Object()
Definition: Karto.cpp:78
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:677
Module(const std::string &rName)
Definition: Karto.cpp:88
std::vector< CustomData * > CustomDataVector
Definition: Karto.h:5114
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:138
ParameterManager * m_pParameterManager
Definition: Karto.h:714
kt_double GetX() const
Definition: Karto.h:2097
virtual ~Module()
Definition: Karto.cpp:93
virtual const std::string GetValueAsString() const =0
kt_int32s GetErrorCode()
Definition: Karto.h:153
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
const T & GetX() const
Definition: Karto.h:1026
static SensorManager * GetInstance()
Definition: Karto.cpp:57
virtual void SetValueFromString(const std::string &rStringValue)=0
bool kt_bool
Definition: Types.h:64
CustomDataVector m_CustomData
Definition: Karto.h:5258
const std::string & GetName() const
Definition: Karto.h:3154
double kt_double
Definition: Types.h:67
Definition: Karto.h:86
const T & GetY() const
Definition: Karto.h:1044
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
kt_double GetY() const
Definition: Karto.h:2115
std::ostream & operator<<(std::ostream &rStream, Exception &rException)
Definition: Karto.cpp:151
const std::string & GetErrorMessage() const
Definition: Karto.h:144
BOOST_CLASS_EXPORT_IMPLEMENT(karto::NonCopyable)
std::vector< AbstractParameter * > ParameterVector
Definition: Karto.h:277
void SetY(const T &y)
Definition: Karto.h:1053
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
uint32_t kt_int32u
Definition: Types.h:52
const PointVectorDouble GetPointReadings(LocalizedRangeScan *pLocalizedRangeScan, CoordinateConverter *pCoordinateConverter, kt_bool ignoreThresholdPoints=true, kt_bool flipY=false) const
Definition: Karto.cpp:164
Name m_Name
Definition: Karto.h:713
void Add(AbstractParameter *pParameter)
Definition: Karto.cpp:243
kt_double GetHeading() const
Definition: Karto.h:2151


slam_toolbox
Author(s): Steve Macenski
autogenerated on Sat Oct 3 2020 03:51:01