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)
74  , m_pParameterManager(new ParameterManager())
75  {
76  }
77 
79  {
80  delete m_pParameterManager;
81  m_pParameterManager = NULL;
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  {
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  {
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
karto::Name
Definition: Karto.h:389
karto::LaserRangeFinder::GetMinimumRange
kt_double GetMinimumRange() const
Definition: Karto.h:3945
karto::Singleton
Definition: Karto.h:211
karto::CellUpdater::operator()
virtual void operator()(kt_int32u index)
Definition: Karto.cpp:138
karto::SensorData::~SensorData
virtual ~SensorData()
Definition: Karto.cpp:124
kt_double
double kt_double
Definition: Types.h:67
karto::NonCopyable
Definition: Karto.h:181
karto::CoordinateConverter::WorldToGrid
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4488
karto::LaserRangeFinder
Definition: Karto.h:3922
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
karto::LocalizedRangeScan::GetSensorPose
Pose2 GetSensorPose() const
Definition: Karto.h:5634
karto::Module
Definition: Karto.h:818
karto::Module::Module
Module(const std::string &rName)
Definition: Karto.cpp:88
karto::Exception::GetErrorMessage
const std::string & GetErrorMessage() const
Definition: Karto.h:146
karto::Pose2::GetY
kt_double GetY() const
Definition: Karto.h:2117
karto::LaserRangeFinder::m_NumberOfRangeReadings
kt_int32u m_NumberOfRangeReadings
Definition: Karto.h:4398
karto::LookupArray
Definition: Karto.h:6747
karto::LaserRangeFinder::GetRangeThreshold
kt_double GetRangeThreshold() const
Definition: Karto.h:3985
karto::Vector2::GetX
const T & GetX() const
Definition: Karto.h:1028
karto::Exception::GetErrorCode
kt_int32s GetErrorCode()
Definition: Karto.h:155
karto::LocalizedRangeScan
Definition: Karto.h:5505
karto::AbstractParameter::GetValueAsString
virtual const std::string GetValueAsString() const =0
karto::ParameterManager::m_Parameters
ParameterVector m_Parameters
Definition: Karto.h:365
karto::Parameters
Definition: Karto.h:3552
karto::SensorData
Definition: Karto.h:5125
karto::Sensor::~Sensor
virtual ~Sensor()
Definition: Karto.cpp:107
karto::OccupancyGrid::m_pCellHitsCnt
Grid< kt_int32u > * m_pCellHitsCnt
Definition: Karto.h:6426
karto::Sensor::m_pOffsetPose
Parameter< Pose2 > * m_pOffsetPose
Definition: Karto.h:3674
karto::math::Clip
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:124
Karto.h
karto::OccupancyGrid::UpdateCell
virtual void UpdateCell(kt_int8u *pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
Definition: Karto.h:6366
karto::Object::Object
Object()
Definition: Karto.cpp:67
karto::ParameterManager::Clear
void Clear()
Definition: Karto.cpp:231
karto::Vector2::GetY
const T & GetY() const
Definition: Karto.h:1046
karto::LaserRangeFinder::GetMinimumAngle
kt_double GetMinimumAngle() const
Definition: Karto.h:4009
karto::SensorManager::GetInstance
static SensorManager * GetInstance()
Definition: Karto.cpp:57
karto::LaserRangeFinder::GetPointReadings
const PointVectorDouble GetPointReadings(LocalizedRangeScan *pLocalizedRangeScan, CoordinateConverter *pCoordinateConverter, kt_bool ignoreThresholdPoints=true, kt_bool flipY=false) const
Definition: Karto.cpp:164
karto::Grid::GetDataPointer
T * GetDataPointer(const Vector2< kt_int32s > &rGrid)
Definition: Karto.h:4822
karto::Dataset
Definition: Karto.h:6549
karto::Module::~Module
virtual ~Module()
Definition: Karto.cpp:93
karto::SensorData::m_CustomData
CustomDataVector m_CustomData
Definition: Karto.h:5260
karto::Parameter< kt_double >
karto::Singleton::Get
T * Get()
Definition: Karto.h:234
karto::CoordinateConverter
Definition: Karto.h:4460
karto::LaserRangeScan::GetNumberOfRangeReadings
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5406
karto::Object::~Object
virtual ~Object()
Definition: Karto.cpp:78
karto::CustomDataVector
std::vector< CustomData * > CustomDataVector
Definition: Karto.h:5116
kt_bool
bool kt_bool
Definition: Types.h:64
karto::ParameterEnum
Definition: Karto.h:3411
karto::LaserRangeScan::GetRangeReadings
kt_double * GetRangeReadings() const
Definition: Karto.h:5340
karto::math::InRange
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
karto::CellUpdater::m_pOccupancyGrid
OccupancyGrid * m_pOccupancyGrid
Definition: Karto.h:6002
karto::PointVectorDouble
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1283
karto::LaserRangeFinder::Validate
virtual kt_bool Validate()
Definition: Karto.h:4152
karto::Vector2< kt_double >
karto::LaserRangeFinder::GetNumberOfRangeReadings
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:4125
karto::AbstractParameter
Definition: Karto.h:3103
karto::Vector2::SetX
void SetX(const T &x)
Definition: Karto.h:1037
karto::Object::GetParameterManager
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:679
karto::Exception
Definition: Karto.h:99
karto::AbstractParameter::GetName
const std::string & GetName() const
Definition: Karto.h:3156
BOOST_CLASS_EXPORT_IMPLEMENT
BOOST_CLASS_EXPORT_IMPLEMENT(karto::NonCopyable)
karto::Object::m_pParameterManager
ParameterManager * m_pParameterManager
Definition: Karto.h:716
karto::CustomData
Definition: Karto.h:5064
kt_int32u
uint32_t kt_int32u
Definition: Types.h:52
karto::Pose2::GetX
kt_double GetX() const
Definition: Karto.h:2099
karto::Vector2::SetY
void SetY(const T &y)
Definition: Karto.h:1055
karto::LaserRangeFinder::GetAngularResolution
kt_double GetAngularResolution() const
Definition: Karto.h:4049
karto::SensorManager
Definition: Karto.h:3701
karto::ParameterManager::Add
void Add(AbstractParameter *pParameter)
Definition: Karto.cpp:243
forEach
#define forEach(listtype, list)
Definition: Macros.h:66
karto::Pose2
Definition: Karto.h:2046
karto::ParameterVector
std::vector< AbstractParameter * > ParameterVector
Definition: Karto.h:279
karto::Object
Definition: Karto.h:634
assert.h
kt_int8u
uint8_t kt_int8u
Definition: Types.h:46
karto::LaserRangeScan
Definition: Karto.h:5288
karto::SensorData::SensorData
SensorData()
Definition: Karto.h:5134
karto::Sensor
Definition: Karto.h:3600
karto::operator<<
std::ostream & operator<<(std::ostream &rStream, Exception &rException)
Definition: Karto.cpp:151
karto::ParameterManager
Definition: Karto.h:289
karto::ParameterManager::m_ParameterLookup
std::map< std::string, AbstractParameter * > m_ParameterLookup
Definition: Karto.h:366
karto::Pose2::GetHeading
kt_double GetHeading() const
Definition: Karto.h:2153
karto::Sensor::Sensor
Sensor()
Definition: Karto.h:3607
karto::OccupancyGrid::m_pCellPassCnt
Grid< kt_int32u > * m_pCellPassCnt
Definition: Karto.h:6421
karto
Definition: Karto.h:88


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55