MRPT  2.0.1
CObservationGPS.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-2020, 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 
12 #include <mrpt/math/matrix_serialization.h> // for << of matrices
15 #include <iomanip>
16 
17 using namespace std;
18 using namespace mrpt;
19 using namespace mrpt::obs;
20 using namespace mrpt::math;
21 
22 // This must be added to any CSerializable class implementation file.
24 
26 {
27  std::swap(timestamp, o.timestamp);
28  std::swap(originalReceivedTimestamp, o.originalReceivedTimestamp);
29  std::swap(has_satellite_timestamp, o.has_satellite_timestamp);
30  std::swap(sensorLabel, o.sensorLabel);
31  std::swap(sensorPose, o.sensorPose);
32  messages.swap(o.messages);
33 }
34 
35 uint8_t CObservationGPS::serializeGetVersion() const { return 11; }
36 void CObservationGPS::serializeTo(mrpt::serialization::CArchive& out) const
37 {
38  out << timestamp << originalReceivedTimestamp << sensorLabel << sensorPose;
39  out << has_satellite_timestamp; // v11
40 
41  const uint32_t nMsgs = messages.size();
42  out << nMsgs;
43  for (const auto& message : messages) message.second->writeToStream(out);
44 }
45 
46 void CObservationGPS::serializeFrom(
47  mrpt::serialization::CArchive& in, uint8_t version)
48 {
49  this->clear();
50 
51  switch (version)
52  {
53  case 10:
54  case 11:
55  {
56  in >> timestamp >> originalReceivedTimestamp >> sensorLabel >>
57  sensorPose;
58  if (version >= 11)
59  in >> has_satellite_timestamp; // v11
60  else
61  has_satellite_timestamp =
62  (this->timestamp != this->originalReceivedTimestamp);
63 
64  uint32_t nMsgs;
65  in >> nMsgs;
66  for (unsigned i = 0; i < nMsgs; i++)
67  {
68  gnss::gnss_message* msg =
69  gnss::gnss_message::readAndBuildFromStream(in);
70  messages[msg->message_type] = gnss::gnss_message_ptr(msg);
71  }
72  };
73  break;
74 
75  // OLD VERSIONS: Ensure we can load datasets from many years ago
76  // ==========
77  case 0:
78  {
79  bool has_GGA_datum_;
80  in >> has_GGA_datum_;
81  if (has_GGA_datum_)
82  {
83  auto* datum = new gnss::Message_NMEA_GGA();
84  in.ReadBuffer(&datum->fields, sizeof(datum->fields));
85  messages[gnss::NMEA_GGA] = gnss::gnss_message_ptr(datum);
86  }
87 
88  bool has_RMC_datum_;
89  in >> has_RMC_datum_;
90  if (has_RMC_datum_)
91  {
92  auto* datum = new gnss::Message_NMEA_RMC();
93  in.ReadBuffer(&datum->fields, sizeof(datum->fields));
94  messages[gnss::NMEA_RMC] = gnss::gnss_message_ptr(datum);
95  }
96  }
97  break;
98  case 1:
99  case 2:
100  case 3:
101  case 4:
102  case 5:
103  case 6:
104  case 7:
105  case 8:
106  case 9:
107  {
108  if (version >= 3)
109  in >> timestamp;
110  else
111  timestamp = INVALID_TIMESTAMP;
112 
113  bool has_GGA_datum_;
114  in >> has_GGA_datum_;
115  if (has_GGA_datum_)
116  {
118  gnss::Message_NMEA_GGA::content_t& GGA_datum = datum.fields;
119 
120  MRPT_READ_POD(in, GGA_datum.UTCTime.hour);
121  MRPT_READ_POD(in, GGA_datum.UTCTime.minute);
122  MRPT_READ_POD(in, GGA_datum.UTCTime.sec);
123  MRPT_READ_POD(in, GGA_datum.latitude_degrees);
124  MRPT_READ_POD(in, GGA_datum.longitude_degrees);
125  MRPT_READ_POD(in, GGA_datum.fix_quality);
126  MRPT_READ_POD(in, GGA_datum.altitude_meters);
127  if (version >= 9)
128  {
129  MRPT_READ_POD(in, GGA_datum.geoidal_distance);
130  MRPT_READ_POD(in, GGA_datum.orthometric_altitude);
132  }
133  else
134  {
135  GGA_datum.geoidal_distance = 0.0f;
136  GGA_datum.orthometric_altitude = 0.0f;
137  GGA_datum.corrected_orthometric_altitude = 0.0f;
138  }
139 
140  MRPT_READ_POD(in, GGA_datum.satellitesUsed);
141  MRPT_READ_POD(in, GGA_datum.thereis_HDOP);
142  MRPT_READ_POD(in, GGA_datum.HDOP);
143  this->setMsg(datum);
144  }
145 
146  bool has_RMC_datum_;
147  in >> has_RMC_datum_;
148  if (has_RMC_datum_)
149  {
151  gnss::Message_NMEA_RMC::content_t& RMC_datum = datum.fields;
152 
153  MRPT_READ_POD(in, RMC_datum.UTCTime.hour);
154  MRPT_READ_POD(in, RMC_datum.UTCTime.minute);
155  MRPT_READ_POD(in, RMC_datum.UTCTime.sec);
156  MRPT_READ_POD(in, RMC_datum.validity_char);
157  MRPT_READ_POD(in, RMC_datum.latitude_degrees);
158  MRPT_READ_POD(in, RMC_datum.longitude_degrees);
159  MRPT_READ_POD(in, RMC_datum.speed_knots);
160  MRPT_READ_POD(in, RMC_datum.direction_degrees);
161  this->setMsg(datum);
162  }
163  if (version > 1)
164  in >> sensorLabel;
165  else
166  sensorLabel = "";
167  if (version >= 4)
168  in >> sensorPose;
169  else
170  sensorPose.setFromValues(0, 0, 0, 0, 0, 0);
171  if (version >= 5)
172  {
173  bool has_PZS_datum_;
174  in >> has_PZS_datum_;
175  if (has_PZS_datum_)
176  {
177  auto* datum = new gnss::Message_TOPCON_PZS();
178  messages[gnss::TOPCON_PZS] = gnss::gnss_message_ptr(datum);
179  gnss::Message_TOPCON_PZS& PZS_datum = *datum;
180 
181  MRPT_READ_POD(in, PZS_datum.latitude_degrees);
182  MRPT_READ_POD(in, PZS_datum.longitude_degrees);
183  MRPT_READ_POD(in, PZS_datum.height_meters);
184  MRPT_READ_POD(in, PZS_datum.RTK_height_meters);
185  MRPT_READ_POD(in, PZS_datum.PSigma);
186  MRPT_READ_POD(in, PZS_datum.angle_transmitter);
187  MRPT_READ_POD(in, PZS_datum.nId);
188  MRPT_READ_POD(in, PZS_datum.Fix);
189  MRPT_READ_POD(in, PZS_datum.TXBattery);
190  MRPT_READ_POD(in, PZS_datum.RXBattery);
191  MRPT_READ_POD(in, PZS_datum.error);
192  // extra data?
193  if (version >= 6)
194  {
195  MRPT_READ_POD(in, PZS_datum.hasCartesianPosVel);
196  MRPT_READ_POD(in, PZS_datum.cartesian_x);
197  MRPT_READ_POD(in, PZS_datum.cartesian_y);
198  MRPT_READ_POD(in, PZS_datum.cartesian_z);
199  MRPT_READ_POD(in, PZS_datum.cartesian_vx);
200  MRPT_READ_POD(in, PZS_datum.cartesian_vy);
201  MRPT_READ_POD(in, PZS_datum.cartesian_vz);
202  MRPT_READ_POD(in, PZS_datum.hasPosCov);
204  &PZS_datum.pos_covariance(0, 0),
205  PZS_datum.pos_covariance.size());
206  MRPT_READ_POD(in, PZS_datum.hasVelCov);
208  &PZS_datum.vel_covariance(0, 0),
209  PZS_datum.vel_covariance.size());
210  MRPT_READ_POD(in, PZS_datum.hasStats);
211  MRPT_READ_POD(in, PZS_datum.stats_GPS_sats_used);
212  MRPT_READ_POD(in, PZS_datum.stats_GLONASS_sats_used);
213  if (version >= 8)
214  MRPT_READ_POD(in, PZS_datum.stats_rtk_fix_progress);
215  else
216  PZS_datum.stats_rtk_fix_progress = 0;
217  }
218  else
219  {
220  PZS_datum.hasCartesianPosVel = PZS_datum.hasPosCov =
221  PZS_datum.hasVelCov = PZS_datum.hasStats = false;
222  }
223  }
224  } // end version >=5
225 
226  // Added in V7:
227  if (version >= 7)
228  {
229  auto* datum = new gnss::Message_TOPCON_SATS();
230  messages[gnss::TOPCON_SATS] = gnss::gnss_message_ptr(datum);
231  bool has_SATS_datum_;
232  in >> has_SATS_datum_;
233  if (has_SATS_datum_)
234  {
236  "Reading TOPCON_SATS is broken: non-trivial POD data "
237  "read.");
238  // was: MRPT_READ_POD(in, SATS_datum.USIs);
239  // gnss::Message_TOPCON_SATS& SATS_datum = *datum;
240  }
241  }
242  }
243  break;
244  default:
246  };
247 
248  if (version < 10 && originalReceivedTimestamp == INVALID_TIMESTAMP)
249  originalReceivedTimestamp = timestamp;
250 }
251 
252 void CObservationGPS::dumpToStream(std::ostream& out) const
253 {
254  out << "\n------------- [CObservationGPS] Dump of " << messages.size()
255  << " messages --------------------\n";
256  for (const auto& m : messages) m.second->dumpToStream(out);
257  out << "-------------- [CObservationGPS] End of dump -----------------\n\n";
258 }
259 
260 void CObservationGPS::dumpToConsole(std::ostream& o) const
261 {
262  this->dumpToStream(o);
263 }
264 
265 mrpt::system::TTimeStamp CObservationGPS::getOriginalReceivedTimeStamp() const
266 {
267  return originalReceivedTimestamp;
268 }
269 
271 {
272  messages.clear();
273  timestamp = INVALID_TIMESTAMP;
274  originalReceivedTimestamp = INVALID_TIMESTAMP;
275 }
276 void CObservationGPS::getDescriptionAsText(std::ostream& o) const
277 {
278  using namespace mrpt::system; // for the TTimeStamp << operator
279 
280  CObservation::getDescriptionAsText(o);
281 
282  o << "Timestamp (UTC) of reception at the computer: "
283  << mrpt::system::dateTimeToString(originalReceivedTimestamp) << std::endl;
284  o << " (as time_t): " << std::fixed << std::setprecision(5)
285  << mrpt::system::timestampTotime_t(originalReceivedTimestamp)
286  << std::endl;
287  o << " (as TTimestamp): " << originalReceivedTimestamp << std::endl;
288 
289  o << "Sensor position on the robot/vehicle: " << sensorPose << std::endl;
290 
291  this->dumpToConsole(o);
292 }
293 
294 bool CObservationGPS::hasMsgType(const gnss::gnss_message_type_t type_id) const
295 {
296  return messages.find(type_id) != messages.end();
297 }
298 
299 mrpt::obs::gnss::gnss_message* CObservationGPS::getMsgByType(
300  const gnss::gnss_message_type_t type_id)
301 {
302  auto it = messages.find(type_id);
303  ASSERTMSG_(
304  it != messages.end(), mrpt::format(
305  "[CObservationGPS::getMsgByType] Cannot find "
306  "any observation of type `%u`",
307  static_cast<unsigned int>(type_id)));
308  return it->second.get();
309 }
310 /** \overload */
311 const mrpt::obs::gnss::gnss_message* CObservationGPS::getMsgByType(
312  const gnss::gnss_message_type_t type_id) const
313 {
314  auto it = messages.find(type_id);
315  ASSERTMSG_(
316  it != messages.end(), mrpt::format(
317  "[CObservationGPS::getMsgByType] Cannot find "
318  "any observation of type `%u`",
319  static_cast<unsigned int>(type_id)));
320  return it->second.get();
321 }
322 
323 // From: http://gnsstk.sourceforge.net/time__conversion_8c-source.html
324 #define TIMECONV_JULIAN_DATE_START_OF_GPS_TIME (2444244.5) // [days]
326  const unsigned short gps_week, const double gps_tow,
327  const unsigned int utc_offset, double* julian_date)
328 {
329  if (gps_tow < 0.0 || gps_tow > 604800.0) return false;
330  // GPS time is ahead of UTC time and Julian time by the UTC offset
331  *julian_date = (gps_week + (gps_tow - utc_offset) / 604800.0) * 7.0 +
333  return true;
334 }
335 
336 bool TIMECONV_IsALeapYear(const unsigned short year)
337 {
338  bool is_a_leap_year = false;
339  if ((year % 4) == 0)
340  {
341  is_a_leap_year = true;
342  if ((year % 100) == 0)
343  {
344  if ((year % 400) == 0)
345  is_a_leap_year = true;
346  else
347  is_a_leap_year = false;
348  }
349  }
350  return is_a_leap_year;
351 }
352 
354  /** Universal Time Coordinated [year] */
355  const unsigned short year,
356  /** Universal Time Coordinated [1-12 months] */
357  const unsigned char month,
358  /** Days in the specified month [1-28|29|30|31 days] */
359  unsigned char* days_in_month)
360 {
361  unsigned char utmp = 0;
362  bool is_a_leapyear = TIMECONV_IsALeapYear(year);
363 
364  switch (month)
365  {
366  case 1:
367  utmp = 31;
368  break;
369  case 2:
370  if (is_a_leapyear)
371  {
372  utmp = 29;
373  }
374  else
375  {
376  utmp = 28;
377  }
378  break;
379  case 3:
380  utmp = 31;
381  break;
382  case 4:
383  utmp = 30;
384  break;
385  case 5:
386  utmp = 31;
387  break;
388  case 6:
389  utmp = 30;
390  break;
391  case 7:
392  utmp = 31;
393  break;
394  case 8:
395  utmp = 31;
396  break;
397  case 9:
398  utmp = 30;
399  break;
400  case 10:
401  utmp = 31;
402  break;
403  case 11:
404  utmp = 30;
405  break;
406  case 12:
407  utmp = 31;
408  break;
409  default:
410  return false;
411  break;
412  }
413  *days_in_month = utmp;
414  return true;
415 }
416 /** Number of days since noon Universal Time Jan 1, 4713 BCE (Julian calendar)
417  * [days] */
419  const double julian_date, mrpt::system::TTimeParts& utc)
420 {
421  int a, b, c, d, e; // temporary values
422 
423  unsigned short year;
424  unsigned char month;
425  unsigned char day;
426  unsigned char hour;
427  unsigned char minute;
428  unsigned char days_in_month = 0;
429  double td; // temporary double
430  double seconds;
431  bool result;
432 
433  // Check the input.
434  if (julian_date < 0.0) return false;
435 
436  a = lround(julian_date);
437  b = a + 1537;
438  c = (int)(((double)b - 122.1) / 365.25);
439  d = (int)(365.25 * c);
440  e = (int)(((double)(b - d)) / 30.6001);
441 
442  td = b - d - (int)(30.6001 * e) + fmod(julian_date + 0.5, 1.0); // [days]
443  day = (unsigned char)td;
444  td -= day;
445  td *= 24.0; // [hours]
446  hour = (unsigned char)td;
447  td -= hour;
448  td *= 60.0; // [minutes]
449  minute = (unsigned char)td;
450  td -= minute;
451  td *= 60.0; // [s]
452  seconds = td;
453  month = (unsigned char)(e - 1 - 12 * (int)(e / 14));
454  year = (unsigned short)(c - 4715 - (int)((7.0 + (double)month) / 10.0));
455  // check for rollover issues
456  if (seconds >= 60.0)
457  {
458  seconds -= 60.0;
459  minute++;
460  if (minute >= 60)
461  {
462  minute -= 60;
463  hour++;
464  if (hour >= 24)
465  {
466  hour -= 24;
467  day++;
469  year, month, &days_in_month);
470  if (result == false) return false;
471  if (day > days_in_month)
472  {
473  day = 1;
474  month++;
475  if (month > 12)
476  {
477  month = 1;
478  year++;
479  }
480  }
481  }
482  }
483  }
484 
485  utc.year = year;
486  utc.month = month;
487  utc.day = day;
488  utc.hour = hour;
489  utc.minute = minute;
490  utc.second = seconds;
491  return true;
492 }
493 
494 bool CObservationGPS::GPS_time_to_UTC(
495  uint16_t gps_week, double gps_sec, const int leap_seconds_count,
496  mrpt::system::TTimeStamp& utc_out)
497 {
499  if (!GPS_time_to_UTC(gps_week, gps_sec, leap_seconds_count, tim))
500  return false;
502  return true;
503 }
504 
505 bool CObservationGPS::GPS_time_to_UTC(
506  uint16_t gps_week, double gps_sec, const int leap_seconds_count,
507  mrpt::system::TTimeParts& utc_out)
508 {
509  double julian_date = 0.0;
510  if (gps_sec < 0.0 || gps_sec > 604800.0) return false;
512  gps_week, gps_sec, leap_seconds_count, &julian_date))
513  return false;
514  if (!TIMECONV_GetUTCTimeFromJulianDate(julian_date, utc_out)) return false;
515  return true;
516 }
double angle_transmitter
Vertical angle of N-beam.
uint8_t stats_rtk_fix_progress
[0,100] %, only in modes other than RTK FIXED.
uint8_t fix_quality
NMEA standard values: 0 = invalid, 1 = GPS fix (SPS), 2 = DGPS fix, 3 = PPS fix, 4 = Real Time Kinema...
double height_meters
ellipsoidal height from N-beam [m] perhaps weighted with regular gps
content_t fields
Message content, accesible by individual fields.
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
constexpr matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CMatrixFixed.h:233
TopCon mmGPS devices: SATS, a generic structure for statistics about tracked satelites and their posi...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::math::CMatrixFloat44 vel_covariance
Only if hasPosCov is true.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
gnss_message_type_t
List of all known GNSS message types.
mrpt::system::TTimeStamp buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
Definition: datetime.cpp:74
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
STL namespace.
bool TIMECONV_GetJulianDateFromGPSTime(const unsigned short gps_week, const double gps_tow, const unsigned int utc_offset, double *julian_date)
bool TIMECONV_IsALeapYear(const unsigned short year)
GPS datum for TopCon&#39;s mmGPS devices: PZS.
int8_t validity_char
This will be: &#39;A&#39;=OK or &#39;V&#39;=void.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
uint8_t nId
ID of the transmitter [1-4], 0 if none.
double orthometric_altitude
The measured orthometric altitude, in meters (A)+(B).
bool hasCartesianPosVel
system error indicator
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
double altitude_meters
The measured altitude, in meters (A).
bool thereis_HDOP
This states whether to take into account the value in the HDOP field.
This base provides a set of functions for maths stuff.
double corrected_orthometric_altitude
The corrected (only for TopCon mmGPS) orthometric altitude, in meters mmGPS(A+B). ...
#define MRPT_READ_POD(_STREAM, _VARIABLE)
Definition: CArchive.h:445
mrpt::math::CMatrixFloat44 pos_covariance
Only if hasPosCov is true.
bool TIMECONV_GetNumberOfDaysInMonth(const unsigned short year, const unsigned char month, unsigned char *days_in_month)
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:49
float HDOP
The HDOP (Horizontal Dilution of Precision) as returned by the sensor.
double timestampTotime_t(const mrpt::system::TTimeStamp t) noexcept
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.h:105
uint8_t day
Month (1-12)
Definition: datetime.h:53
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
This namespace contains representation of robot actions and observations.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
gnss_message_type_t message_type
Type of GNSS message.
uint8_t RXBattery
battery level on receiver
double second
Minute (0-59)
Definition: datetime.h:56
double cartesian_x
Only if hasCartesianPosVel is true.
double geoidal_distance
Undulation: Difference between the measured altitude and the geoid, in meters (B).
bool TIMECONV_GetUTCTimeFromJulianDate(const double julian_date, mrpt::system::TTimeParts &utc)
Number of days since noon Universal Time Jan 1, 4713 BCE (Julian calendar) [days].
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
uint8_t minute
Hour (0-23)
Definition: datetime.h:55
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
Pure virtual base for all message types.
std::string dateTimeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC time): YEAR/MONTH/DAY,HH:MM:SS.MMM.
Definition: datetime.cpp:154
This file implements matrix/vector text and binary serialization.
double direction_degrees
Measured speed direction (in degrees)
content_t fields
Message content, accesible by individual fields.
uint8_t month
The year.
Definition: datetime.h:52
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
uint8_t hour
Day (1-31)
Definition: datetime.h:54
#define TIMECONV_JULIAN_DATE_START_OF_GPS_TIME
uint8_t TXBattery
battery level on transmitter
double cartesian_vx
Only if hasCartesianPosVel is true.
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
double RTK_height_meters
ellipsoidal height [m] without N-beam correction
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
A smart pointer to a GNSS message.
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020