Main MRPT website > C++ reference for MRPT 1.9.9
CObservationGPS.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 #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 (message_list_t::const_iterator it = messages.begin();
44  it != messages.end(); ++it)
45  it->second->writeToStream(out);
46 }
47 
48 void CObservationGPS::serializeFrom(
50 {
51  this->clear();
52 
53  switch (version)
54  {
55  case 10:
56  case 11:
57  {
58  in >> timestamp >> originalReceivedTimestamp >> sensorLabel >>
59  sensorPose;
60  if (version >= 11)
61  in >> has_satellite_timestamp; // v11
62  else
63  has_satellite_timestamp =
64  (this->timestamp != this->originalReceivedTimestamp);
65 
66  uint32_t nMsgs;
67  in >> nMsgs;
68  for (unsigned i = 0; i < nMsgs; i++)
69  {
70  gnss::gnss_message* msg =
71  gnss::gnss_message::readAndBuildFromStream(in);
72  messages[msg->message_type] = gnss::gnss_message_ptr(msg);
73  }
74  };
75  break;
76 
77  // OLD VERSIONS: Ensure we can load datasets from many years ago
78  // ==========
79  case 0:
80  {
81  bool has_GGA_datum_;
82  in >> has_GGA_datum_;
83  if (has_GGA_datum_)
84  {
86  in.ReadBuffer(&datum->fields, sizeof(datum->fields));
87  messages[gnss::NMEA_GGA] = gnss::gnss_message_ptr(datum);
88  }
89 
90  bool has_RMC_datum_;
91  in >> has_RMC_datum_;
92  if (has_RMC_datum_)
93  {
95  in.ReadBuffer(&datum->fields, sizeof(datum->fields));
96  messages[gnss::NMEA_RMC] = gnss::gnss_message_ptr(datum);
97  }
98  }
99  break;
100  case 1:
101  case 2:
102  case 3:
103  case 4:
104  case 5:
105  case 6:
106  case 7:
107  case 8:
108  case 9:
109  {
110  if (version >= 3)
111  in >> timestamp;
112  else
113  timestamp = INVALID_TIMESTAMP;
114 
115  bool has_GGA_datum_;
116  in >> has_GGA_datum_;
117  if (has_GGA_datum_)
118  {
120  gnss::Message_NMEA_GGA::content_t& GGA_datum = datum.fields;
121 
122  MRPT_READ_POD(in, GGA_datum.UTCTime.hour);
123  MRPT_READ_POD(in, GGA_datum.UTCTime.minute);
124  MRPT_READ_POD(in, GGA_datum.UTCTime.sec);
125  MRPT_READ_POD(in, GGA_datum.latitude_degrees);
126  MRPT_READ_POD(in, GGA_datum.longitude_degrees);
127  MRPT_READ_POD(in, GGA_datum.fix_quality);
128  MRPT_READ_POD(in, GGA_datum.altitude_meters);
129  if (version >= 9)
130  {
131  MRPT_READ_POD(in, GGA_datum.geoidal_distance);
134  }
135  else
136  {
137  GGA_datum.geoidal_distance = 0.0f;
138  GGA_datum.orthometric_altitude = 0.0f;
139  GGA_datum.corrected_orthometric_altitude = 0.0f;
140  }
141 
142  MRPT_READ_POD(in, GGA_datum.satellitesUsed);
143  MRPT_READ_POD(in, GGA_datum.thereis_HDOP);
144  MRPT_READ_POD(in, GGA_datum.HDOP);
145  this->setMsg(datum);
146  }
147 
148  bool has_RMC_datum_;
149  in >> has_RMC_datum_;
150  if (has_RMC_datum_)
151  {
153  gnss::Message_NMEA_RMC::content_t& RMC_datum = datum.fields;
154 
155  MRPT_READ_POD(in, RMC_datum.UTCTime.hour);
156  MRPT_READ_POD(in, RMC_datum.UTCTime.minute);
157  MRPT_READ_POD(in, RMC_datum.UTCTime.sec);
158  MRPT_READ_POD(in, RMC_datum.validity_char);
159  MRPT_READ_POD(in, RMC_datum.latitude_degrees);
160  MRPT_READ_POD(in, RMC_datum.longitude_degrees);
161  MRPT_READ_POD(in, RMC_datum.speed_knots);
162  MRPT_READ_POD(in, RMC_datum.direction_degrees);
163  this->setMsg(datum);
164  }
165  if (version > 1)
166  in >> sensorLabel;
167  else
168  sensorLabel = "";
169  if (version >= 4)
170  in >> sensorPose;
171  else
172  sensorPose.setFromValues(0, 0, 0, 0, 0, 0);
173  if (version >= 5)
174  {
175  bool has_PZS_datum_;
176  in >> has_PZS_datum_;
177  if (has_PZS_datum_)
178  {
179  gnss::Message_TOPCON_PZS* datum =
181  messages[gnss::TOPCON_PZS] = gnss::gnss_message_ptr(datum);
182  gnss::Message_TOPCON_PZS& PZS_datum = *datum;
183 
184  MRPT_READ_POD(in, PZS_datum.latitude_degrees);
185  MRPT_READ_POD(in, PZS_datum.longitude_degrees);
186  MRPT_READ_POD(in, PZS_datum.height_meters);
187  MRPT_READ_POD(in, PZS_datum.RTK_height_meters);
188  MRPT_READ_POD(in, PZS_datum.PSigma);
189  MRPT_READ_POD(in, PZS_datum.angle_transmitter);
190  MRPT_READ_POD(in, PZS_datum.nId);
191  MRPT_READ_POD(in, PZS_datum.Fix);
192  MRPT_READ_POD(in, PZS_datum.TXBattery);
193  MRPT_READ_POD(in, PZS_datum.RXBattery);
194  MRPT_READ_POD(in, PZS_datum.error);
195  // extra data?
196  if (version >= 6)
197  {
198  MRPT_READ_POD(in, PZS_datum.hasCartesianPosVel);
199  MRPT_READ_POD(in, PZS_datum.cartesian_x);
200  MRPT_READ_POD(in, PZS_datum.cartesian_y);
201  MRPT_READ_POD(in, PZS_datum.cartesian_z);
202  MRPT_READ_POD(in, PZS_datum.cartesian_vx);
203  MRPT_READ_POD(in, PZS_datum.cartesian_vy);
204  MRPT_READ_POD(in, PZS_datum.cartesian_vz);
205  MRPT_READ_POD(in, PZS_datum.hasPosCov);
206  MRPT_READ_POD(in, PZS_datum.pos_covariance);
207  MRPT_READ_POD(in, PZS_datum.hasVelCov);
208  MRPT_READ_POD(in, PZS_datum.vel_covariance);
209  MRPT_READ_POD(in, PZS_datum.hasStats);
212  if (version >= 8)
214  else
215  PZS_datum.stats_rtk_fix_progress = 0;
216  }
217  else
218  {
219  PZS_datum.hasCartesianPosVel = PZS_datum.hasPosCov =
220  PZS_datum.hasVelCov = PZS_datum.hasStats = false;
221  }
222  }
223  } // end version >=5
224 
225  // Added in V7:
226  if (version >= 7)
227  {
230  messages[gnss::TOPCON_SATS] = gnss::gnss_message_ptr(datum);
231  gnss::Message_TOPCON_SATS& SATS_datum = *datum;
232  bool has_SATS_datum_;
233  in >> has_SATS_datum_;
234  if (has_SATS_datum_)
235  {
236  MRPT_READ_POD(in, SATS_datum.USIs);
237  MRPT_READ_POD(in, SATS_datum.ELs);
238  MRPT_READ_POD(in, SATS_datum.AZs);
239  }
240  }
241  }
242  break;
243  default:
245  };
246 
247  if (originalReceivedTimestamp == INVALID_TIMESTAMP)
248  originalReceivedTimestamp = timestamp;
249 }
250 
251 void CObservationGPS::dumpToStream(std::ostream& out) const
252 {
253  out << "\n------------- [CObservationGPS] Dump of " << messages.size()
254  << " messages --------------------\n";
255  for (const auto& m : messages) m.second->dumpToStream(out);
256  out << "-------------- [CObservationGPS] End of dump -----------------\n\n";
257 }
258 
259 void CObservationGPS::dumpToConsole(std::ostream& o) const
260 {
261  this->dumpToStream(o);
262 }
263 
264 mrpt::system::TTimeStamp CObservationGPS::getOriginalReceivedTimeStamp() const
265 {
266  return originalReceivedTimestamp;
267 }
268 
270 {
271  messages.clear();
272  timestamp = INVALID_TIMESTAMP;
273  originalReceivedTimestamp = INVALID_TIMESTAMP;
274 }
275 void CObservationGPS::getDescriptionAsText(std::ostream& o) const
276 {
277  CObservation::getDescriptionAsText(o);
278 
279  o << "Timestamp (UTC) of reception at the computer: "
280  << mrpt::system::dateTimeToString(originalReceivedTimestamp) << std::endl;
281  o << " (as time_t): " << std::fixed << std::setprecision(5)
282  << mrpt::system::timestampTotime_t(originalReceivedTimestamp)
283  << std::endl;
284  o << " (as TTimestamp): " << originalReceivedTimestamp << std::endl;
285 
286  o << "Sensor position on the robot/vehicle: " << sensorPose << std::endl;
287 
288  this->dumpToConsole(o);
289 }
290 
291 bool CObservationGPS::hasMsgType(const gnss::gnss_message_type_t type_id) const
292 {
293  return messages.find(type_id) != messages.end();
294 }
295 
296 mrpt::obs::gnss::gnss_message* CObservationGPS::getMsgByType(
297  const gnss::gnss_message_type_t type_id)
298 {
299  message_list_t::iterator it = messages.find(type_id);
300  ASSERTMSG_(
301  it != messages.end(), mrpt::format(
302  "[CObservationGPS::getMsgByType] Cannot find "
303  "any observation of type `%u`",
304  static_cast<unsigned int>(type_id)));
305  return it->second.get();
306 }
307 /** \overload */
308 const mrpt::obs::gnss::gnss_message* CObservationGPS::getMsgByType(
309  const gnss::gnss_message_type_t type_id) const
310 {
311  message_list_t::const_iterator it = messages.find(type_id);
312  ASSERTMSG_(
313  it != messages.end(), mrpt::format(
314  "[CObservationGPS::getMsgByType] Cannot find "
315  "any observation of type `%u`",
316  static_cast<unsigned int>(type_id)));
317  return it->second.get();
318 }
319 
320 // From: http://gnsstk.sourceforge.net/time__conversion_8c-source.html
321 #define TIMECONV_JULIAN_DATE_START_OF_GPS_TIME (2444244.5) // [days]
323  const unsigned short gps_week, const double gps_tow,
324  const unsigned int utc_offset, double* julian_date)
325 {
326  if (gps_tow < 0.0 || gps_tow > 604800.0) return false;
327  // GPS time is ahead of UTC time and Julian time by the UTC offset
328  *julian_date = (gps_week + (gps_tow - utc_offset) / 604800.0) * 7.0 +
330  return true;
331 }
332 
333 bool TIMECONV_IsALeapYear(const unsigned short year)
334 {
335  bool is_a_leap_year = false;
336  if ((year % 4) == 0)
337  {
338  is_a_leap_year = true;
339  if ((year % 100) == 0)
340  {
341  if ((year % 400) == 0)
342  is_a_leap_year = true;
343  else
344  is_a_leap_year = false;
345  }
346  }
347  return is_a_leap_year;
348 }
349 
351  /** Universal Time Coordinated [year] */
352  const unsigned short year,
353  /** Universal Time Coordinated [1-12 months] */
354  const unsigned char month,
355  /** Days in the specified month [1-28|29|30|31 days] */
356  unsigned char* days_in_month)
357 {
358  unsigned char utmp = 0;
359  bool is_a_leapyear = TIMECONV_IsALeapYear(year);
360 
361  switch (month)
362  {
363  case 1:
364  utmp = 31;
365  break;
366  case 2:
367  if (is_a_leapyear)
368  {
369  utmp = 29;
370  }
371  else
372  {
373  utmp = 28;
374  }
375  break;
376  case 3:
377  utmp = 31;
378  break;
379  case 4:
380  utmp = 30;
381  break;
382  case 5:
383  utmp = 31;
384  break;
385  case 6:
386  utmp = 30;
387  break;
388  case 7:
389  utmp = 31;
390  break;
391  case 8:
392  utmp = 31;
393  break;
394  case 9:
395  utmp = 30;
396  break;
397  case 10:
398  utmp = 31;
399  break;
400  case 11:
401  utmp = 30;
402  break;
403  case 12:
404  utmp = 31;
405  break;
406  default:
407  return false;
408  break;
409  }
410  *days_in_month = utmp;
411  return true;
412 }
413 /** Number of days since noon Universal Time Jan 1, 4713 BCE (Julian calendar)
414  * [days] */
416  const double julian_date, mrpt::system::TTimeParts& utc)
417 {
418  int a, b, c, d, e; // temporary values
419 
420  unsigned short year;
421  unsigned char month;
422  unsigned char day;
423  unsigned char hour;
424  unsigned char minute;
425  unsigned char days_in_month = 0;
426  double td; // temporary double
427  double seconds;
428  bool result;
429 
430  // Check the input.
431  if (julian_date < 0.0) return false;
432 
433  a = (int)(julian_date + 0.5);
434  b = a + 1537;
435  c = (int)(((double)b - 122.1) / 365.25);
436  d = (int)(365.25 * c);
437  e = (int)(((double)(b - d)) / 30.6001);
438 
439  td = b - d - (int)(30.6001 * e) + fmod(julian_date + 0.5, 1.0); // [days]
440  day = (unsigned char)td;
441  td -= day;
442  td *= 24.0; // [hours]
443  hour = (unsigned char)td;
444  td -= hour;
445  td *= 60.0; // [minutes]
446  minute = (unsigned char)td;
447  td -= minute;
448  td *= 60.0; // [s]
449  seconds = td;
450  month = (unsigned char)(e - 1 - 12 * (int)(e / 14));
451  year = (unsigned short)(c - 4715 - (int)((7.0 + (double)month) / 10.0));
452  // check for rollover issues
453  if (seconds >= 60.0)
454  {
455  seconds -= 60.0;
456  minute++;
457  if (minute >= 60)
458  {
459  minute -= 60;
460  hour++;
461  if (hour >= 24)
462  {
463  hour -= 24;
464  day++;
466  year, month, &days_in_month);
467  if (result == false) return false;
468  if (day > days_in_month)
469  {
470  day = 1;
471  month++;
472  if (month > 12)
473  {
474  month = 1;
475  year++;
476  }
477  }
478  }
479  }
480  }
481 
482  utc.year = year;
483  utc.month = month;
484  utc.day = day;
485  utc.hour = hour;
486  utc.minute = minute;
487  utc.second = seconds;
488  return true;
489 }
490 
491 bool CObservationGPS::GPS_time_to_UTC(
492  uint16_t gps_week, double gps_sec, const int leap_seconds_count,
493  mrpt::system::TTimeStamp& utc_out)
494 {
496  if (!GPS_time_to_UTC(gps_week, gps_sec, leap_seconds_count, tim))
497  return false;
499  return true;
500 }
501 
502 bool CObservationGPS::GPS_time_to_UTC(
503  uint16_t gps_week, double gps_sec, const int leap_seconds_count,
504  mrpt::system::TTimeParts& utc_out)
505 {
506  double julian_date = 0.0;
507  if (gps_sec < 0.0 || gps_sec > 604800.0) return false;
509  gps_week, gps_sec, leap_seconds_count, &julian_date))
510  return false;
511  if (!TIMECONV_GetUTCTimeFromJulianDate(julian_date, utc_out)) return false;
512  return true;
513 }
double angle_transmitter
Vertical angle of N-beam.
std::vector< int16_t > AZs
Azimuth (in degrees, 0-360) for each satellite in USIs.
Scalar * iterator
Definition: eigen_plugins.h:26
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:-)
unsigned __int16 uint16_t
Definition: rptypes.h:44
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
TopCon mmGPS devices: SATS, a generic structure for statistics about tracked satelites and their posi...
mrpt::math::CMatrixFloat44 vel_covariance
Only if hasPosCov is true.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in 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:128
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
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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:417
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)
const GLubyte * c
Definition: glext.h:6313
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:37
float HDOP
The HDOP (Horizontal Dilution of Precision) as returned by the sensor.
uint8_t day
Month (1-12)
Definition: datetime.h:41
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
gnss_message_type_t message_type
Type of GNSS message.
uint8_t RXBattery
battery level on receiver
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
double second
Minute (0-59)
Definition: datetime.h:44
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.
std::vector< uint8_t > USIs
The list of USI (Universal Sat ID) for the detected sats (See GRIL Manual, pag 4-31).
uint8_t minute
Hour (0-23)
Definition: datetime.h:43
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
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:248
This file implements matrix/vector text and binary serialization.
double direction_degrees
Measured speed direction (in degrees)
GLuint in
Definition: glext.h:7274
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
content_t fields
Message content, accesible by individual fields.
uint8_t month
The year.
Definition: datetime.h:40
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
uint8_t hour
Day (1-31)
Definition: datetime.h:42
#define TIMECONV_JULIAN_DATE_START_OF_GPS_TIME
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
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...
unsigned __int32 uint32_t
Definition: rptypes.h:47
double RTK_height_meters
ellipsoidal height [m] without N-beam correction
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
const Scalar * const_iterator
Definition: eigen_plugins.h:27
std::vector< int8_t > ELs
Elevation (in degrees, 0-90) for each satellite in USIs.
double timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.cpp:56
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 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019