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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019