SensorData.h
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 #pragma once
00019 
00020 #ifndef __OpenKarto_SensorData_h__
00021 #define __OpenKarto_SensorData_h__
00022 
00023 #include <vector>
00024 
00025 #include <OpenKarto/List.h>
00026 #include <OpenKarto/Object.h>
00027 #include <OpenKarto/Geometry.h>
00028 #include <OpenKarto/Sensor.h>
00029 #include <OpenKarto/Objects.h>
00030 #include <OpenKarto/PoseTransform.h>
00031 #include <OpenKarto/RigidBodyTransform.h>
00032 #include <OpenKarto/SensorRegistry.h>
00033 #include <OpenKarto/AbstractGpsEstimationManager.h>
00034 
00035 namespace karto
00036 {
00037 
00039 
00040 
00044 
00048   typedef List<kt_double> RangeReadingsList;
00049 
00053   typedef List<kt_double> DoubleList;
00054 
00058 
00059   struct SensorDataPrivate;
00060 
00064   class KARTO_EXPORT SensorData : public Object
00065   {
00066     KARTO_RTTI();
00067 
00068   protected:
00073     SensorData(const Identifier& rSensorIdentifier);
00074 
00075     //@cond EXCLUDE
00079     virtual ~SensorData();
00080     //@endcond
00081     
00082   public:
00087     inline kt_int32s GetStateId() const
00088     {
00089       return m_StateId;
00090     }
00091 
00096     inline void SetStateId(kt_int32s stateId)
00097     {
00098       m_StateId = stateId;
00099     }
00100 
00105     inline kt_int32s GetUniqueId() const
00106     {
00107       return m_UniqueId;
00108     }
00109 
00114     inline void SetUniqueId(kt_int32u uniqueId)
00115     {
00116       m_UniqueId = uniqueId;
00117     }
00118     
00123     inline kt_int64s GetTime() const
00124     {
00125       return m_Time;
00126     }
00127 
00132     inline void SetTime(kt_int64s time)
00133     {
00134       m_Time = time;
00135     }
00136 
00141     inline const Identifier& GetSensorIdentifier() const
00142     {
00143       return m_SensorIdentifier;
00144     }
00145 
00150     inline void SetSensorIdentifier(const Identifier& rSensorIdentifier)
00151     {
00152       m_SensorIdentifier = rSensorIdentifier;
00153     }
00154 
00159     void AddCustomItem(CustomItem* pCustomItem);
00160     
00165     const CustomItemList& GetCustomItems() const;
00166 
00171      kt_bool HasCustomItem();
00172 
00173   private:
00174     // restrict the following functions
00175     SensorData(const SensorData&);
00176     const SensorData& operator=(const SensorData&);
00177 
00178   private:
00179     SensorDataPrivate* m_pSensorDataPrivate;
00180 
00184     kt_int32s m_StateId;
00185 
00189     kt_int32s m_UniqueId;
00190     
00194     Identifier m_SensorIdentifier;
00195 
00199     kt_int64s m_Time;
00200   }; // SensorData
00201 
00205   KARTO_TYPE(SensorData);
00206 
00210 
00214   class KARTO_EXPORT LaserRangeScan : public SensorData
00215   {
00216     KARTO_RTTI();
00217 
00218   public:
00223     LaserRangeScan(const Identifier& rSensorIdentifier);
00224 
00230     LaserRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rRangeReadings);
00231 
00232   protected:
00233     //@cond EXCLUDE
00237     virtual ~LaserRangeScan();
00238     //@endcond
00239     
00240   public:
00245     inline const RangeReadingsList& GetRangeReadings() const
00246     {
00247       return m_RangeReadings;
00248     }
00249   
00254     inline void SetRangeReadings(const RangeReadingsList& rRangeReadings)
00255     {
00256       m_RangeReadings = rRangeReadings;
00257     }
00258 
00263     inline LaserRangeFinder* GetLaserRangeFinder() const
00264     {
00265       return SensorRegistry::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorIdentifier());
00266     }
00267 
00268   private:
00269     // restrict the following functions
00270     LaserRangeScan(const LaserRangeScan&);
00271     const LaserRangeScan& operator=(const LaserRangeScan&);
00272 
00273   private:
00274     RangeReadingsList m_RangeReadings;
00275   }; // LaserRangeScan
00276 
00280   KARTO_TYPE(LaserRangeScan);
00281 
00285 
00289   class DrivePose : public SensorData
00290   {
00291     KARTO_RTTI();
00292 
00293   public:
00298     DrivePose(const Identifier& rSensorIdentifier)
00299       : SensorData(rSensorIdentifier)
00300     {
00301     }
00302 
00303   protected:
00304     //@cond EXCLUDE
00308     virtual ~DrivePose()
00309     {
00310     }
00311     //@endcond
00312 
00313   public:
00318     inline const Pose2& GetOdometricPose() const 
00319     {
00320       return m_OdometricPose;
00321     }
00322 
00327     inline void SetOdometricPose(const Pose2& rPose)
00328     {
00329       m_OdometricPose = rPose;
00330     }
00331 
00332   private:
00333     // restrict the following functions
00334     DrivePose(const DrivePose&);
00335     const DrivePose& operator=(const DrivePose&);
00336 
00337   private:
00341     Pose2 m_OdometricPose;
00342   }; // class DrivePose
00343 
00347   KARTO_TYPE(DrivePose);
00348 
00352 
00356   class KARTO_EXPORT LocalizedObject : public SensorData
00357   {
00358     KARTO_RTTI();
00359 
00360   public:
00365     LocalizedObject(const Identifier& rSensorIdentifier);
00366     
00367   protected:
00368     //@cond EXCLUDE
00372     virtual ~LocalizedObject();
00373     //@endcond
00374     
00375   public:
00380     inline const Pose2& GetOdometricPose() const 
00381     {
00382       return m_OdometricPose;
00383     }
00384     
00389     inline void SetOdometricPose(const Pose2& rOdometricPose)
00390     {
00391       m_OdometricPose = rOdometricPose;
00392     }
00393     
00402     inline const Pose2& GetCorrectedPose() const 
00403     {
00404       return m_CorrectedPose;
00405     }
00406     
00411     virtual inline void SetCorrectedPose(const Pose2& rPose)
00412     {
00413       m_CorrectedPose = rPose;
00414     }
00415 
00421     inline gps::PointGps GetGpsReading() const
00422     {
00423       return m_GpsReading;
00424     }
00425     
00430     inline void SetGpsReading(const gps::PointGps& rGpsReading)
00431     {
00432       m_GpsReading = rGpsReading;
00433       m_IsGpsReadingValid = true;
00434     }
00435     
00440     inline kt_bool IsGpsReadingValid() const
00441     {
00442       return m_IsGpsReadingValid;
00443     }
00444     
00450     inline gps::PointGps GetGpsEstimate() const
00451     {
00452       if (m_pGpsEstimationManager != NULL)
00453       {
00454         return m_pGpsEstimationManager->GetGpsEstimate(this);
00455       }
00456       else
00457       {
00458         return m_GpsEstimate;
00459       }
00460     }
00461     
00466     inline void SetGpsEstimate(const gps::PointGps& rGpsEstimate)
00467     {
00468       if (m_pGpsEstimationManager != NULL)
00469       {
00470         m_pGpsEstimationManager->SetGpsEstimate(this, rGpsEstimate);
00471       }
00472       else
00473       {
00474         m_GpsEstimate = rGpsEstimate;
00475         m_IsGpsEstimateValid = true;
00476       }
00477     }
00478     
00483     inline kt_bool IsGpsEstimateValid() const
00484     {
00485       if (m_pGpsEstimationManager != NULL)
00486       {
00487         return m_pGpsEstimationManager->IsGpsEstimateValid(this);
00488       }
00489       else
00490       {
00491         return m_IsGpsEstimateValid;
00492       }
00493     }
00494     
00499     inline void SetGpsEstimationManager(AbstractGpsEstimationManager* pGpsEstimationManager)
00500     {
00501       m_pGpsEstimationManager = pGpsEstimationManager;
00502     }
00503 
00504   private:
00508     Pose2 m_OdometricPose;
00509     
00513     Pose2 m_CorrectedPose;
00514 
00519     gps::PointGps m_GpsReading;
00520     
00524     kt_bool m_IsGpsReadingValid;
00525 
00530     gps::PointGps m_GpsEstimate;
00531     
00535     kt_bool m_IsGpsEstimateValid;
00536 
00540     AbstractGpsEstimationManager* m_pGpsEstimationManager;
00541   }; // LocalizedObject
00542 
00546   KARTO_TYPE(LocalizedObject);
00547 
00551   typedef SmartPointer<LocalizedObject> LocalizedObjectPtr;
00552 
00556   typedef List<LocalizedObjectPtr> LocalizedObjectList;
00557 
00561 
00562   class AbstractGpsEstimationManager;
00563 
00567   class KARTO_EXPORT LocalizedLaserScan : public LocalizedObject
00568   {    
00569     KARTO_RTTI();
00570 
00571   public:
00576     inline LaserRangeFinder* GetLaserRangeFinder() const
00577     {
00578       return SensorRegistry::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorIdentifier());
00579     }
00580 
00585     virtual inline void SetCorrectedPose(const Pose2& rCorrectedPose)
00586     {
00587       LocalizedObject::SetCorrectedPose(rCorrectedPose);
00588       
00589       m_IsDirty = true;
00590     }
00591     
00596     inline const Pose2& GetBarycenterPose() const 
00597     {
00598       if (m_IsDirty)
00599       {
00600         // throw away constness and do an update!
00601         const_cast<LocalizedLaserScan*>(this)->Update();
00602       }
00603       
00604       return m_BarycenterPose;
00605     }
00606     
00612     inline Pose2 GetReferencePose(kt_bool useBarycenter) const 
00613     {
00614       if (m_IsDirty)
00615       {
00616         // throw away constness and do an update!
00617         const_cast<LocalizedLaserScan*>(this)->Update();
00618       }
00619       
00620       return useBarycenter ? GetBarycenterPose() : GetSensorPose();
00621     }    
00622     
00627     inline Pose2 GetSensorPose() const
00628     {
00629       return GetSensorAt(GetCorrectedPose());
00630     }
00631     
00636     void SetSensorPose(const Pose2& rSensorPose)
00637     {
00638       Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
00639       kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
00640       kt_double offsetHeading = deviceOffsetPose2.GetHeading();
00641       kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
00642       kt_double correctedHeading = math::NormalizeAngle(rSensorPose.GetHeading());
00643       Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
00644                                       offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
00645                                       offsetHeading);
00646       
00647       SetCorrectedPose(rSensorPose - worldSensorOffset);
00648       
00649       Update();
00650     }
00651     
00657     inline Pose2 GetSensorAt(const Pose2& rPose) const
00658     {
00659       return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
00660     }    
00661     
00666     inline const BoundingBox2& GetBoundingBox() const
00667     {
00668       if (m_IsDirty)
00669       {
00670         // throw away constness and do an update!
00671         const_cast<LocalizedLaserScan*>(this)->Update();
00672       }
00673       
00674       return m_BoundingBox;
00675     }
00676 
00682     const Vector2dList& GetPointReadings(kt_bool wantFiltered = false) const;
00683     
00688     inline const RangeReadingsList& GetRangeReadings() const
00689     {
00690       return m_RangeReadings;
00691     }
00692 
00697     inline kt_size_t GetNumberOfRangeReadings() const
00698     {
00699       return m_RangeReadings.Size();
00700     }
00701 
00702   protected:
00707     LocalizedLaserScan(const Identifier& rSensorIdentifier);
00708     virtual ~LocalizedLaserScan();
00709 
00713     void Update();
00714     
00718     virtual void ComputePointReadings() = 0;
00719 
00724     virtual const Vector2dList& GetFilteredPointReadings() const
00725     {
00726       return m_FilteredPointReadings;
00727     }
00728     
00733     virtual const Vector2dList& GetUnfilteredPointReadings() const
00734     {
00735       return m_UnfilteredPointReadings;
00736     }
00737 
00738   protected:
00742     Vector2dList m_FilteredPointReadings;
00743     
00747     Vector2dList m_UnfilteredPointReadings;
00748     
00752     RangeReadingsList m_RangeReadings;
00753     
00754   private:
00758     Identifier m_SensorIdentifier;
00759         
00763     Pose2 m_BarycenterPose;
00764     
00768     BoundingBox2 m_BoundingBox;
00769     
00773     kt_bool m_IsDirty;
00774     
00775   }; // LocalizedLaserScan
00776   
00780   KARTO_TYPE(LocalizedLaserScan);
00781 
00785   typedef SmartPointer<LocalizedLaserScan> LocalizedLaserScanPtr;
00786   
00790   typedef List<LocalizedLaserScanPtr> LocalizedLaserScanList;
00791   
00795   
00799   class KARTO_EXPORT LocalizedPointScan : public LocalizedLaserScan
00800   {
00801     KARTO_RTTI();
00802    
00803   public:
00809     LocalizedPointScan(const Identifier& rSensorIdentifier, const Vector2dList& rLocalPoints);
00810     
00815     inline const DoubleList& GetLocalAngles() const
00816     {
00817       return m_LocalAngles;
00818     }    
00819     
00820   protected:
00821     //@cond EXCLUDE
00825     virtual ~LocalizedPointScan();
00826     //@endcond
00827     
00828   private:
00832     virtual void ComputePointReadings();
00833 
00834   private:
00835     LocalizedPointScan(const LocalizedPointScan&);
00836     const LocalizedPointScan& operator=(const LocalizedPointScan&);
00837     
00838   private:
00839     Vector2dList m_LocalPointReadings;
00840     DoubleList m_LocalAngles;
00841   }; // LocalizedPointScan
00842   
00846   KARTO_TYPE(LocalizedPointScan);
00847 
00851   
00874   class KARTO_EXPORT LocalizedRangeScan : public LocalizedLaserScan
00875   {
00876     KARTO_RTTI();
00877 
00878   public:
00884     LocalizedRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rReadings);
00885 
00892     KARTO_DEPRECATED KARTO_FORCEINLINE LocalizedRangeScan(const Identifier& rSensorIdentifier, std::vector<kt_double>& rRangeReadings)
00893       : LocalizedLaserScan(rSensorIdentifier)
00894     {
00895       m_RangeReadings.Resize(static_cast<kt_int32u>(rRangeReadings.size()));
00896       if (rRangeReadings.size() > 0)
00897       {
00898         memcpy(&(m_RangeReadings[0]), &(rRangeReadings[0]), sizeof(kt_double) * rRangeReadings.size());
00899       }
00900     }
00901   
00902   protected:
00903     //@cond EXCLUDE
00907     virtual ~LocalizedRangeScan();
00908     //@endcond
00909     
00910   private:
00915     virtual void ComputePointReadings();
00916     
00917   private:
00918     LocalizedRangeScan(const LocalizedRangeScan&);
00919     const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
00920   }; // LocalizedRangeScan
00921 
00925   KARTO_TYPE(LocalizedRangeScan);
00926 
00928 
00929 }
00930 
00931 #endif // __OpenKarto_SensorData_h__


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