Main MRPT website > C++ reference for MRPT 1.5.7
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-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 #include <mrpt/math/matrix_serialization.h> // for << of matrices
16 #include <iomanip>
17 
18 using namespace std;
19 using namespace mrpt;
20 using namespace mrpt::utils;
21 using namespace mrpt::obs;
22 using namespace mrpt::math;
23 
24 // This must be added to any CSerializable class implementation file.
26 
28  sensorPose(),
29  originalReceivedTimestamp(INVALID_TIMESTAMP),
30  has_satellite_timestamp(false),
31  messages(),
32  has_GGA_datum (messages),
33  has_RMC_datum (messages),
34  has_PZS_datum (messages),
35  has_SATS_datum(messages)
36 {
37 }
38 
39 void CObservationGPS::swap(CObservationGPS &o)
40 {
41  std::swap(timestamp, o.timestamp);
42  std::swap(originalReceivedTimestamp, o.originalReceivedTimestamp);
43  std::swap(has_satellite_timestamp,o.has_satellite_timestamp);
44  std::swap(sensorLabel, o.sensorLabel);
45  std::swap(sensorPose, o.sensorPose);
46  messages.swap( o.messages );
47 }
48 
49 /*---------------------------------------------------------------
50  Implements the writing to a CStream capability of CSerializable objects
51  ---------------------------------------------------------------*/
52 void CObservationGPS::writeToStream(mrpt::utils::CStream &out, int *version) const
53 {
54  if (version)
55  *version = 11;
56  else
57  {
58  out << timestamp << originalReceivedTimestamp << sensorLabel << sensorPose;
59  out << has_satellite_timestamp; // v11
60 
61  const uint32_t nMsgs = messages.size();
62  out << nMsgs;
63  for (message_list_t::const_iterator it=messages.begin();it!=messages.end();++it)
64  it->second->writeToStream(out);
65  }
66 }
67 
68 /*---------------------------------------------------------------
69  Implements the reading from a CStream capability of CSerializable objects
70  ---------------------------------------------------------------*/
71 void CObservationGPS::readFromStream(mrpt::utils::CStream &in, int version)
72 {
73  this->clear();
74 
75  switch(version)
76  {
77  case 10:
78  case 11:
79  {
80  in >> timestamp >> originalReceivedTimestamp >> sensorLabel >>sensorPose;
81  if (version>=11)
82  in >> has_satellite_timestamp; // v11
83  else has_satellite_timestamp = (this->timestamp!=this->originalReceivedTimestamp);
84 
85  uint32_t nMsgs;
86  in >> nMsgs;
87  for (unsigned i=0;i<nMsgs;i++) {
88  gnss::gnss_message * msg = gnss::gnss_message::readAndBuildFromStream(in);
89  messages[msg->message_type] = gnss::gnss_message_ptr(msg);
90  }
91  };
92  break;
93 
94  // OLD VERSIONS: Ensure we can load datasets from many years ago ==========
95  case 0:
96  {
97  bool has_GGA_datum_;
98  in >> has_GGA_datum_;
99  if (has_GGA_datum_) {
101  in.ReadBuffer( &datum->fields, sizeof(datum->fields) );
102  messages[gnss::NMEA_GGA] = gnss::gnss_message_ptr(datum);
103  }
104 
105  bool has_RMC_datum_;
106  in >> has_RMC_datum_;
107  if (has_RMC_datum_) {
109  in.ReadBuffer( &datum->fields, sizeof(datum->fields) );
110  messages[gnss::NMEA_RMC] = gnss::gnss_message_ptr(datum);
111  }
112  } break;
113  case 1:
114  case 2:
115  case 3:
116  case 4:
117  case 5:
118  case 6:
119  case 7:
120  case 8:
121  case 9:
122  {
123  if (version>=3)
124  in >> timestamp;
125  else timestamp = INVALID_TIMESTAMP;
126 
127  bool has_GGA_datum_;
128  in >> has_GGA_datum_;
129  if (has_GGA_datum_)
130  {
132  gnss::Message_NMEA_GGA::content_t & GGA_datum = datum.fields;
133 
134  in >> GGA_datum.UTCTime.hour >> GGA_datum.UTCTime.minute >> GGA_datum.UTCTime.sec >> GGA_datum.latitude_degrees
135  >> GGA_datum.longitude_degrees >> GGA_datum.fix_quality >> GGA_datum.altitude_meters;
136  if( version >= 9 ) {
137  in >> GGA_datum.geoidal_distance
138  >> GGA_datum.orthometric_altitude
139  >> GGA_datum.corrected_orthometric_altitude;
140  }
141  else {
142  GGA_datum.geoidal_distance = 0.0f;
143  GGA_datum.orthometric_altitude = 0.0f;
144  GGA_datum.corrected_orthometric_altitude = 0.0f;
145  }
146 
147  in >> GGA_datum.satellitesUsed >> GGA_datum.thereis_HDOP >> GGA_datum.HDOP;
148  this->setMsg(datum);
149  }
150 
151  bool has_RMC_datum_;
152  in >> has_RMC_datum_;
153  if (has_RMC_datum_)
154  {
156  gnss::Message_NMEA_RMC::content_t & RMC_datum = datum.fields;
157 
158  in >> RMC_datum.UTCTime.hour >> RMC_datum.UTCTime.minute >> RMC_datum.UTCTime.sec
159  >> RMC_datum.validity_char >> RMC_datum.latitude_degrees >> RMC_datum.longitude_degrees
160  >> RMC_datum.speed_knots >> RMC_datum.direction_degrees;
161  this->setMsg(datum);
162  }
163  if (version>1)
164  in >> sensorLabel;
165  else sensorLabel = "";
166  if (version>=4)
167  in >> sensorPose;
168  else sensorPose.setFromValues(0,0,0,0,0,0);
169  if (version>=5)
170  {
171  bool has_PZS_datum_;
172  in >> has_PZS_datum_;
173  if (has_PZS_datum_)
174  {
176  messages[gnss::TOPCON_PZS] = gnss::gnss_message_ptr(datum);
177  gnss::Message_TOPCON_PZS & PZS_datum = *datum;
178 
179  in >>
180  PZS_datum.latitude_degrees >> PZS_datum.longitude_degrees >> PZS_datum.height_meters >>
181  PZS_datum.RTK_height_meters >> PZS_datum.PSigma >> PZS_datum.angle_transmitter >> PZS_datum.nId >>
182  PZS_datum.Fix >> PZS_datum.TXBattery >> PZS_datum.RXBattery >> PZS_datum.error;
183  // extra data?
184  if (version>=6) {
185  in >>
186  PZS_datum.hasCartesianPosVel >> PZS_datum.cartesian_x >> PZS_datum.cartesian_y >> PZS_datum.cartesian_z >>
187  PZS_datum.cartesian_vx >> PZS_datum.cartesian_vy >> PZS_datum.cartesian_vz >>
188  PZS_datum.hasPosCov >> PZS_datum.pos_covariance >> PZS_datum.hasVelCov >>
189  PZS_datum.vel_covariance >> PZS_datum.hasStats >> PZS_datum.stats_GPS_sats_used >> PZS_datum.stats_GLONASS_sats_used;
190  if (version>=8)
191  in >> PZS_datum.stats_rtk_fix_progress;
192  else
193  PZS_datum.stats_rtk_fix_progress=0;
194  }
195  else {
196  PZS_datum.hasCartesianPosVel = PZS_datum.hasPosCov = PZS_datum.hasVelCov = PZS_datum.hasStats = false;
197  }
198  }
199  } // end version >=5
200 
201  // Added in V7:
202  if (version>=7) {
204  messages[gnss::TOPCON_SATS] = gnss::gnss_message_ptr(datum);
205  gnss::Message_TOPCON_SATS & SATS_datum = *datum;
206  bool has_SATS_datum_;
207  in >> has_SATS_datum_;
208  if (has_SATS_datum_)
209  {
210  in >> SATS_datum.USIs >> SATS_datum.ELs >> SATS_datum.AZs;
211  }
212  }
213  } break;
214  default:
216  };
217 
218  if (originalReceivedTimestamp==INVALID_TIMESTAMP)
219  originalReceivedTimestamp = timestamp;
220 }
221 
222 /*---------------------------------------------------------------
223  dumpToStream
224  ---------------------------------------------------------------*/
225 void CObservationGPS::dumpToStream( CStream &out ) const
226 {
227  out.printf("\n------------- [CObservationGPS] Dump of %u messages -----------------------\n",static_cast<unsigned int>(messages.size()));
228  for (message_list_t::const_iterator it=messages.begin();it!=messages.end();++it)
229  it->second->dumpToStream(out);
230  out.printf("-------------- [CObservationGPS] End of dump ------------------------------\n\n");
231 }
232 
233 void CObservationGPS::dumpToConsole(std::ostream &o) const
234 {
236  this->dumpToStream( memStr );
237  if (memStr.getTotalBytesCount()) {
238  o.write((const char*)memStr.getRawBufferData(),memStr.getTotalBytesCount());
239  }
240 }
241 
242 mrpt::system::TTimeStamp CObservationGPS::getOriginalReceivedTimeStamp() const {
243  return originalReceivedTimestamp;
244 }
245 
247 {
248  messages.clear();
249  timestamp = INVALID_TIMESTAMP;
250  originalReceivedTimestamp = INVALID_TIMESTAMP;
251 }
252 void CObservationGPS::getDescriptionAsText(std::ostream &o) const
253 {
254  CObservation::getDescriptionAsText(o);
255 
256  o << "Timestamp (UTC) of reception at the computer: " << mrpt::system::dateTimeToString(originalReceivedTimestamp) << std::endl;
257  o << " (as time_t): " << std::fixed << std::setprecision(5) << mrpt::system::timestampTotime_t(originalReceivedTimestamp) << std::endl;
258  o << " (as TTimestamp): " << originalReceivedTimestamp << std::endl;
259 
260  o << "Sensor position on the robot/vehicle: " << sensorPose << std::endl;
261 
262  this->dumpToConsole(o);
263 }
264 
265 bool CObservationGPS::hasMsgType(const gnss::gnss_message_type_t type_id) const
266 {
267  return messages.find(type_id)!=messages.end();
268 }
269 
270 mrpt::obs::gnss::gnss_message* CObservationGPS::getMsgByType(const gnss::gnss_message_type_t type_id)
271 {
272  message_list_t::iterator it = messages.find(type_id);
273  ASSERTMSG_(it!=messages.end(), mrpt::format("[CObservationGPS::getMsgByType] Cannot find any observation of type `%u`",static_cast<unsigned int>(type_id) ));
274  return it->second.get();
275 }
276 /** \overload */
277 const mrpt::obs::gnss::gnss_message* CObservationGPS::getMsgByType(const gnss::gnss_message_type_t type_id) const
278 {
279  message_list_t::const_iterator it = messages.find(type_id);
280  ASSERTMSG_(it!=messages.end(), mrpt::format("[CObservationGPS::getMsgByType] Cannot find any observation of type `%u`",static_cast<unsigned int>(type_id) ));
281  return it->second.get();
282 }
283 
284 
285 // From: http://gnsstk.sourceforge.net/time__conversion_8c-source.html
286 #define TIMECONV_JULIAN_DATE_START_OF_GPS_TIME (2444244.5) // [days]
287 bool TIMECONV_GetJulianDateFromGPSTime( const unsigned short gps_week, const double gps_tow, const unsigned int utc_offset, double * julian_date)
288 {
289  if( gps_tow < 0.0 || gps_tow > 604800.0 )
290  return false;
291  // GPS time is ahead of UTC time and Julian time by the UTC offset
292  *julian_date = (gps_week + (gps_tow-utc_offset)/604800.0)*7.0 + TIMECONV_JULIAN_DATE_START_OF_GPS_TIME;
293  return true;
294 }
295 
296 bool TIMECONV_IsALeapYear( const unsigned short year )
297 {
298  bool is_a_leap_year = false;
299  if( (year%4) == 0 )
300  {
301  is_a_leap_year = true;
302  if( (year%100) == 0 )
303  {
304  if( (year%400) == 0 )
305  is_a_leap_year = true;
306  else is_a_leap_year = false;
307  }
308  }
309  return is_a_leap_year;
310 }
311 
313  const unsigned short year, //!< Universal Time Coordinated [year]
314  const unsigned char month, //!< Universal Time Coordinated [1-12 months]
315  unsigned char* days_in_month //!< Days in the specified month [1-28|29|30|31 days]
316  )
317 {
318  unsigned char utmp = 0;
319  bool is_a_leapyear = TIMECONV_IsALeapYear( year );
320 
321  switch(month)
322  {
323  case 1: utmp = 31; break;
324  case 2: if( is_a_leapyear ){ utmp = 29; }else{ utmp = 28; }break;
325  case 3: utmp = 31; break;
326  case 4: utmp = 30; break;
327  case 5: utmp = 31; break;
328  case 6: utmp = 30; break;
329  case 7: utmp = 31; break;
330  case 8: utmp = 31; break;
331  case 9: utmp = 30; break;
332  case 10: utmp = 31; break;
333  case 11: utmp = 30; break;
334  case 12: utmp = 31; break;
335  default: return false; break;
336  }
337  *days_in_month = utmp;
338  return true;
339 }
340 bool TIMECONV_GetUTCTimeFromJulianDate(const double julian_date, //!< Number of days since noon Universal Time Jan 1, 4713 BCE (Julian calendar) [days]
342  )
343 {
344  int a, b, c, d, e; // temporary values
345 
346  unsigned short year;
347  unsigned char month;
348  unsigned char day;
349  unsigned char hour;
350  unsigned char minute;
351  unsigned char days_in_month = 0;
352  double td; // temporary double
353  double seconds;
354  bool result;
355 
356  // Check the input.
357  if( julian_date < 0.0 )
358  return false;
359 
360  a = (int)(julian_date+0.5);
361  b = a + 1537;
362  c = (int)( ((double)b-122.1)/365.25 );
363  d = (int)(365.25*c);
364  e = (int)( ((double)(b-d))/30.6001 );
365 
366  td = b - d - (int)(30.6001*e) + fmod( julian_date+0.5, 1.0 ); // [days]
367  day = (unsigned char)td;
368  td -= day;
369  td *= 24.0; // [hours]
370  hour = (unsigned char)td;
371  td -= hour;
372  td *= 60.0; // [minutes]
373  minute = (unsigned char)td;
374  td -= minute;
375  td *= 60.0; // [s]
376  seconds = td;
377  month = (unsigned char)(e - 1 - 12*(int)(e/14));
378  year = (unsigned short)(c - 4715 - (int)( (7.0+(double)month) / 10.0 ));
379  // check for rollover issues
380  if( seconds >= 60.0 )
381  {
382  seconds -= 60.0;
383  minute++;
384  if( minute >= 60 )
385  {
386  minute -= 60;
387  hour++;
388  if( hour >= 24 )
389  {
390  hour -= 24;
391  day++;
392  result = TIMECONV_GetNumberOfDaysInMonth( year, month, &days_in_month );
393  if( result == false )
394  return false;
395  if( day > days_in_month )
396  {
397  day = 1;
398  month++;
399  if( month > 12 )
400  {
401  month = 1;
402  year++;
403  }
404  }
405  }
406  }
407  }
408 
409  utc.year = year;
410  utc.month = month;
411  utc.day = day;
412  utc.hour = hour;
413  utc.minute = minute;
414  utc.second = seconds;
415  return true;
416 }
417 
418 bool CObservationGPS::GPS_time_to_UTC(uint16_t gps_week,double gps_sec,const int leap_seconds_count, mrpt::system::TTimeStamp &utc_out)
419 {
421  if (!GPS_time_to_UTC(gps_week,gps_sec,leap_seconds_count,tim)) return false;
423  return true;
424 }
425 
426 bool CObservationGPS::GPS_time_to_UTC(uint16_t gps_week,double gps_sec,const int leap_seconds_count, mrpt::system::TTimeParts &utc_out)
427 {
428  double julian_date = 0.0;
429  if( gps_sec < 0.0 || gps_sec > 604800.0 )
430  return false;
432  gps_week,
433  gps_sec,
434  leap_seconds_count,
435  &julian_date ) )
436  return false;
437  if (!TIMECONV_GetUTCTimeFromJulianDate(julian_date,utc_out))
438  return false;
439  return true;
440 }
441 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
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 cartesian_z
Only if hasCartesianPosVel is true.
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:-)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::vector_byte USIs
The list of USI (Universal Sat ID) for the detected sats (See GRIL Manual, pag 4-31).
unsigned __int16 uint16_t
Definition: rptypes.h:46
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.
mrpt::system::TTimeStamp originalReceivedTimestamp
The local computer-based timestamp based on the reception of the message in the computer.
double cartesian_vz
Only if hasCartesianPosVel is true.
gnss_message_type_t
List of all known GNSS message types.
message_list_t messages
The main piece of data in this class: a list of GNNS messages.
mrpt::system::TTimeStamp BASE_IMPEXP buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
Definition: datetime.cpp:127
Scalar * iterator
Definition: eigen_plugins.h:23
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)
const Scalar * const_iterator
Definition: eigen_plugins.h:24
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)
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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
double altitude_meters
The measured altitude, in meters (A).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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.
Definition: CArrayNumeric.h:19
double corrected_orthometric_altitude
The corrected (only for TopCon mmGPS) orthometric altitude, in meters mmGPS(A+B). ...
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)
This CStream derived class allow using a memory buffer as a CStream.
Definition: CMemoryStream.h:26
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:35
float HDOP
The HDOP (Horizontal Dilution of Precision) as returned by the sensor.
uint8_t day
Month (1-12)
Definition: datetime.h:39
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
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
GLubyte GLubyte b
Definition: glext.h:5575
bool has_satellite_timestamp
If true, CObservation::timestamp has been generated from accurate satellite clock. Otherwise, no GPS data is available and timestamps are based on the local computer clock.
int version
Definition: mrpt_jpeglib.h:898
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:17
mrpt::vector_signed_byte ELs
Elevation (in degrees, 0-90) for each satellite in USIs.
double second
Minute (0-59)
Definition: datetime.h:42
void * getRawBufferData()
Method for getting a pointer to the raw stored data.
mrpt::vector_signed_word AZs
Azimuth (in degrees, 0-360) for each satellite in USIs.
double geoidal_distance
Undulation: Difference between the measured altitude and the geoid, in meters (B).
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
bool TIMECONV_GetUTCTimeFromJulianDate(const double julian_date, mrpt::system::TTimeParts &utc)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
uint8_t minute
Hour (0-23)
Definition: datetime.h:41
mrpt::poses::CPose3D sensorPose
The sensor pose on the robot/vehicle.
Declares a class that represents any robot&#39;s observation.
Pure virtual base for all message types.
std::string BASE_IMPEXP 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:247
This file implements matrix/vector text and binary serialization.
double direction_degrees
Measured speed direction (in degrees)
GLuint in
Definition: glext.h:6301
content_t fields
Message content, accesible by individual fields.
uint8_t month
The year.
Definition: datetime.h:38
uint64_t getTotalBytesCount() MRPT_OVERRIDE
Returns the total size of the internal buffer.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
uint8_t hour
Day (1-31)
Definition: datetime.h:40
#define TIMECONV_JULIAN_DATE_START_OF_GPS_TIME
uint8_t TXBattery
battery level on transmitter
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
unsigned __int32 uint32_t
Definition: rptypes.h:49
double RTK_height_meters
ellipsoidal height [m] without N-beam correction
#define ASSERTMSG_(f, __ERROR_MSG)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
double BASE_IMPEXP 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:53
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019