Main MRPT website > C++ reference for MRPT 1.9.9
CObservationRange.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 
15 using namespace mrpt::obs;
16 using namespace mrpt::poses;
17 
18 // This must be added to any CSerializable class implementation file.
20 
21 /** Default constructor.
22  */
24  : minSensorDistance(0),
25  maxSensorDistance(5),
26  sensorConeApperture(DEG2RAD(20)),
27  sensedData()
28 {
29 }
30 
33 {
34  // The data
35  out << minSensorDistance << maxSensorDistance << sensorConeApperture;
36  const uint32_t n = sensedData.size();
37  out << n;
38  for (uint32_t i = 0; i < n; i++)
39  out << sensedData[i].sensorID << CPose3D(sensedData[i].sensorPose)
40  << sensedData[i].sensedDistance;
41  out << sensorLabel << timestamp;
42 }
43 
46 {
47  switch (version)
48  {
49  case 0:
50  case 1:
51  case 2:
52  case 3:
53  {
54  uint32_t i, n;
55 
56  // The data
57  in >> minSensorDistance >> maxSensorDistance >> sensorConeApperture;
58 
59  in >> n;
60  sensedData.resize(n);
61  CPose3D aux;
62  for (i = 0; i < n; i++)
63  {
64  if (version >= 3)
65  in >> sensedData[i].sensorID;
66  else
67  sensedData[i].sensorID = i;
68 
69  in >> aux >> sensedData[i].sensedDistance;
70  sensedData[i].sensorPose = aux.asTPose();
71  }
72 
73  if (version >= 1)
74  in >> sensorLabel;
75  else
76  sensorLabel = "";
77 
78  if (version >= 2)
79  in >> timestamp;
80  else
81  timestamp = INVALID_TIMESTAMP;
82  }
83  break;
84  default:
86  };
87 }
88 
89 /*---------------------------------------------------------------
90  getSensorPose
91  ---------------------------------------------------------------*/
92 void CObservationRange::getSensorPose(CPose3D& out_sensorPose) const
93 {
94  if (!sensedData.empty())
95  out_sensorPose = CPose3D(sensedData[0].sensorPose);
96  else
97  out_sensorPose = CPose3D(0, 0, 0);
98 }
99 
100 /*---------------------------------------------------------------
101  setSensorPose
102  ---------------------------------------------------------------*/
103 void CObservationRange::setSensorPose(const CPose3D& newSensorPose)
104 {
105  for (auto& sd : sensedData) sd.sensorPose = newSensorPose.asTPose();
106 }
107 
108 void CObservationRange::getDescriptionAsText(std::ostream& o) const
109 {
110  using namespace std;
112 
113  o << "minSensorDistance = " << minSensorDistance << " m" << endl;
114  o << "maxSensorDistance = " << maxSensorDistance << " m" << endl;
115  o << "sensorConeApperture = " << RAD2DEG(sensorConeApperture) << " deg"
116  << endl;
117 
118  // For each entry in this sequence:
119  o << " SENSOR_ID RANGE (m) SENSOR POSE (on the robot)" << endl;
120  o << "-------------------------------------------------------" << endl;
121  for (size_t q = 0; q < sensedData.size(); q++)
122  {
123  o << format(" %7u", (unsigned int)sensedData[q].sensorID);
124  o << format(" %4.03f ", sensedData[q].sensedDistance);
125  o << sensedData[q].sensorPose << endl;
126  }
127 }
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::obs::CObservationRange::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CObservationRange.cpp:44
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
obs-precomp.h
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
mrpt::obs::CObservation::getDescriptionAsText
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservation.cpp:44
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
mrpt::obs::CObservationRange::setSensorPose
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
Definition: CObservationRange.cpp:103
mrpt::obs::CObservationRange::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CObservationRange.cpp:32
mrpt::obs::CObservationRange::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CObservationRange.cpp:31
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:114
mrpt::obs::CObservationRange
Declares a class derived from "CObservation" that encapsules a single range measurement,...
Definition: CObservationRange.h:30
mrpt::obs::CObservationRange::getDescriptionAsText
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservationRange.cpp:108
CObservationRange.h
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
in
GLuint in
Definition: glext.h:7274
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::obs::CObservationRange::getSensorPose
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
Definition: CObservationRange.cpp:92
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST