MRPT  1.9.9
CObservationBeaconRanges.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 #include <mrpt/system/os.h>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::poses;
18 
19 // This must be added to any CSerializable class implementation file.
21 
22 /** Default constructor.
23  */
25  : sensedData(), auxEstimatePose()
26 {
27 }
28 
32 {
33  uint32_t i, n;
34  // The data
35  out << minSensorDistance << maxSensorDistance << stdError;
36  n = sensedData.size();
37  out << n;
38  for (i = 0; i < n; i++)
39  out << sensedData[i].sensorLocationOnRobot
40  << sensedData[i].sensedDistance << sensedData[i].beaconID;
41  out << auxEstimatePose;
42  out << sensorLabel << timestamp;
43 }
44 
47 {
48  switch (version)
49  {
50  case 0:
51  case 1:
52  case 2:
53  case 3:
54  {
55  uint32_t i, n, id;
56 
57  // The data
58  in >> minSensorDistance >> maxSensorDistance >> stdError;
59 
60  in >> n;
61  sensedData.resize(n);
62  for (i = 0; i < n; i++)
63  {
64  in >> sensedData[i].sensorLocationOnRobot >>
65  sensedData[i].sensedDistance;
66  in >> id;
67  sensedData[i].beaconID = id;
68  }
69 
70  if (version >= 1) in >> auxEstimatePose;
71 
72  if (version >= 2)
73  in >> sensorLabel;
74  else
75  sensorLabel = "";
76 
77  if (version >= 3)
78  in >> timestamp;
79  else
80  timestamp = INVALID_TIMESTAMP;
81  }
82  break;
83  default:
85  };
86 }
87 
89 {
90  printf("[CObservationBeaconRanges::debugPrintOut] Dumping:\n");
91  printf(
92  "[CObservationBeaconRanges::debugPrintOut] minSensorDistance:\t%f\n",
93  minSensorDistance);
94  printf(
95  "[CObservationBeaconRanges::debugPrintOut] maxSensorDistance:\t%f:\n",
96  maxSensorDistance);
97  printf(
98  "[CObservationBeaconRanges::debugPrintOut] stdError:\t%f\n", stdError);
99  printf(
100  "[CObservationBeaconRanges::debugPrintOut] %u ranges:\n",
101  static_cast<unsigned>(sensedData.size()));
102 
103  size_t i, n = sensedData.size();
104  for (i = 0; i < n; i++)
105  printf(
106  "[CObservationBeaconRanges::debugPrintOut] \tID[%u]: %f\n",
107  sensedData[i].beaconID, sensedData[i].sensedDistance);
108 }
109 
110 /*---------------------------------------------------------------
111  getSensorPose
112  ---------------------------------------------------------------*/
114 {
115  if (!sensedData.empty())
116  out_sensorPose = CPose3D(sensedData[0].sensorLocationOnRobot);
117  else
118  out_sensorPose = CPose3D(0, 0, 0);
119 }
120 
121 /*---------------------------------------------------------------
122  setSensorPose
123  ---------------------------------------------------------------*/
125 {
126  size_t i, n = sensedData.size();
127  if (n)
128  for (i = 0; i < n; i++)
129  sensedData[i].sensorLocationOnRobot = CPoint3D(newSensorPose);
130 }
131 
132 /*---------------------------------------------------------------
133  getSensedRangeByBeaconID
134  ---------------------------------------------------------------*/
136 {
137  for (auto& i : sensedData)
138  if (i.beaconID == beaconID) return i.sensedDistance;
139  return 0;
140 }
141 
143 {
144  using namespace std;
146 
147  o << "Auxiliary estimated pose (if available): " << auxEstimatePose << endl;
148 
149  o << format("minSensorDistance=%f m\n", minSensorDistance);
150  o << format("maxSensorDistance=%f m\n", maxSensorDistance);
151  o << format("stdError=%f m\n\n", stdError);
152 
153  o << format(
154  "There are %u range measurements:\n\n", (unsigned)sensedData.size());
155 
156  o << " BEACON RANGE SENSOR POSITION ON ROBOT \n";
157  o << "------------------------------------------------\n";
158  for (const auto& it : sensedData)
159  {
160  o << format(
161  " %i %.04f (%.03f,%.03f,%.03f)\n", (int)it.beaconID,
162  it.sensedDistance, it.sensorLocationOnRobot.x(),
163  it.sensorLocationOnRobot.y(), it.sensorLocationOnRobot.z());
164  }
165 }
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
GLenum GLsizei n
Definition: glext.h:5136
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
STL namespace.
float getSensedRangeByBeaconID(int32_t beaconID)
Easy look-up into the vector sensedData, returns the range for a given beacon, or 0 if the beacon is ...
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.
A class used to store a 3D point.
Definition: CPoint3D.h:31
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.
__int32 int32_t
Definition: rptypes.h:49
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
GLuint id
Definition: glext.h:3920
GLuint in
Definition: glext.h:7391
void debugPrintOut()
Prints out the contents of the object.
unsigned __int32 uint32_t
Definition: rptypes.h:50
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#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: 928d0abbd Sun Oct 13 02:28:49 2019 +0200 at dom oct 13 02:30:11 CEST 2019