SensorData.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #pragma once
19 
20 #ifndef __OpenKarto_SensorData_h__
21 #define __OpenKarto_SensorData_h__
22 
23 #include <vector>
24 
25 #include <OpenKarto/List.h>
26 #include <OpenKarto/Object.h>
27 #include <OpenKarto/Geometry.h>
28 #include <OpenKarto/Sensor.h>
29 #include <OpenKarto/Objects.h>
34 
35 namespace karto
36 {
37 
39 
40 
44 
49 
54 
58 
59  struct SensorDataPrivate;
60 
65  {
66  KARTO_RTTI();
67 
68  protected:
73  SensorData(const Identifier& rSensorIdentifier);
74 
75  //@cond EXCLUDE
79  virtual ~SensorData();
80  //@endcond
81 
82  public:
87  inline kt_int32s GetStateId() const
88  {
89  return m_StateId;
90  }
91 
96  inline void SetStateId(kt_int32s stateId)
97  {
98  m_StateId = stateId;
99  }
100 
105  inline kt_int32s GetUniqueId() const
106  {
107  return m_UniqueId;
108  }
109 
114  inline void SetUniqueId(kt_int32u uniqueId)
115  {
116  m_UniqueId = uniqueId;
117  }
118 
123  inline kt_int64s GetTime() const
124  {
125  return m_Time;
126  }
127 
132  inline void SetTime(kt_int64s time)
133  {
134  m_Time = time;
135  }
136 
141  inline const Identifier& GetSensorIdentifier() const
142  {
143  return m_SensorIdentifier;
144  }
145 
150  inline void SetSensorIdentifier(const Identifier& rSensorIdentifier)
151  {
152  m_SensorIdentifier = rSensorIdentifier;
153  }
154 
159  void AddCustomItem(CustomItem* pCustomItem);
160 
165  const CustomItemList& GetCustomItems() const;
166 
171  kt_bool HasCustomItem();
172 
173  private:
174  // restrict the following functions
175  SensorData(const SensorData&);
176  const SensorData& operator=(const SensorData&);
177 
178  private:
180 
185 
190 
195 
200  }; // SensorData
201 
206 
210 
215  {
216  KARTO_RTTI();
217 
218  public:
223  LaserRangeScan(const Identifier& rSensorIdentifier);
224 
230  LaserRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rRangeReadings);
231 
232  protected:
233  //@cond EXCLUDE
237  virtual ~LaserRangeScan();
238  //@endcond
239 
240  public:
245  inline const RangeReadingsList& GetRangeReadings() const
246  {
247  return m_RangeReadings;
248  }
249 
254  inline void SetRangeReadings(const RangeReadingsList& rRangeReadings)
255  {
256  m_RangeReadings = rRangeReadings;
257  }
258 
264  {
265  return SensorRegistry::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorIdentifier());
266  }
267 
268  private:
269  // restrict the following functions
271  const LaserRangeScan& operator=(const LaserRangeScan&);
272 
273  private:
274  RangeReadingsList m_RangeReadings;
275  }; // LaserRangeScan
276 
281 
285 
289  class DrivePose : public SensorData
290  {
291  KARTO_RTTI();
292 
293  public:
298  DrivePose(const Identifier& rSensorIdentifier)
299  : SensorData(rSensorIdentifier)
300  {
301  }
302 
303  protected:
304  //@cond EXCLUDE
308  virtual ~DrivePose()
309  {
310  }
311  //@endcond
312 
313  public:
318  inline const Pose2& GetOdometricPose() const
319  {
320  return m_OdometricPose;
321  }
322 
327  inline void SetOdometricPose(const Pose2& rPose)
328  {
329  m_OdometricPose = rPose;
330  }
331 
332  private:
333  // restrict the following functions
334  DrivePose(const DrivePose&);
335  const DrivePose& operator=(const DrivePose&);
336 
337  private:
342  }; // class DrivePose
343 
348 
352 
357  {
358  KARTO_RTTI();
359 
360  public:
365  LocalizedObject(const Identifier& rSensorIdentifier);
366 
367  protected:
368  //@cond EXCLUDE
372  virtual ~LocalizedObject();
373  //@endcond
374 
375  public:
380  inline const Pose2& GetOdometricPose() const
381  {
382  return m_OdometricPose;
383  }
384 
389  inline void SetOdometricPose(const Pose2& rOdometricPose)
390  {
391  m_OdometricPose = rOdometricPose;
392  }
393 
402  inline const Pose2& GetCorrectedPose() const
403  {
404  return m_CorrectedPose;
405  }
406 
411  virtual inline void SetCorrectedPose(const Pose2& rPose)
412  {
413  m_CorrectedPose = rPose;
414  }
415 
422  {
423  return m_GpsReading;
424  }
425 
430  inline void SetGpsReading(const gps::PointGps& rGpsReading)
431  {
432  m_GpsReading = rGpsReading;
433  m_IsGpsReadingValid = true;
434  }
435 
440  inline kt_bool IsGpsReadingValid() const
441  {
442  return m_IsGpsReadingValid;
443  }
444 
451  {
452  if (m_pGpsEstimationManager != NULL)
453  {
454  return m_pGpsEstimationManager->GetGpsEstimate(this);
455  }
456  else
457  {
458  return m_GpsEstimate;
459  }
460  }
461 
466  inline void SetGpsEstimate(const gps::PointGps& rGpsEstimate)
467  {
468  if (m_pGpsEstimationManager != NULL)
469  {
470  m_pGpsEstimationManager->SetGpsEstimate(this, rGpsEstimate);
471  }
472  else
473  {
474  m_GpsEstimate = rGpsEstimate;
475  m_IsGpsEstimateValid = true;
476  }
477  }
478 
484  {
485  if (m_pGpsEstimationManager != NULL)
486  {
487  return m_pGpsEstimationManager->IsGpsEstimateValid(this);
488  }
489  else
490  {
491  return m_IsGpsEstimateValid;
492  }
493  }
494 
499  inline void SetGpsEstimationManager(AbstractGpsEstimationManager* pGpsEstimationManager)
500  {
501  m_pGpsEstimationManager = pGpsEstimationManager;
502  }
503 
504  private:
509 
514 
520 
525 
531 
536 
541  }; // LocalizedObject
542 
547 
552 
557 
561 
563 
568  {
569  KARTO_RTTI();
570 
571  public:
577  {
578  return SensorRegistry::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorIdentifier());
579  }
580 
585  virtual inline void SetCorrectedPose(const Pose2& rCorrectedPose)
586  {
587  LocalizedObject::SetCorrectedPose(rCorrectedPose);
588 
589  m_IsDirty = true;
590  }
591 
596  inline const Pose2& GetBarycenterPose() const
597  {
598  if (m_IsDirty)
599  {
600  // throw away constness and do an update!
601  const_cast<LocalizedLaserScan*>(this)->Update();
602  }
603 
604  return m_BarycenterPose;
605  }
606 
612  inline Pose2 GetReferencePose(kt_bool useBarycenter) const
613  {
614  if (m_IsDirty)
615  {
616  // throw away constness and do an update!
617  const_cast<LocalizedLaserScan*>(this)->Update();
618  }
619 
620  return useBarycenter ? GetBarycenterPose() : GetSensorPose();
621  }
622 
627  inline Pose2 GetSensorPose() const
628  {
629  return GetSensorAt(GetCorrectedPose());
630  }
631 
636  void SetSensorPose(const Pose2& rSensorPose)
637  {
638  Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
639  kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
640  kt_double offsetHeading = deviceOffsetPose2.GetHeading();
641  kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
642  kt_double correctedHeading = math::NormalizeAngle(rSensorPose.GetHeading());
643  Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
644  offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
645  offsetHeading);
646 
647  SetCorrectedPose(rSensorPose - worldSensorOffset);
648 
649  Update();
650  }
651 
657  inline Pose2 GetSensorAt(const Pose2& rPose) const
658  {
659  return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
660  }
661 
666  inline const BoundingBox2& GetBoundingBox() const
667  {
668  if (m_IsDirty)
669  {
670  // throw away constness and do an update!
671  const_cast<LocalizedLaserScan*>(this)->Update();
672  }
673 
674  return m_BoundingBox;
675  }
676 
682  const Vector2dList& GetPointReadings(kt_bool wantFiltered = false) const;
683 
688  inline const RangeReadingsList& GetRangeReadings() const
689  {
690  return m_RangeReadings;
691  }
692 
698  {
699  return m_RangeReadings.Size();
700  }
701 
702  protected:
707  LocalizedLaserScan(const Identifier& rSensorIdentifier);
708  virtual ~LocalizedLaserScan();
709 
713  void Update();
714 
718  virtual void ComputePointReadings() = 0;
719 
724  virtual const Vector2dList& GetFilteredPointReadings() const
725  {
726  return m_FilteredPointReadings;
727  }
728 
734  {
735  return m_UnfilteredPointReadings;
736  }
737 
738  protected:
743 
748 
752  RangeReadingsList m_RangeReadings;
753 
754  private:
759 
764 
769 
774 
775  }; // LocalizedLaserScan
776 
781 
786 
791 
795 
800  {
801  KARTO_RTTI();
802 
803  public:
809  LocalizedPointScan(const Identifier& rSensorIdentifier, const Vector2dList& rLocalPoints);
810 
815  inline const DoubleList& GetLocalAngles() const
816  {
817  return m_LocalAngles;
818  }
819 
820  protected:
821  //@cond EXCLUDE
825  virtual ~LocalizedPointScan();
826  //@endcond
827 
828  private:
832  virtual void ComputePointReadings();
833 
834  private:
836  const LocalizedPointScan& operator=(const LocalizedPointScan&);
837 
838  private:
840  DoubleList m_LocalAngles;
841  }; // LocalizedPointScan
842 
847 
851 
875  {
876  KARTO_RTTI();
877 
878  public:
884  LocalizedRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rReadings);
885 
892  KARTO_DEPRECATED KARTO_FORCEINLINE LocalizedRangeScan(const Identifier& rSensorIdentifier, std::vector<kt_double>& rRangeReadings)
893  : LocalizedLaserScan(rSensorIdentifier)
894  {
895  m_RangeReadings.Resize(static_cast<kt_int32u>(rRangeReadings.size()));
896  if (rRangeReadings.size() > 0)
897  {
898  memcpy(&(m_RangeReadings[0]), &(rRangeReadings[0]), sizeof(kt_double) * rRangeReadings.size());
899  }
900  }
901 
902  protected:
903  //@cond EXCLUDE
907  virtual ~LocalizedRangeScan();
908  //@endcond
909 
910  private:
915  virtual void ComputePointReadings();
916 
917  private:
919  const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
920  }; // LocalizedRangeScan
921 
926 
928 
929 }
930 
931 #endif // __OpenKarto_SensorData_h__
BoundingBox2 m_BoundingBox
Definition: SensorData.h:768
void SetOdometricPose(const Pose2 &rOdometricPose)
Definition: SensorData.h:389
kt_int32s GetStateId() const
Definition: SensorData.h:87
bool kt_bool
Definition: Types.h:145
std::size_t kt_size_t
Definition: Types.h:138
kt_double GetHeading() const
Definition: Geometry.h:2274
void SetUniqueId(kt_int32u uniqueId)
Definition: SensorData.h:114
SensorDataPrivate * m_pSensorDataPrivate
Definition: SensorData.h:179
List< kt_double > DoubleList
Definition: SensorData.h:53
gps::PointGps GetGpsReading() const
Definition: SensorData.h:421
virtual void SetCorrectedPose(const Pose2 &rCorrectedPose)
Definition: SensorData.h:585
void SetOdometricPose(const Pose2 &rPose)
Definition: SensorData.h:327
const BoundingBox2 & GetBoundingBox() const
Definition: SensorData.h:666
Vector2dList m_LocalPointReadings
Definition: SensorData.h:839
const DoubleList & GetLocalAngles() const
Definition: SensorData.h:815
SmartPointer< LocalizedObject > LocalizedObjectPtr
Definition: SensorData.h:551
#define KARTO_EXPORT
Definition: Macros.h:78
#define KARTO_DEPRECATED
Definition: Macros.h:39
kt_double GetX() const
Definition: Geometry.h:2220
kt_int64s GetTime() const
Definition: SensorData.h:123
RangeReadingsList m_RangeReadings
Definition: SensorData.h:274
virtual const Vector2dList & GetFilteredPointReadings() const
Definition: SensorData.h:724
kt_bool m_IsGpsEstimateValid
Definition: SensorData.h:535
void SetGpsEstimate(const gps::PointGps &rGpsEstimate)
Definition: SensorData.h:466
Pose2 TransformPose(const Pose2 &rSourcePose) const
Definition: PoseTransform.h:61
void SetStateId(kt_int32s stateId)
Definition: SensorData.h:96
void SetRangeReadings(const RangeReadingsList &rRangeReadings)
Definition: SensorData.h:254
Identifier m_SensorIdentifier
Definition: SensorData.h:194
Pose2 GetSensorAt(const Pose2 &rPose) const
Definition: SensorData.h:657
void SetGpsEstimationManager(AbstractGpsEstimationManager *pGpsEstimationManager)
Definition: SensorData.h:499
Pose2 GetSensorPose() const
Definition: SensorData.h:627
kt_double Length() const
Definition: Geometry.h:423
const RangeReadingsList & GetRangeReadings() const
Definition: SensorData.h:688
gps::PointGps GetGpsEstimate() const
Definition: SensorData.h:450
kt_int32s m_UniqueId
Definition: SensorData.h:189
kt_bool IsGpsEstimateValid() const
Definition: SensorData.h:483
void SetSensorIdentifier(const Identifier &rSensorIdentifier)
Definition: SensorData.h:150
Identifier m_SensorIdentifier
Definition: SensorData.h:758
kt_int32s m_StateId
Definition: SensorData.h:184
void SetSensorPose(const Pose2 &rSensorPose)
Definition: SensorData.h:636
#define KARTO_FORCEINLINE
Definition: Macros.h:56
const Pose2 & GetBarycenterPose() const
Definition: SensorData.h:596
uint32_t kt_int32u
Definition: Types.h:111
SmartPointer< LocalizedLaserScan > LocalizedLaserScanPtr
Definition: SensorData.h:785
RangeReadingsList m_RangeReadings
Definition: SensorData.h:752
const Identifier & GetSensorIdentifier() const
Definition: SensorData.h:141
const Pose2 & GetOdometricPose() const
Definition: SensorData.h:318
Vector2dList m_UnfilteredPointReadings
Definition: SensorData.h:747
#define KARTO_RTTI()
Definition: Meta.h:198
kt_int32s GetUniqueId() const
Definition: SensorData.h:105
const RangeReadingsList & GetRangeReadings() const
Definition: SensorData.h:245
Pose2 m_OdometricPose
Definition: SensorData.h:341
void SetGpsReading(const gps::PointGps &rGpsReading)
Definition: SensorData.h:430
DrivePose(const Identifier &rSensorIdentifier)
Definition: SensorData.h:298
int32_t kt_int32s
Definition: Types.h:106
KARTO_TYPE(Grid< kt_int8u >)
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
Sensor * GetSensorByName(const Identifier &rName)
gps::PointGps m_GpsEstimate
Definition: SensorData.h:530
virtual void SetCorrectedPose(const Pose2 &rPose)
Definition: SensorData.h:411
static SensorRegistry * GetInstance()
kt_bool IsGpsReadingValid() const
Definition: SensorData.h:440
Vector2dList m_FilteredPointReadings
Definition: SensorData.h:742
double kt_double
Definition: Types.h:160
LaserRangeFinder * GetLaserRangeFinder() const
Definition: SensorData.h:263
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:213
void SetTime(kt_int64s time)
Definition: SensorData.h:132
signed long long kt_int64s
Definition: Types.h:127
AbstractGpsEstimationManager * m_pGpsEstimationManager
Definition: SensorData.h:540
kt_double GetY() const
Definition: Geometry.h:2238
Definition: Any.cpp:20
const Pose2 & GetOdometricPose() const
Definition: SensorData.h:380
kt_size_t GetNumberOfRangeReadings() const
Definition: SensorData.h:697
KARTO_DEPRECATED KARTO_FORCEINLINE LocalizedRangeScan(const Identifier &rSensorIdentifier, std::vector< kt_double > &rRangeReadings)
Definition: SensorData.h:892
virtual const Vector2dList & GetUnfilteredPointReadings() const
Definition: SensorData.h:733
kt_int64s m_Time
Definition: SensorData.h:199
List< kt_double > RangeReadingsList
Definition: SensorData.h:48
const Pose2 & GetCorrectedPose() const
Definition: SensorData.h:402
List< LocalizedObjectPtr > LocalizedObjectList
Definition: SensorData.h:556
Pose2 GetReferencePose(kt_bool useBarycenter) const
Definition: SensorData.h:612
List< LocalizedLaserScanPtr > LocalizedLaserScanList
Definition: SensorData.h:790
LaserRangeFinder * GetLaserRangeFinder() const
Definition: SensorData.h:576
gps::PointGps m_GpsReading
Definition: SensorData.h:519


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24