Sensor.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
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     // compute point readings
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     // make sure rangeThreshold is within laser range finder range
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     // check if min < max range!
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     // set range threshold to valid value
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     // verify number of range readings in laser scan matches the number of expected range
00217     // readings; validation only valid with LocalizedRangeScan (LocalizedPointScan may have
00218     // variable number of readings)
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       //      assert(false);
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       // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
00236       // set range threshold to 18m
00237     case LaserRangeFinder_Sick_LMS100:
00238       {
00239         pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 100");
00240 
00241         // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
00242         pLrf->m_pMinimumRange->SetValue(0.0);
00243         pLrf->m_pMaximumRange->SetValue(20.0);
00244 
00245         // 270 degree range, 50 Hz 
00246         pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135)); 
00247         pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135)); 
00248 
00249         // 0.25 degree angular resolution
00250         pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); 
00251 
00252         pLrf->m_NumberOfRangeReadings = 1081;
00253       }
00254       break;
00255 
00256       // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
00257       // set range threshold to 10m
00258     case LaserRangeFinder_Sick_LMS200:
00259       {
00260         pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 200");
00261 
00262         // Sensing range is 80 meters
00263         pLrf->m_pMinimumRange->SetValue(0.0);
00264         pLrf->m_pMaximumRange->SetValue(80.0);
00265 
00266         // 180 degree range, 75 Hz 
00267         pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); 
00268         pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); 
00269 
00270         // 0.5 degree angular resolution
00271         pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5)); 
00272 
00273         pLrf->m_NumberOfRangeReadings = 361;
00274       }
00275       break;
00276 
00277       // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
00278       // set range threshold to 30m
00279     case LaserRangeFinder_Sick_LMS291:
00280       {
00281         pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Sick LMS 291");
00282 
00283         // Sensing range is 80 meters
00284         pLrf->m_pMinimumRange->SetValue(0.0);
00285         pLrf->m_pMaximumRange->SetValue(80.0);
00286 
00287         // 180 degree range, 75 Hz 
00288         pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); 
00289         pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); 
00290 
00291         // 0.5 degree angular resolution
00292         pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5)); 
00293 
00294         pLrf->m_NumberOfRangeReadings = 361;
00295       }
00296       break;
00297 
00298       // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
00299       // set range threshold to 30m
00300     case LaserRangeFinder_Hokuyo_UTM_30LX:
00301       {
00302         pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Hokuyo UTM-30LX");
00303 
00304         // Sensing range is 30 meters
00305         pLrf->m_pMinimumRange->SetValue(0.1);
00306         pLrf->m_pMaximumRange->SetValue(30.0);
00307 
00308         // 270 degree range, 40 Hz 
00309         pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
00310         pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135)); 
00311 
00312         // 0.25 degree angular resolution
00313         pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); 
00314 
00315         pLrf->m_NumberOfRangeReadings = 1081;
00316       }
00317       break;
00318 
00319       // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
00320       // set range threshold to 3.5m
00321     case LaserRangeFinder_Hokuyo_URG_04LX:
00322       {
00323         pLrf = new LaserRangeFinder((rName.ToString() != "") ? rName : "Hokuyo URG-04LX");
00324 
00325         // Sensing range is 4 meters. It has detection problems when scanning absorptive surfaces (such as black trimming). 
00326         pLrf->m_pMinimumRange->SetValue(0.02);
00327         pLrf->m_pMaximumRange->SetValue(4.0);
00328 
00329         // 240 degree range, 10 Hz 
00330         pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120)); 
00331         pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120));
00332 
00333         // 0.352 degree angular resolution
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         // Sensing range is 80 meters.
00345         pLrf->m_pMinimumRange->SetValue(0.0);
00346         pLrf->m_pMaximumRange->SetValue(80.0);
00347 
00348         // 180 degree range
00349         pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); 
00350         pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
00351 
00352         // 1.0 degree angular resolution
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 


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Mon Oct 6 2014 02:44:17