Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
13 #include <mrpt/utils/CStream.h>
14 
15 using namespace mrpt::obs;
16 using namespace mrpt::utils;
17 using namespace mrpt::poses;
18 
19 
20 // This must be added to any CSerializable class implementation file.
22 
23 
24 /** Default constructor.
25  */
27  minSensorDistance ( 0 ),
28  maxSensorDistance ( 5 ),
29  sensorConeApperture ( DEG2RAD(20) ),
30  sensedData()
31 {
32 }
33 
34 /*---------------------------------------------------------------
35  Implements the writing to a CStream capability of CSerializable objects
36  ---------------------------------------------------------------*/
38 {
39  if (version)
40  *version = 3;
41  else
42  {
43  uint32_t i,n;
44 
45  // The data
46  out << minSensorDistance << maxSensorDistance << sensorConeApperture;
47 
48  n = sensedData.size();
49  out << n;
50  for (i=0;i<n;i++)
51  out << sensedData[i].sensorID << CPose3D(sensedData[i].sensorPose) << sensedData[i].sensedDistance;
52 
53  out << sensorLabel
54  << timestamp;
55  }
56 }
57 
58 /*---------------------------------------------------------------
59  Implements the reading from a CStream capability of CSerializable objects
60  ---------------------------------------------------------------*/
62 {
63  switch(version)
64  {
65  case 0:
66  case 1:
67  case 2:
68  case 3:
69  {
70  uint32_t i,n;
71 
72  // The data
73  in >> minSensorDistance >> maxSensorDistance >> sensorConeApperture;
74 
75  in >> n;
76  sensedData.resize(n);
77  CPose3D aux;
78  for (i=0;i<n;i++)
79  {
80  if (version>=3)
81  in >> sensedData[i].sensorID;
82  else sensedData[i].sensorID = i;
83 
84  in >> aux >> sensedData[i].sensedDistance;
85  sensedData[i].sensorPose = aux;
86  }
87 
88  if (version>=1)
89  in >> sensorLabel;
90  else sensorLabel = "";
91 
92  if (version>=2)
93  in >> timestamp;
94  else timestamp = INVALID_TIMESTAMP;
95  } break;
96  default:
98 
99  };
100 
101 }
102 
103 /*---------------------------------------------------------------
104  getSensorPose
105  ---------------------------------------------------------------*/
106 void CObservationRange::getSensorPose( CPose3D &out_sensorPose ) const
107 {
108  if (!sensedData.empty())
109  out_sensorPose = CPose3D(sensedData[0].sensorPose);
110  else out_sensorPose = CPose3D(0,0,0);
111 }
112 
113 /*---------------------------------------------------------------
114  setSensorPose
115  ---------------------------------------------------------------*/
116 void CObservationRange::setSensorPose( const CPose3D &newSensorPose )
117 {
118  size_t i, n = sensedData.size();
119  if (n)
120  for (i=0;i<n;i++)
121  sensedData[i].sensorPose=newSensorPose;
122 }
123 
124 
125 void CObservationRange::getDescriptionAsText(std::ostream &o) const
126 {
127  using namespace std;
129 
130  o << "minSensorDistance = " << minSensorDistance << " m" << endl;
131  o << "maxSensorDistance = " << maxSensorDistance << " m" << endl;
132  o << "sensorConeApperture = " << RAD2DEG(sensorConeApperture) << " deg" << endl;
133 
134  // For each entry in this sequence:
135  o << " SENSOR_ID RANGE (m) SENSOR POSE (on the robot)" << endl;
136  o << "-------------------------------------------------------" << endl;
137  for (size_t q=0;q<sensedData.size();q++)
138  {
139  o << format(" %7u",(unsigned int)sensedData[q].sensorID );
140  o << format(" %4.03f ",sensedData[q].sensedDistance);
141  o << sensedData[q].sensorPose << endl;
142  }
143 }
144 
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
GLenum GLsizei n
Definition: glext.h:4618
STL namespace.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
int version
Definition: mrpt_jpeglib.h:898
#define DEG2RAD
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#define RAD2DEG
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
GLuint in
Definition: glext.h:6301
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
unsigned __int32 uint32_t
Definition: rptypes.h:49
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019