MRPT  1.9.9
CGPSInterface_unittest.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 <gtest/gtest.h>
12 #include <mrpt/io/CMemoryStream.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::hwdrivers;
16 using namespace mrpt::obs;
17 using namespace std;
18 
19 // Example cmds:
20 // https://www.sparkfun.com/datasheets/GPS/NMEA%20Reference%20Manual-Rev2.1-Dec07.pdf
21 
22 TEST(CGPSInterface, parse_NMEA_GGA)
23 {
24  // Test with a correct frame:
25  {
26  const char* test_cmd =
27  "$GPGGA,101830.00,3649.76162994,N,00224.53709052,W,2,08,1.1,9.3,M,"
28  "47.4,M,5.0,0120*58";
30  const bool parse_ret = CGPSInterface::parse_NMEA(test_cmd, obsGPS);
31  EXPECT_TRUE(parse_ret) << "Failed parse of: " << test_cmd << endl;
32 
33  const gnss::Message_NMEA_GGA* msg =
35  EXPECT_TRUE(msg != nullptr);
36  if (!msg) return;
37  EXPECT_NEAR(
38  msg->fields.latitude_degrees, 36 + 49.76162994 / 60.0, 1e-10);
39  EXPECT_NEAR(
40  msg->fields.longitude_degrees, -(002 + 24.53709052 / 60.0), 1e-10);
41  EXPECT_NEAR(msg->fields.altitude_meters, 9.3, 1e-10);
42  }
43  // Test with an empty frame:
44  {
45  const char* test_cmd = "$GPGGA,,,,,,0,,,,M,,M,,*6";
47  const bool parse_ret = CGPSInterface::parse_NMEA(test_cmd, obsGPS);
48  EXPECT_FALSE(parse_ret);
49  }
50 }
51 
52 TEST(CGPSInterface, parse_NMEA_RMC)
53 {
54  const char* test_cmd =
55  "$GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598, ,*10";
57  const bool parse_ret = CGPSInterface::parse_NMEA(test_cmd, obsGPS);
58  EXPECT_TRUE(parse_ret) << "Failed parse of: " << test_cmd << endl;
59 
60  const gnss::Message_NMEA_RMC* msg =
62 
63  EXPECT_TRUE(msg != nullptr);
64  if (!msg) return;
65  EXPECT_NEAR(msg->fields.latitude_degrees, 37 + 23.2475 / 60.0, 1e-10);
66  EXPECT_NEAR(msg->fields.longitude_degrees, -(121 + 58.3416 / 60.0), 1e-10);
67 }
68 
69 TEST(CGPSInterface, parse_NMEA_GLL)
70 {
71  const char* test_cmd = "$GPGLL,3723.2475,N,12158.3416,W,161229.487,A,A*41";
73  const bool parse_ret = CGPSInterface::parse_NMEA(test_cmd, obsGPS);
74  EXPECT_TRUE(parse_ret) << "Failed parse of: " << test_cmd << endl;
75 
76  const gnss::Message_NMEA_GLL* msg =
78 
79  EXPECT_TRUE(msg != nullptr);
80  if (!msg) return;
81  EXPECT_NEAR(msg->fields.latitude_degrees, 37 + 23.2475 / 60.0, 1e-10);
82  EXPECT_NEAR(msg->fields.longitude_degrees, -(121 + 58.3416 / 60.0), 1e-10);
83 }
84 
85 TEST(CGPSInterface, parse_NMEA_VTG)
86 {
87  const char* test_cmd = "$GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48";
89  const bool parse_ret = CGPSInterface::parse_NMEA(test_cmd, obsGPS);
90  EXPECT_TRUE(parse_ret) << "Failed parse of: " << test_cmd << endl;
91 
92  const gnss::Message_NMEA_VTG* msg =
94 
95  EXPECT_TRUE(msg != nullptr);
96  if (!msg) return;
97  EXPECT_NEAR(msg->fields.true_track, 54.7, 1e-6);
98  EXPECT_NEAR(msg->fields.magnetic_track, 34.4, 1e-6);
99  EXPECT_NEAR(msg->fields.ground_speed_knots, 5.5, 1e-6);
100  EXPECT_NEAR(msg->fields.ground_speed_kmh, 10.2, 1e-6);
101 }
102 
103 TEST(CGPSInterface, parse_NMEA_ZDA)
104 {
105  const char* test_cmd = "$GPZDA,181813,14,10,2003,00,00*4F";
107  const bool parse_ret = CGPSInterface::parse_NMEA(test_cmd, obsGPS);
108  EXPECT_TRUE(parse_ret) << "Failed parse of: " << test_cmd << endl;
109 
110  const gnss::Message_NMEA_ZDA* msg =
112 
113  EXPECT_TRUE(msg != nullptr);
114  if (!msg) return;
115  EXPECT_TRUE(msg->fields.date_day == 14);
116  EXPECT_TRUE(msg->fields.date_month == 10);
117  EXPECT_TRUE(msg->fields.date_year == 2003);
118  EXPECT_TRUE(msg->fields.UTCTime.hour == 18);
119  EXPECT_TRUE(msg->fields.UTCTime.minute == 18);
120  EXPECT_TRUE(msg->fields.UTCTime.sec == 13.0);
121  // Replaced from EXPECT_EQ() to avoid a "bus error" in a gtest template
122  // under armhf.
123 }
124 
125 TEST(CGPSInterface, parse_NMEA_ZDA_stream)
126 {
128  {
129  const std::string s("$GPZDA,181813,14,10,2003,00,00*4F\n");
130  buf.Write(s.c_str(), s.size());
131  buf.Seek(0);
132  }
133 
134  CGPSInterface gps;
135  gps.bindStream(&buf);
136 
137  gps.initialize();
138  gps.doProcess();
139 
141  gps.getObservations(obss);
142 
143  EXPECT_EQ(obss.size(), 1U);
144 
145  auto obsGPS = mrpt::ptr_cast<CObservationGPS>::from(obss.begin()->second);
146 
147  const gnss::Message_NMEA_ZDA* msg =
148  obsGPS->getMsgByClassPtr<gnss::Message_NMEA_ZDA>();
149 
150  EXPECT_TRUE(msg != nullptr);
151  if (!msg) return;
152  EXPECT_TRUE(msg->fields.date_day == 14);
153  EXPECT_TRUE(msg->fields.date_month == 10);
154  EXPECT_TRUE(msg->fields.date_year == 2003);
155  EXPECT_TRUE(msg->fields.UTCTime.hour == 18);
156  EXPECT_TRUE(msg->fields.UTCTime.minute == 18);
157  EXPECT_TRUE(msg->fields.UTCTime.sec == 13.0);
158  // Replaced from EXPECT_EQ() to avoid a "bus error" in a gtest template
159  // under armhf.
160 }
161 
162 TEST(CGPSInterface, parse_NOVATEL6_stream)
163 {
165  {
166  const unsigned char sample_novatel6_gps[] = {
167  0xaa, 0x44, 0x12, 0x1c, 0x2a, 0x00, 0x00, 0xa0, 0x48, 0x00, 0x00,
168  0x00, 0x5a, 0xb4, 0x59, 0x07, 0x10, 0x4a, 0xb7, 0x16, 0x00, 0x00,
169  0x00, 0x00, 0xf6, 0xb1, 0x4a, 0x34, 0x00, 0x00, 0x00, 0x00, 0x38,
170  0x00, 0x00, 0x00, 0x97, 0x2b, 0x45, 0xa9, 0xc8, 0x6a, 0x42, 0x40,
171  0xfc, 0x54, 0x43, 0x6f, 0x11, 0x18, 0x03, 0xc0, 0x00, 0x00, 0x20,
172  0x8f, 0xe8, 0x0e, 0x1c, 0x40, 0x66, 0x66, 0x48, 0x42, 0x3d, 0x00,
173  0x00, 0x00, 0x1d, 0x9b, 0x96, 0x3c, 0x2c, 0xd5, 0x9c, 0x3c, 0xd1,
174  0x39, 0xa8, 0x3c, 0x35, 0x35, 0x35, 0x00, 0x00, 0x00, 0x60, 0x41,
175  0x00, 0x00, 0x00, 0x00, 0x0f, 0x0e, 0x0e, 0x0d, 0x00, 0x00, 0x00,
176  0x33, 0x82, 0xba, 0x79, 0xe5, 0xaa, 0x44, 0x13, 0x58, 0xfc, 0x01,
177  0x59, 0x07, 0x10, 0x4a, 0xb7, 0x16, 0x59, 0x07, 0x00, 0x00, 0x33,
178  0x33, 0x33, 0x33, 0xdb, 0x42, 0x17, 0x41, 0xa7, 0xf0, 0xaf, 0xa5,
179  0xc8, 0x6a, 0x42, 0x40, 0xa2, 0xad, 0xac, 0x28, 0x12, 0x18, 0x03,
180  0xc0, 0x00, 0x00, 0x8a, 0x8b, 0x52, 0x8d, 0x4c, 0x40, 0x10, 0xe2,
181  0xdb, 0x3c, 0x4b, 0xbd, 0x82, 0xbf, 0x52, 0x23, 0x1e, 0x50, 0x08,
182  0xf1, 0x9b, 0xbf, 0xd4, 0xa6, 0xd1, 0x7c, 0xcd, 0x16, 0xc8, 0x3f,
183  0x31, 0x27, 0xe1, 0x16, 0xa2, 0x6b, 0x10, 0x40, 0xc7, 0x1c, 0xc7,
184  0x39, 0x6a, 0x9c, 0x00, 0x40, 0xa0, 0x3c, 0x9f, 0x79, 0xca, 0xdd,
185  0x63, 0x40, 0x03, 0x00, 0x00, 0x00, 0x27, 0xbb, 0xff, 0xf8, 0xaa,
186  0x44, 0x12, 0x1c, 0x2a, 0x00, 0x00, 0xa0, 0x48, 0x00, 0x00, 0x00,
187  0x5a, 0xb4, 0x59, 0x07, 0x42, 0x4a, 0xb7, 0x16, 0x00, 0x00, 0x00,
188  0x00, 0xf6, 0xb1, 0x4a, 0x34, 0x00, 0x00, 0x00, 0x00, 0x38, 0x00,
189  0x00, 0x00, 0xf0, 0x23, 0x3c, 0xa9, 0xc8, 0x6a, 0x42, 0x40, 0xdd,
190  0x10, 0x6c, 0x71, 0x11, 0x18, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x03,
191  0xa7, 0x18, 0x1c, 0x40, 0x66, 0x66, 0x48, 0x42, 0x3d, 0x00, 0x00,
192  0x00, 0x32, 0x9b, 0x96, 0x3c, 0x82, 0xd4, 0x9c, 0x3c, 0x5d, 0x3a,
193  0xa8, 0x3c, 0x35, 0x35, 0x35, 0x00, 0x00, 0x00, 0x60, 0x41, 0x00,
194  0x00, 0x00, 0x00, 0x0f, 0x0e, 0x0e, 0x0d, 0x00, 0x00, 0x00, 0x33,
195  0xcb, 0x95, 0xa0, 0x9b, 0xaa, 0x44, 0x13, 0x58, 0xfc, 0x01, 0x59,
196  0x07, 0x42, 0x4a, 0xb7, 0x16, 0x59, 0x07, 0x00, 0x00, 0x67, 0x66,
197  0x66, 0x66, 0xdb, 0x42, 0x17, 0x41, 0xe6, 0xae, 0xa1, 0xa5, 0xc8,
198  0x6a, 0x42, 0x40, 0x26, 0x1e, 0x82, 0x2b, 0x12, 0x18, 0x03, 0xc0,
199  0x00, 0x00, 0x62, 0xb6, 0x8b, 0x8e, 0x4c, 0x40, 0x10, 0x63, 0x42,
200  0x19, 0x38, 0x19, 0x7a, 0xbf, 0x1e, 0xa9, 0x79, 0x02, 0x24, 0x6c,
201  0x9d, 0xbf, 0x52, 0x13, 0x38, 0xa4, 0x35, 0x2c, 0xc8, 0x3f, 0xa9,
202  0x3b, 0x21, 0x59, 0xe0, 0xa0, 0x10, 0x40, 0x51, 0xd1, 0x8c, 0x50,
203  0x0b, 0xa0, 0x00, 0x40, 0x16, 0x40, 0x94, 0xbe, 0xc2, 0xdd, 0x63,
204  0x40, 0x03, 0x00, 0x00, 0x00, 0x20, 0x4d, 0xe7, 0xa2, 0xaa, 0x44,
205  0x12, 0x1c, 0x2a, 0x00, 0x00, 0xa0, 0x48, 0x00, 0x00, 0x00, 0x5a,
206  0xb4, 0x59, 0x07, 0x74, 0x4a, 0xb7, 0x16, 0x00, 0x00, 0x00, 0x00,
207  0xf6, 0xb1, 0x4a, 0x34, 0x00, 0x00, 0x00, 0x00, 0x38, 0x00, 0x00,
208  0x00, 0xaa, 0x41, 0x32, 0xa9, 0xc8, 0x6a, 0x42, 0x40, 0xff, 0x59,
209  0xa8, 0x73, 0x11, 0x18, 0x03, 0xc0, 0x00, 0x00, 0xa0, 0xd6, 0x6b,
210  0x22, 0x1c, 0x40, 0x66, 0x66, 0x48, 0x42, 0x3d, 0x00, 0x00, 0x00,
211  0x92, 0x9b, 0x96, 0x3c, 0x70, 0xd3, 0x9c, 0x3c, 0x06, 0x3b, 0xa8,
212  0x3c, 0x35, 0x35, 0x35, 0x00};
213  const unsigned int sample_novatel6_gps_len = 500;
214  buf.Write(sample_novatel6_gps, sample_novatel6_gps_len);
215  buf.Seek(0);
216  }
217 
218  CGPSInterface gps;
219  gps.bindStream(&buf);
220 
221  gps.initialize();
222  gps.doProcess();
223 
225  gps.getObservations(obss);
226 
227  EXPECT_EQ(obss.size(), 4U);
228 
229  auto itObs = obss.begin();
230  auto obsGPS1 = mrpt::ptr_cast<CObservationGPS>::from(itObs->second);
231  ++itObs;
232  auto obsGPS2 = mrpt::ptr_cast<CObservationGPS>::from(itObs->second);
233 
234  EXPECT_TRUE(obsGPS1);
235  EXPECT_TRUE(obsGPS2);
236 
237  const auto* msg1 =
238  obsGPS1->getMsgByClassPtr<gnss::Message_NV_OEM6_BESTPOS>();
239  EXPECT_TRUE(msg1 != nullptr);
240  if (!msg1) return;
241  EXPECT_TRUE(msg1->fields.num_sats_tracked == 15);
242 
243  const auto* msg2 =
244  obsGPS2->getMsgByClassPtr<gnss::Message_NV_OEM6_INSPVAS>();
245  EXPECT_TRUE(msg2 != nullptr);
246  if (!msg2) return;
247  EXPECT_NEAR(msg2->fields.roll, 4.10511, 1e-4);
248 }
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
content_t fields
Message content, accesible by individual fields.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
A class capable of reading GPS/GNSS/GNSS+IMU receiver data, from a serial port or from any input stre...
MSG_CLASS * getMsgByClassPtr()
Like CObservationGPS::getMsgByClass() but returns a nullptr pointer if message is not found...
void bindStream(mrpt::io::CStream *external_stream, std::mutex *csOptionalExternalStream=nullptr)
This enforces the use of a given user stream, instead of trying to open the serial port set in this c...
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
TEST(CGPSInterface, parse_NMEA_GGA)
content_t fields
Message content, accesible by individual fields.
Contains classes for various device interfaces.
void getObservations(TListObservations &lstObjects)
Returns a list of enqueued objects, emptying it (thread-safe).
content_t fields
Message content, accesible by individual fields.
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
STL namespace.
GLdouble s
Definition: glext.h:3682
double altitude_meters
The measured altitude, in meters (A).
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
This CStream derived class allow using a memory buffer as a CStream.
content_t fields
Message content, accesible by individual fields.
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) override
Introduces a pure virtual method for moving to a specified position in the streamed resource...
This namespace contains representation of robot actions and observations.
GLsizei const GLchar ** string
Definition: glext.h:4116
static bool parse_NMEA(const std::string &cmd_line, mrpt::obs::CObservationGPS &out_obs, const bool verbose=false)
Parses one line of NMEA data from a GPS receiver, and writes the recognized fields (if any) into an o...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
content_t fields
Message content, accesible by individual fields.
virtual void initialize()
This method can or cannot be implemented in the derived class, depending on the need for it...
static CAST_TO::Ptr from(const CAST_FROM_PTR &ptr)
Definition: CObject.h:366
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019