Karto.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
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 <math.h>
00019 #include <assert.h>
00020 #include <float.h>
00021 #include <limits.h>
00022 #include <stdlib.h>
00023 #include <string.h>
00024 #include <stdio.h>
00025 
00026 #include "open_karto/Karto.h"
00027 
00028 namespace karto
00029 {
00030 
00034 
00035   SensorManager* SensorManager::GetInstance()
00036   {
00037     static Singleton<SensorManager> sInstance;
00038     return sInstance.Get();
00039   }
00040 
00044 
00045   Object::Object()
00046     : m_pParameterManager(new ParameterManager())
00047   {
00048   }
00049 
00050   Object::Object(const Name& rName)
00051     : m_Name(rName)
00052     , m_pParameterManager(new ParameterManager())
00053   {
00054   }
00055 
00056   Object::~Object()
00057   {
00058     delete m_pParameterManager;
00059     m_pParameterManager = NULL;
00060   }
00061 
00065 
00066   Module::Module(const std::string& rName)
00067     : Object(rName)
00068   {
00069   }
00070 
00071   Module::~Module()
00072   {
00073   }
00074 
00078 
00079   Sensor::Sensor(const Name& rName)
00080     : Object(rName)
00081   {
00082     m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
00083   }
00084 
00085   Sensor::~Sensor()
00086   {
00087   }
00088 
00092 
00093   SensorData::SensorData(const Name& rSensorName)
00094     : Object()
00095     , m_StateId(-1)
00096     , m_UniqueId(-1)
00097     , m_SensorName(rSensorName)
00098     , m_Time(0.0)
00099   {
00100   }
00101 
00102   SensorData::~SensorData()
00103   {
00104     forEach(CustomDataVector, &m_CustomData)
00105     {
00106       delete *iter;
00107     }
00108 
00109     m_CustomData.clear();
00110   }
00111 
00115 
00116   void CellUpdater::operator() (kt_int32u index)
00117   {
00118     kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
00119     kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
00120     kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
00121 
00122     m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
00123   }
00124 
00128 
00129   std::ostream& operator << (std::ostream& rStream, Exception& rException)
00130   {
00131     rStream << "Error detect: " << std::endl;
00132     rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
00133     rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;
00134 
00135     return rStream;
00136   }
00137 
00141 
00142   const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
00143                                                              CoordinateConverter* pCoordinateConverter,
00144                                                              kt_bool ignoreThresholdPoints,
00145                                                              kt_bool flipY) const
00146   {
00147     PointVectorDouble pointReadings;
00148 
00149     Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();
00150 
00151     // compute point readings
00152     kt_int32u beamNum = 0;
00153     kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
00154     for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
00155     {
00156       kt_double rangeReading = pRangeReadings[i];
00157 
00158       if (ignoreThresholdPoints)
00159       {
00160         if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
00161         {
00162           continue;
00163         }
00164       }
00165       else
00166       {
00167         rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
00168       }
00169 
00170       kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();
00171 
00172       Vector2<kt_double> point;
00173 
00174       point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
00175       point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
00176 
00177       if (pCoordinateConverter != NULL)
00178       {
00179         Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
00180         point.SetX(gridPoint.GetX());
00181         point.SetY(gridPoint.GetY());
00182       }
00183 
00184       pointReadings.push_back(point);
00185     }
00186 
00187     return pointReadings;
00188   }
00189 
00190   kt_bool LaserRangeFinder::Validate(SensorData* pSensorData)
00191   {
00192     LaserRangeScan* pLaserRangeScan = dynamic_cast<LaserRangeScan*>(pSensorData);
00193 
00194     // verify number of range readings in LaserRangeScan matches the number of expected range readings
00195     if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
00196     {
00197       std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
00198                 << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
00199       return false;
00200     }
00201 
00202     return true;
00203   }
00204 
00208 
00209   void ParameterManager::Clear()
00210   {
00211     forEach(karto::ParameterVector, &m_Parameters)
00212     {
00213       delete *iter;
00214     }
00215 
00216     m_Parameters.clear();
00217 
00218     m_ParameterLookup.clear();
00219   }
00220 
00221   void ParameterManager::Add(AbstractParameter* pParameter)
00222   {
00223     if (pParameter != NULL && pParameter->GetName() != "")
00224     {
00225       if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
00226       {
00227         m_Parameters.push_back(pParameter);
00228 
00229         m_ParameterLookup[pParameter->GetName()] = pParameter;
00230       }
00231       else
00232       {
00233         m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());
00234 
00235         assert(false);
00236       }
00237     }
00238   }
00239 
00243 
00244   /*
00245   std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
00246   {
00247     "Custom",
00248 
00249     "Sick_LMS100",
00250     "Sick_LMS200",
00251     "Sick_LMS291",
00252 
00253     "Hokuyo_UTM_30LX",
00254     "Hokuyo_URG_04LX"
00255   };
00256    */
00257 }  // namespace karto


open_karto
Author(s):
autogenerated on Thu Aug 27 2015 14:14:06