SensorData.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 <OpenKarto/SensorData.h>
00019 
00020 namespace karto
00021 {
00022 
00026 
00027   struct SensorDataPrivate
00028   {
00032     CustomItemList m_CustomItems;
00033   };
00034 
00035   SensorData::SensorData(const Identifier& rSensorIdentifier)
00036     : Object()
00037     , m_pSensorDataPrivate(new SensorDataPrivate())
00038     , m_StateId(-1)
00039     , m_UniqueId(-1)
00040     , m_SensorIdentifier(rSensorIdentifier)
00041     , m_Time(0)
00042   {
00043     assert(m_SensorIdentifier.Size() != 0);
00044   }
00045 
00046   SensorData::~SensorData()
00047   {
00048     m_pSensorDataPrivate->m_CustomItems.Clear();
00049 
00050     delete m_pSensorDataPrivate;    
00051   }
00052 
00053   void SensorData::AddCustomItem(CustomItem* pCustomData)
00054   {
00055     m_pSensorDataPrivate->m_CustomItems.Add(pCustomData);
00056   }
00057 
00058   const CustomItemList& SensorData::GetCustomItems() const
00059   {
00060     return m_pSensorDataPrivate->m_CustomItems;
00061   }
00062 
00063   kt_bool SensorData::HasCustomItem() 
00064   {
00065     return m_pSensorDataPrivate->m_CustomItems.Size() > 0;
00066   }
00067 
00071 
00072   LaserRangeScan::LaserRangeScan(const Identifier& rSensorIdentifier)
00073     : SensorData(rSensorIdentifier)
00074   {
00075   }
00076 
00077   LaserRangeScan::LaserRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rRangeReadings)
00078     : SensorData(rSensorIdentifier)
00079     , m_RangeReadings(rRangeReadings)
00080   {
00081   }
00082 
00083   LaserRangeScan::~LaserRangeScan()
00084   {
00085   }
00086 
00090 
00091   LocalizedObject::LocalizedObject(const Identifier& rSensorIdentifier)
00092     : SensorData(rSensorIdentifier)
00093     , m_IsGpsReadingValid(false)
00094     , m_IsGpsEstimateValid(false)
00095     , m_pGpsEstimationManager(NULL)
00096   {
00097   }
00098 
00099   LocalizedObject::~LocalizedObject()
00100   {
00101   }
00102 
00106   
00107   LocalizedLaserScan::LocalizedLaserScan(const Identifier& rSensorIdentifier)
00108     : LocalizedObject(rSensorIdentifier)
00109     , m_IsDirty(true)
00110   {
00111   }
00112   
00113   LocalizedLaserScan::~LocalizedLaserScan()
00114   {
00115   }
00116 
00117   void LocalizedLaserScan::Update()
00118   {
00119     ComputePointReadings();
00120 
00121     // set flag early, otherwise call to GetPointReadings()
00122     // below will recurse infinitely
00123     m_IsDirty = false;
00124 
00125     Pose2 scanPose = GetSensorPose();
00126     
00127     Vector2d rangePointsSum;
00128     const Vector2dList& rPointReadings = GetPointReadings(true);
00129 
00130     // calculate bounding box of scan
00131     m_BoundingBox = BoundingBox2();
00132     m_BoundingBox.Add(scanPose.GetPosition());
00133     karto_const_forEach(Vector2dList, &rPointReadings)
00134     {
00135       m_BoundingBox.Add(*iter);
00136       rangePointsSum += *iter;
00137     }          
00138     
00139     // compute barycenter
00140     kt_double nPoints = static_cast<kt_double>(rPointReadings.Size());
00141     if (nPoints != 0.0)
00142     {
00143       Vector2d averagePosition = Vector2d(rangePointsSum / nPoints);
00144       m_BarycenterPose = Pose2(averagePosition, 0.0);
00145     }
00146     else
00147     {
00148       m_BarycenterPose = scanPose;
00149     }    
00150   }
00151 
00152   const Vector2dList& LocalizedLaserScan::GetPointReadings(kt_bool wantFiltered) const
00153   {
00154     if (m_IsDirty)
00155     {
00156       // throw away constness and do an update!
00157       const_cast<LocalizedLaserScan*>(this)->Update();
00158     }    
00159     
00160     if (wantFiltered == true)
00161     {
00162       return GetFilteredPointReadings(); 
00163     }
00164     else
00165     {
00166       return GetUnfilteredPointReadings();
00167     }    
00168   }
00169 
00173 
00174   LocalizedRangeScan::LocalizedRangeScan(const Identifier& rSensorIdentifier, const List<kt_double>& rReadings)
00175     : LocalizedLaserScan(rSensorIdentifier)
00176   {
00177     m_RangeReadings = rReadings;
00178   }
00179 
00180   LocalizedRangeScan::~LocalizedRangeScan()
00181   {
00182   }
00183 
00184   void LocalizedRangeScan::ComputePointReadings()
00185   {
00186     LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
00187     if (pLaserRangeFinder == NULL)
00188     {
00189       return;
00190     }
00191     
00192     m_FilteredPointReadings.Clear();
00193     m_UnfilteredPointReadings.Clear();
00194     
00195     kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
00196     kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
00197 
00198     kt_double minimumRange = pLaserRangeFinder->GetMinimumRange();
00199     kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
00200 
00201     Pose2 scanPose = GetSensorPose();
00202     
00203     const List<kt_double>& rRangeReadings = GetRangeReadings();
00204 
00205     // compute point readings
00206     kt_int32u numberOfReadings = pLaserRangeFinder->GetNumberOfRangeReadings();
00207     for (kt_int32u i = 0; i < numberOfReadings; i++)
00208     {
00209       kt_double angle = scanPose.GetHeading() + minimumAngle + i * angularResolution;
00210       kt_double rangeReading = rRangeReadings[i];
00211       
00212       Vector2d point;
00213       point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
00214       point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
00215       m_UnfilteredPointReadings.Add(point);
00216       
00217       if (math::InRange(rangeReading, minimumRange, rangeThreshold))
00218       {
00219         m_FilteredPointReadings.Add(point);
00220       }
00221     }
00222   }
00223 
00227   
00228   LocalizedPointScan::LocalizedPointScan(const Identifier& rSensorIdentifier, const Vector2dList& rLocalPoints)
00229     : LocalizedLaserScan(rSensorIdentifier)
00230   {
00231     m_LocalPointReadings = rLocalPoints;
00232     
00233     Vector2d origin(0, 0);
00234     Pose2 originPose(origin, 0);
00235     karto_const_forEach(Vector2dList, &rLocalPoints)
00236     {
00237       m_RangeReadings.Add(origin.Distance(*iter));
00238       m_LocalAngles.Add(originPose.AngleTo(*iter));
00239     }
00240   }
00241   
00242   LocalizedPointScan::~LocalizedPointScan()
00243   {
00244   }
00245   
00246   void LocalizedPointScan::ComputePointReadings()
00247   {
00248     LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
00249     if (pLaserRangeFinder == NULL)
00250     {
00251       return;
00252     }
00253 
00254     m_FilteredPointReadings.Clear();
00255     m_UnfilteredPointReadings.Clear();
00256     
00257     kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
00258     Pose2 scanPose = GetSensorPose();
00259     Vector2d scanPosition = scanPose.GetPosition();
00260     
00261     // compute point readings
00262     for (kt_int32u i = 0; i < m_LocalPointReadings.Size(); i++)
00263     {
00264       RigidBodyTransform transform(Pose2(m_LocalPointReadings[i], 0));
00265       Pose2 pointPose = transform.TransformPose(scanPose);
00266       Vector2d point = pointPose.GetPosition();
00267       m_UnfilteredPointReadings.Add(point);
00268       
00269       kt_double range = scanPosition.Distance(point);
00270       if (math::InRange(range, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
00271       {
00272         m_FilteredPointReadings.Add(point);
00273       }
00274     }
00275   }
00276 
00277 }


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:25