00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <iostream>
00019
00020 #include <OpenKarto/StringHelper.h>
00021 #include <OpenKarto/CoordinateConverter.h>
00022 #include <OpenKarto/Sensor.h>
00023 #include <OpenKarto/SensorData.h>
00024 #include <OpenKarto/SensorRegistry.h>
00025 #include <OpenKarto/Logger.h>
00026
00027 namespace karto
00028 {
00029
00033
00034 void RegisterLaserRangeFinderType()
00035 {
00036 MetaEnum::Register<LaserRangeFinderType>("LaserRangeFinderType")
00037 .Value("Custom", LaserRangeFinder_Custom)
00038 .Value("Sick_LMS100", LaserRangeFinder_Sick_LMS100)
00039 .Value("Sick_LMS200", LaserRangeFinder_Sick_LMS200)
00040 .Value("Sick_LMS291", LaserRangeFinder_Sick_LMS291)
00041 .Value("Hokuyo_UTM_30LX", LaserRangeFinder_Hokuyo_UTM_30LX)
00042 .Value("Hokuyo_URG_04LX", LaserRangeFinder_Hokuyo_URG_04LX);
00043 }
00044
00048
00049 Sensor::Sensor(const Identifier& rIdentifier)
00050 : Object(rIdentifier)
00051 {
00052 m_pOffsetPose = new Parameter< karto::Pose2 >(GetParameterSet(), "OffsetPose", "Offset", "", karto::Pose2());
00053
00054 SensorRegistry::GetInstance()->RegisterSensor(this);
00055 }
00056
00057 Sensor::~Sensor()
00058 {
00059 SensorRegistry::GetInstance()->UnregisterSensor(this);
00060 }
00061
00065
00066 LaserRangeFinder::LaserRangeFinder(const Identifier& rName)
00067 : Sensor(rName)
00068 , m_NumberOfRangeReadings(0)
00069 {
00070 m_pMinimumRange = new Parameter<kt_double>(GetParameterSet(), "MinimumRange", "Minimum Range", "", 0.0);
00071 m_pMaximumRange = new Parameter<kt_double>(GetParameterSet(), "MaximumRange", "Maximum Range", "", 80.0);
00072
00073 m_pMinimumAngle = new Parameter<kt_double>(GetParameterSet(), "MinimumAngle", "Minimum Angle", "", -KT_PI_2);
00074 m_pMaximumAngle = new Parameter<kt_double>(GetParameterSet(), "MaximumAngle", "Minimum Angle", "", KT_PI_2);
00075
00076 m_pAngularResolution = new Parameter<kt_double>(GetParameterSet(), "AngularResolution", "Angular Resolution", "", math::DegreesToRadians(1));
00077
00078 m_pRangeThreshold = new Parameter<kt_double>(GetParameterSet(), "RangeThreshold", "Range Threshold", "", 12.0);
00079
00080 m_pType = new ParameterEnum(GetParameterSet(), "Type", "Type", "", (kt_int64s)karto::LaserRangeFinder_Custom);
00081 const MetaEnum& rMetaEnum = karto::GetMetaEnumByType<karto::LaserRangeFinderType>();
00082 for (kt_size_t i = 0; i < rMetaEnum.GetSize(); i++)
00083 {
00084 const EnumPair& rEnumPair = rMetaEnum.GetPair(i);
00085 m_pType->DefineEnumValue(rEnumPair.name, rEnumPair.value);
00086 }
00087 }
00088
00089 LaserRangeFinder::~LaserRangeFinder()
00090 {
00091 }
00092
00093 const Vector2dList LaserRangeFinder::GetPointReadings(LocalizedLaserScan* pLocalizedLaserScan, CoordinateConverter* pCoordinateConverter, kt_bool ignoreThresholdPoints, kt_bool flipY) const
00094 {
00095 Vector2dList pointReadings;
00096
00097 Vector2d scanPosition = pLocalizedLaserScan->GetSensorPose().GetPosition();
00098
00099
00100 const Vector2dList& rPoints = pLocalizedLaserScan->GetPointReadings(ignoreThresholdPoints);
00101 for (kt_int32u i = 0; i < rPoints.Size(); i++)
00102 {
00103 Vector2d point = rPoints[i];
00104
00105 kt_double range = scanPosition.Distance(point);
00106 kt_double clippedRange = math::Clip(range, GetMinimumRange(), GetRangeThreshold());
00107 if (karto::math::DoubleEqual(range, clippedRange) == false)
00108 {
00109 kt_double ratio = clippedRange / range;
00110 point.SetX(scanPosition.GetX() + ratio * (point.GetX() - scanPosition.GetX()));
00111 point.SetY(scanPosition.GetY() + ratio * (point.GetY() - scanPosition.GetY()));
00112 }
00113
00114 if (pCoordinateConverter != NULL)
00115 {
00116 Vector2i gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
00117 point.SetX(gridPoint.GetX());
00118 point.SetY(gridPoint.GetY());
00119 }
00120
00121 pointReadings.Add(point);
00122 }
00123
00124 return pointReadings;
00125 }
00126
00127 void LaserRangeFinder::SetRangeThreshold(kt_double rangeThreshold)
00128 {
00129
00130 m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
00131
00132 if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
00133 {
00134 Log(LOG_INFORMATION, "Info: clipped range threshold to be within minimum and maximum range!");
00135 }
00136 }
00137
00138 void LaserRangeFinder::SetAngularResolution(kt_double angularResolution)
00139 {
00140 if (m_pType->GetValue() == LaserRangeFinder_Custom)
00141 {
00142 m_pAngularResolution->SetValue(angularResolution);
00143 }
00144 else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
00145 {
00146 if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
00147 {
00148 m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
00149 }
00150 else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
00151 {
00152 m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
00153 }
00154 else
00155 {
00156 String errorMessage;
00157 errorMessage.Append("Invalid resolution for Sick LMS100: ");
00158 errorMessage.Append(karto::StringHelper::ToString(angularResolution));
00159 throw Exception(errorMessage);
00160 }
00161 }
00162 else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 || m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
00163 {
00164 if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
00165 {
00166 m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
00167 }
00168 else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
00169 {
00170 m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
00171 }
00172 else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
00173 {
00174 m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
00175 }
00176 else
00177 {
00178 String errorMessage;
00179 errorMessage.Append("Invalid resolution for Sick LMS291: ");
00180 errorMessage.Append(karto::StringHelper::ToString(angularResolution));
00181 throw Exception(errorMessage);
00182 }
00183 }
00184 else
00185 {
00186 throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
00187 }
00188
00189 Update();
00190 }
00191
00192 void LaserRangeFinder::Validate()
00193 {
00194 Update();
00195
00196
00197 if (GetMinimumRange() >= GetMaximumRange())
00198 {
00199 assert(false);
00200 throw Exception("LaserRangeFinder::Validate() - MinimumRange must be less than MaximumRange. Please set MinimumRange and MaximumRange to valid values.");
00201 }
00202
00203
00204 if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
00205 {
00206 kt_double newValue = karto::math::Clip(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange());
00207 Log(LOG_INFORMATION, String("Updating RangeThreshold from ") + StringHelper::ToString(GetRangeThreshold()) + " to " + StringHelper::ToString(newValue));
00208 SetRangeThreshold(newValue);
00209 }
00210 }
00211
00212 void LaserRangeFinder::Validate(SensorData* pSensorData)
00213 {
00214 LocalizedRangeScan* pScan = dynamic_cast<LocalizedRangeScan*>(pSensorData);
00215
00216
00217
00218
00219 if (pScan != NULL && (pScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()))
00220 {
00221 StringBuilder errorMessage;
00222 errorMessage << "LaserRangeFinder::Validate() - LocalizedRangeScan contains " << pScan->GetNumberOfRangeReadings() << " range readings, expected " << GetNumberOfRangeReadings();
00223
00224
00225 throw Exception(errorMessage.ToString());
00226 }
00227 }
00228
00229 LaserRangeFinder* LaserRangeFinder::CreateLaserRangeFinder(LaserRangeFinderType type, const Identifier& rName)
00230 {
00231 LaserRangeFinder* pLrf = NULL;
00232
00233 switch(type)
00234 {
00235
00236
00237 case LaserRangeFinder_Sick_LMS100:
00238 {
00239 pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 100");
00240
00241
00242 pLrf->m_pMinimumRange->SetValue(0.0);
00243 pLrf->m_pMaximumRange->SetValue(20.0);
00244
00245
00246 pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
00247 pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
00248
00249
00250 pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
00251
00252 pLrf->m_NumberOfRangeReadings = 1081;
00253 }
00254 break;
00255
00256
00257
00258 case LaserRangeFinder_Sick_LMS200:
00259 {
00260 pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 200");
00261
00262
00263 pLrf->m_pMinimumRange->SetValue(0.0);
00264 pLrf->m_pMaximumRange->SetValue(80.0);
00265
00266
00267 pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
00268 pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
00269
00270
00271 pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
00272
00273 pLrf->m_NumberOfRangeReadings = 361;
00274 }
00275 break;
00276
00277
00278
00279 case LaserRangeFinder_Sick_LMS291:
00280 {
00281 pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 291");
00282
00283
00284 pLrf->m_pMinimumRange->SetValue(0.0);
00285 pLrf->m_pMaximumRange->SetValue(80.0);
00286
00287
00288 pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
00289 pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
00290
00291
00292 pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
00293
00294 pLrf->m_NumberOfRangeReadings = 361;
00295 }
00296 break;
00297
00298
00299
00300 case LaserRangeFinder_Hokuyo_UTM_30LX:
00301 {
00302 pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Hokuyo UTM-30LX");
00303
00304
00305 pLrf->m_pMinimumRange->SetValue(0.1);
00306 pLrf->m_pMaximumRange->SetValue(30.0);
00307
00308
00309 pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
00310 pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
00311
00312
00313 pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
00314
00315 pLrf->m_NumberOfRangeReadings = 1081;
00316 }
00317 break;
00318
00319
00320
00321 case LaserRangeFinder_Hokuyo_URG_04LX:
00322 {
00323 pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Hokuyo URG-04LX");
00324
00325
00326 pLrf->m_pMinimumRange->SetValue(0.02);
00327 pLrf->m_pMaximumRange->SetValue(4.0);
00328
00329
00330 pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120));
00331 pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120));
00332
00333
00334 pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
00335
00336 pLrf->m_NumberOfRangeReadings = 751;
00337 }
00338 break;
00339
00340 case LaserRangeFinder_Custom:
00341 {
00342 pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "User-Defined LaserRangeFinder");
00343
00344
00345 pLrf->m_pMinimumRange->SetValue(0.0);
00346 pLrf->m_pMaximumRange->SetValue(80.0);
00347
00348
00349 pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
00350 pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
00351
00352
00353 pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0));
00354
00355 pLrf->m_NumberOfRangeReadings = 181;
00356 }
00357 break;
00358 }
00359
00360 if (pLrf != NULL)
00361 {
00362 pLrf->m_pType->SetValue(type);
00363
00364 Pose2 defaultOffset;
00365 pLrf->SetOffsetPose(defaultOffset);
00366 }
00367
00368 return pLrf;
00369 }
00370
00371 }
00372