parser_tests.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <novatel_gps_driver/parsers/bestpos.h>
00031 #include <novatel_gps_driver/parsers/gpgsv.h>
00032 #include <novatel_gps_driver/novatel_message_extractor.h>
00033 #include <novatel_gps_driver/parsers/bestxyz.h>
00034 #include <novatel_gps_driver/parsers/heading2.h>
00035 #include <novatel_gps_driver/parsers/dual_antenna_heading.h>
00036 
00037 #include <gtest/gtest.h>
00038 #include <novatel_gps_driver/parsers/inspva.h>
00039 #include <novatel_gps_driver/parsers/insstdev.h>
00040 #include <novatel_gps_driver/parsers/corrimudata.h>
00041 #include <novatel_gps_driver/parsers/inscov.h>
00042 
00043 TEST(ParserTestSuite, testBestposAsciiParsing)
00044 {
00045   novatel_gps_driver::BestposParser parser;
00046   std::string bestpos_str = "#BESTPOSA,ICOM1,0,87.5,FINESTEERING,1956,157432.000,00000800,7145,6938;"
00047   "SOL_COMPUTED,SINGLE,29.44391220792,-98.61476921244,261.4344,-26.0000,WGS84,2.1382,"
00048   "3.1092,4.0429,\"\",0.000,0.000,8,8,8,8,0,06,00,03*ecf2202b\r\n"
00049   "#BESTPOSA,COM1,0,83.5,FINESTEERING,1419,336148.000,02000040,6145,2724;SOL_COMPUTED,SINGLE,"
00050   "51.11636418888,-114.03832502118,1064.9520,-16.2712,WGS84,1.6961,1.3636,3.6449,\"\","
00051   "0.000,0.000,8,8,8,8,0,0,0,06,0,03*f181ad10\r\n"
00052   "#BESTPOSA,COM1,0,78.5,FINESTEERING,1419,336208.000,02000040,6145,2724;SOL_COMPUTED,"
00053   "NARROW_INT,51.11635910984,-114.03833105168,1063.8416,-16.2712,WGS84,0.0135,0.0084,"
00054   "0.0172,\"AAAA\",1.000,0.000,8,8,8,8,0,01,0,03*072421c0\r\n";
00055   std::string extracted_str;
00056 
00057   novatel_gps_driver::NovatelMessageExtractor extractor;
00058 
00059   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00060   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00061   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00062   std::string remaining;
00063 
00064   extractor.ExtractCompleteMessages(bestpos_str, nmea_sentences, novatel_sentences,
00065                                     binary_messages, remaining);
00066 
00067   ASSERT_EQ(0, nmea_sentences.size());
00068   ASSERT_EQ(0, binary_messages.size());
00069   ASSERT_EQ(3, novatel_sentences.size());
00070 
00071   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00072 
00073   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00074 
00075   novatel_gps_msgs::NovatelPositionPtr msg = parser.ParseAscii(sentence);
00076 
00077   ASSERT_NE(msg.get(), nullptr);
00078 
00079   ASSERT_EQ("SOL_COMPUTED", msg->solution_status);
00080   ASSERT_DOUBLE_EQ(29.44391220792, msg->lat);
00081   ASSERT_DOUBLE_EQ(-98.61476921244, msg->lon);
00082 }
00083 
00084 TEST(ParserTestSuite, testCorrimudataAsciiParsing)
00085 {
00086   novatel_gps_driver::CorrImuDataParser parser;
00087   std::string sentence_str = "#CORRIMUDATAA,COM1,0,77.5,FINESTEERING,1769,237601.000,02000020,"
00088       "bdba,12597;1769,237601.000000000,-0.000003356,0.000002872,0.000001398,0.000151593,"
00089       "0.000038348,-0.000078820*e370e1d9\r\n";
00090   std::string extracted_str;
00091 
00092   novatel_gps_driver::NovatelMessageExtractor extractor;
00093 
00094   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00095   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00096   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00097   std::string remaining;
00098 
00099   extractor.ExtractCompleteMessages(sentence_str, nmea_sentences, novatel_sentences,
00100                                     binary_messages, remaining);
00101 
00102   ASSERT_EQ(0, nmea_sentences.size());
00103   ASSERT_EQ(0, binary_messages.size());
00104   ASSERT_EQ(1, novatel_sentences.size());
00105 
00106   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00107 
00108   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00109 
00110   novatel_gps_msgs::NovatelCorrectedImuDataPtr msg = parser.ParseAscii(sentence);
00111 
00112   ASSERT_NE(msg.get(), nullptr);
00113   ASSERT_EQ(1769, msg->gps_week_num);
00114   ASSERT_DOUBLE_EQ(237601.0, msg->gps_seconds);
00115   ASSERT_DOUBLE_EQ(-0.000003356, msg->pitch_rate);
00116   ASSERT_DOUBLE_EQ(0.000002872, msg->roll_rate);
00117   ASSERT_DOUBLE_EQ(0.000001398, msg->yaw_rate);
00118   ASSERT_DOUBLE_EQ(0.000151593, msg->lateral_acceleration);
00119   ASSERT_DOUBLE_EQ(0.000038348, msg->longitudinal_acceleration);
00120   ASSERT_DOUBLE_EQ(-0.00007882, msg->vertical_acceleration);
00121 }
00122 
00123 TEST(ParserTestSuite, testGpgsvParsing)
00124 {
00125   novatel_gps_driver::GpgsvParser parser;
00126   std::string sentence_str = "$GPGSV,3,3,11,12,07,00.,32,13,03,227,36,22,0.,041,*4A\r\n";
00127   std::string extracted_str;
00128 
00129   novatel_gps_driver::NovatelMessageExtractor extractor;
00130 
00131   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00132   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00133   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00134   std::string remaining;
00135 
00136   extractor.ExtractCompleteMessages(sentence_str, nmea_sentences, novatel_sentences,
00137                                     binary_messages, remaining);
00138 
00139   ASSERT_EQ(1, nmea_sentences.size());
00140   ASSERT_EQ(0, binary_messages.size());
00141   ASSERT_EQ(0, novatel_sentences.size());
00142 
00143   novatel_gps_driver::NmeaSentence sentence = nmea_sentences.front();
00144 
00145   ASSERT_EQ(parser.GetMessageName(), sentence.id);
00146   ASSERT_FALSE(sentence.body.empty());
00147 
00148   novatel_gps_msgs::GpgsvPtr msg = parser.ParseAscii(sentence);
00149 
00150   ASSERT_NE(msg.get(), nullptr);
00151 
00152   ASSERT_EQ(3, msg->n_msgs);
00153   ASSERT_EQ(3, msg->msg_number);
00154   ASSERT_EQ(3, msg->satellites.size());
00155   ASSERT_EQ(11, msg->n_satellites);
00156   ASSERT_EQ(12, msg->satellites[0].prn);
00157   ASSERT_EQ(7, msg->satellites[0].elevation);
00158   ASSERT_EQ(0, msg->satellites[0].azimuth);
00159   ASSERT_EQ(32, msg->satellites[0].snr);
00160   ASSERT_EQ(13, msg->satellites[1].prn);
00161   ASSERT_EQ(3, msg->satellites[1].elevation);
00162   ASSERT_EQ(227, msg->satellites[1].azimuth);
00163   ASSERT_EQ(36, msg->satellites[1].snr);
00164   ASSERT_EQ(22, msg->satellites[2].prn);
00165   ASSERT_EQ(0, msg->satellites[2].elevation);
00166   ASSERT_EQ(41, msg->satellites[2].azimuth);
00167   ASSERT_EQ(-1, msg->satellites[2].snr);
00168 }
00169 
00170 TEST(ParserTestSuite, testInscovAsciiParsing)
00171 {
00172   novatel_gps_driver::InscovParser parser;
00173   std::string sentence_str = "#INSCOVA,COM1,0,66.5,FINESTEERING,1959,336623.000,00000000,"
00174       "f078,13754;1959,336623.000000000,0.0211295047125775,-0.0018214277429204,-0.0016153828661881,"
00175       "-0.0018214277429204,0.0174981375607521,0.0049878113409426,-0.0016153828661881,"
00176       "0.0049878113409426,0.0285474196118174,0.0000332609098342,-0.0000003409117564,"
00177       "0.0000018468158360,-0.0000003409117564,0.0000340563145112,-0.0000136598582783,"
00178       "0.0000018468158360,-0.0000136598582783,0.1515644215075894,0.0000008850783906,"
00179       "0.0000000006144488,-0.0000001633832672,0.0000000006144488,0.0000010749675168,"
00180       "0.0000000004985751,-0.0000001633832672,0.0000000004985751,0.0000009343218169*bc5352ab\r\n";
00181   std::string extracted_str;
00182 
00183   novatel_gps_driver::NovatelMessageExtractor extractor;
00184 
00185   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00186   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00187   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00188   std::string remaining;
00189 
00190   extractor.ExtractCompleteMessages(sentence_str, nmea_sentences, novatel_sentences,
00191                                     binary_messages, remaining);
00192 
00193   ASSERT_EQ(0, nmea_sentences.size());
00194   ASSERT_EQ(0, binary_messages.size());
00195   ASSERT_EQ(1, novatel_sentences.size());
00196 
00197   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00198 
00199   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00200 
00201   novatel_gps_msgs::InscovPtr msg = parser.ParseAscii(sentence);
00202 
00203   ASSERT_NE(msg.get(), nullptr);
00204 
00205   ASSERT_EQ(1959, msg->week);
00206   ASSERT_DOUBLE_EQ(336623.0, msg->seconds);
00207   ASSERT_DOUBLE_EQ(0.0211295047125775, msg->position_covariance[0]);
00208   ASSERT_DOUBLE_EQ(0.0000009343218169, msg->velocity_covariance[8]);
00209 }
00210 
00211 TEST(ParserTestSuite, testInspvaAsciiParsing)
00212 {
00213   novatel_gps_driver::InspvaParser parser;
00214   std::string sentence_str = "#INSPVAA,COM1,0,31.0,FINESTEERING,1264,144088.000,02040000,"
00215   "5615,1541;1264,144088.002284950,51.116827527,-114.037738908,401.191547167,354.846489850,"
00216   "108.429407241,-10.837482850,1.116219952,-3.476059035,7.372686190,INS_ALIGNMENT_COMPLETE*a2913d36\r\n";
00217   std::string extracted_str;
00218 
00219   novatel_gps_driver::NovatelMessageExtractor extractor;
00220 
00221   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00222   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00223   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00224   std::string remaining;
00225 
00226   extractor.ExtractCompleteMessages(sentence_str, nmea_sentences, novatel_sentences,
00227                                     binary_messages, remaining);
00228 
00229   ASSERT_EQ(0, nmea_sentences.size());
00230   ASSERT_EQ(0, binary_messages.size());
00231   ASSERT_EQ(1, novatel_sentences.size());
00232 
00233   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00234 
00235   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00236 
00237   novatel_gps_msgs::InspvaPtr msg = parser.ParseAscii(sentence);
00238 
00239   ASSERT_NE(msg.get(), nullptr);
00240 
00241   ASSERT_EQ(1264, msg->week);
00242   ASSERT_DOUBLE_EQ(144088.002284950, msg->seconds);
00243   ASSERT_DOUBLE_EQ(51.116827527, msg->latitude);
00244   ASSERT_DOUBLE_EQ(-114.037738908, msg->longitude);
00245   ASSERT_DOUBLE_EQ(401.191547167, msg->height);
00246   ASSERT_DOUBLE_EQ(354.846489850, msg->north_velocity);
00247   ASSERT_DOUBLE_EQ(108.429407241, msg->east_velocity);
00248   ASSERT_DOUBLE_EQ(-10.837482850, msg->up_velocity);
00249   ASSERT_DOUBLE_EQ(1.116219952, msg->roll);
00250   ASSERT_DOUBLE_EQ(-3.476059035, msg->pitch);
00251   ASSERT_DOUBLE_EQ(7.372686190, msg->azimuth);
00252   ASSERT_EQ("INS_ALIGNMENT_COMPLETE", msg->status);
00253 }
00254 
00255 TEST(ParserTestSuite, testInsstdevAsciiParsing)
00256 {
00257   novatel_gps_driver::InsstdevParser parser;
00258   std::string sentence_str = "#INSSTDEVA,COM1,0,78.0,FINESTEERING,1907,233990.000,02000020,"
00259       "3e6d,32768;0.4372,0.3139,0.7547,0.0015,0.0015,0.0014,3.7503,3.7534,5.1857,26000005,"
00260       "0,0,01ffd1bf,0*3deca7d2\r\n";
00261   std::string extracted_str;
00262 
00263   novatel_gps_driver::NovatelMessageExtractor extractor;
00264 
00265   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00266   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00267   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00268   std::string remaining;
00269 
00270   extractor.ExtractCompleteMessages(sentence_str, nmea_sentences, novatel_sentences,
00271                                     binary_messages, remaining);
00272 
00273   ASSERT_EQ(0, nmea_sentences.size());
00274   ASSERT_EQ(0, binary_messages.size());
00275   ASSERT_EQ(1, novatel_sentences.size());
00276 
00277   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00278 
00279   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00280 
00281   novatel_gps_msgs::InsstdevPtr msg = parser.ParseAscii(sentence);
00282 
00283   ASSERT_NE(msg.get(), nullptr);
00284 
00285   ASSERT_FLOAT_EQ(0.4372, msg->latitude_dev);
00286   ASSERT_FLOAT_EQ(0.3139, msg->longitude_dev);
00287   ASSERT_FLOAT_EQ(0.7547, msg->height_dev);
00288   ASSERT_FLOAT_EQ(0.0015, msg->north_velocity_dev);
00289   ASSERT_FLOAT_EQ(0.0015, msg->east_velocity_dev);
00290   ASSERT_FLOAT_EQ(0.0014, msg->up_velocity_dev);
00291   ASSERT_FLOAT_EQ(3.7503, msg->roll_dev);
00292   ASSERT_FLOAT_EQ(3.7534, msg->pitch_dev);
00293   ASSERT_FLOAT_EQ(5.1857, msg->azimuth_dev);
00294   ASSERT_EQ(26000005, msg->extended_solution_status.original_mask);
00295 }
00296 
00297 TEST(ParserTestSuite, testBestxyzAsciiParsing)
00298 {
00299   novatel_gps_driver::BestxyzParser parser;
00300   std::string bestxyz_str = "#BESTXYZA,COM1,0,55.0,FINESTEERING,1419,340033.000,02000040,d821,2724;"
00301   "SOL_COMPUTED,NARROW_INT,-1634531.5683,-3664618.0326,4942496.3270,0.0099,"
00302   "0.0219,0.0115,SOL_COMPUTED,NARROW_INT,0.0011,-0.0049,-0.0001,0.0199,0.0439,"
00303   "0.0230,\"AAAA\",0.250,1.000,0.000,12,11,11,11,0,01,0,33*1fe2f509\r\n";
00304 
00305   std::string extracted_str;
00306 
00307   novatel_gps_driver::NovatelMessageExtractor extractor;
00308 
00309   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00310   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00311   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00312   std::string remaining;
00313 
00314   extractor.ExtractCompleteMessages(bestxyz_str, nmea_sentences, novatel_sentences,
00315                                     binary_messages, remaining);
00316 
00317   ASSERT_EQ(0, nmea_sentences.size());
00318   ASSERT_EQ(0, binary_messages.size());
00319   ASSERT_EQ(1, novatel_sentences.size());
00320 
00321   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00322 
00323   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00324 
00325   novatel_gps_msgs::NovatelXYZPtr msg = parser.ParseAscii(sentence);
00326 
00327   ASSERT_NE(msg.get(), nullptr);
00328 
00329   ASSERT_EQ("SOL_COMPUTED", msg->solution_status);
00330   ASSERT_EQ("NARROW_INT", msg->position_type);
00331   ASSERT_DOUBLE_EQ(-1634531.5683, msg->x);
00332   ASSERT_DOUBLE_EQ(-3664618.0326, msg->y);
00333   ASSERT_DOUBLE_EQ(4942496.3270, msg->z);
00334   ASSERT_FLOAT_EQ(0.0099, msg->x_sigma);
00335   ASSERT_FLOAT_EQ(0.0219, msg->y_sigma);
00336   ASSERT_FLOAT_EQ(0.0115, msg->z_sigma);
00337   ASSERT_EQ("SOL_COMPUTED", msg->velocity_solution_status);
00338   ASSERT_EQ("NARROW_INT", msg->velocity_type);
00339   ASSERT_DOUBLE_EQ(0.0011, msg->x_vel);
00340   ASSERT_DOUBLE_EQ(-0.0049, msg->y_vel);
00341   ASSERT_DOUBLE_EQ(-0.0001, msg->z_vel);
00342   ASSERT_FLOAT_EQ(0.0199, msg->x_vel_sigma);
00343   ASSERT_FLOAT_EQ(0.0439, msg->y_vel_sigma);
00344   ASSERT_FLOAT_EQ(0.0230, msg->z_vel_sigma);
00345   ASSERT_EQ("\"AAAA\"", msg->base_station_id);
00346   ASSERT_FLOAT_EQ(0.250, msg->velocity_latency);
00347   ASSERT_FLOAT_EQ(1.000, msg->diff_age);
00348   ASSERT_FLOAT_EQ(0.000, msg->solution_age);
00349   ASSERT_EQ(12, msg->num_satellites_tracked);
00350   ASSERT_EQ(11, msg->num_satellites_used_in_solution);
00351   ASSERT_EQ(11, msg->num_gps_and_glonass_l1_used_in_solution);
00352   ASSERT_EQ(11, msg->num_gps_and_glonass_l1_and_l2_used_in_solution);
00353   ASSERT_EQ(1, msg->extended_solution_status.original_mask);
00354 }
00355 
00356 TEST(ParserTestSuite, testHeading2AsciiParsing)
00357 {
00358   novatel_gps_driver::Heading2Parser parser;
00359   std::string heading2_str = "#HEADING2A,COM1,0,39.5,FINESTEERING,1622,422892.200,02040000,f9bf,6521;"
00360   "SOL_COMPUTED,NARROW_INT,0.927607417,178.347869873,-1.3037414550,0,0.261901051,0.391376048,"
00361   "\"R222\",\"AAAA\",18,17,17,16,0,01,0,33*8c48d77c\r\n";
00362 
00363   std::string extracted_str;
00364 
00365   novatel_gps_driver::NovatelMessageExtractor extractor;
00366 
00367   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00368   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00369   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00370   std::string remaining;
00371 
00372   extractor.ExtractCompleteMessages(heading2_str, nmea_sentences, novatel_sentences,
00373                                     binary_messages, remaining);
00374 
00375   ASSERT_EQ(0, nmea_sentences.size());
00376   ASSERT_EQ(0, binary_messages.size());
00377   ASSERT_EQ(1, novatel_sentences.size());
00378 
00379   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00380 
00381   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00382 
00383   novatel_gps_msgs::NovatelHeading2Ptr msg = parser.ParseAscii(sentence);
00384 
00385   ASSERT_NE(msg.get(), nullptr);
00386 
00387   ASSERT_EQ("SOL_COMPUTED", msg->solution_status);
00388   ASSERT_EQ("NARROW_INT", msg->position_type);
00389   ASSERT_FLOAT_EQ(0.927607417, msg->baseline_length);
00390   ASSERT_FLOAT_EQ(178.347869873, msg->heading);
00391   ASSERT_FLOAT_EQ(-1.3037414550, msg->pitch);
00392   ASSERT_FLOAT_EQ(0.261901051, msg->heading_sigma);
00393   ASSERT_FLOAT_EQ(0.391376048, msg->pitch_sigma);
00394   ASSERT_EQ("\"R222\"", msg->rover_station_id);
00395   ASSERT_EQ("\"AAAA\"", msg->master_station_id);
00396   ASSERT_EQ(18, msg->num_satellites_tracked);
00397   ASSERT_EQ(17, msg->num_satellites_used_in_solution);
00398   ASSERT_EQ(17, msg->num_satellites_above_elevation_mask_angle);
00399   ASSERT_EQ(16, msg->num_satellites_above_elevation_mask_angle_l2);
00400   ASSERT_EQ(0, msg->solution_source);
00401   ASSERT_EQ(1, msg->extended_solution_status.original_mask);
00402 }
00403 
00404 TEST(ParserTestSuite, testDualAntennaHeadingAsciiParsing)
00405 {
00406   novatel_gps_driver::DualAntennaHeadingParser parser;
00407   std::string heading_str = "#DUALANTENNAHEADINGA,UNKNOWN,0,66.5,FINESTEERING,1949,575614.000,02000000,d426,32768;"
00408   "SOL_COMPUTED,NARROW_INT,-1.000000000,255.538528442,0.006041416,0.0,0.043859947,0.052394450,"
00409   "\"J56X\",24,18,18,17,04,01,00,33*1f082ec5\r\n";
00410 
00411   std::string extracted_str;
00412 
00413   novatel_gps_driver::NovatelMessageExtractor extractor;
00414 
00415   std::vector<novatel_gps_driver::NmeaSentence> nmea_sentences;
00416   std::vector<novatel_gps_driver::NovatelSentence> novatel_sentences;
00417   std::vector<novatel_gps_driver::BinaryMessage> binary_messages;
00418   std::string remaining;
00419 
00420   extractor.ExtractCompleteMessages(heading_str, nmea_sentences, novatel_sentences,
00421                                     binary_messages, remaining);
00422 
00423   ASSERT_EQ(0, nmea_sentences.size());
00424   ASSERT_EQ(0, binary_messages.size());
00425   ASSERT_EQ(1, novatel_sentences.size());
00426 
00427   novatel_gps_driver::NovatelSentence sentence = novatel_sentences.front();
00428 
00429   ASSERT_EQ(parser.GetMessageName() + "A", sentence.id);
00430 
00431   novatel_gps_msgs::NovatelDualAntennaHeadingPtr msg = parser.ParseAscii(sentence);
00432 
00433   ASSERT_NE(msg.get(), nullptr);
00434 
00435   ASSERT_EQ("SOL_COMPUTED", msg->solution_status);
00436   ASSERT_EQ("NARROW_INT", msg->position_type);
00437   ASSERT_FLOAT_EQ(-1.000000000, msg->baseline_length);
00438   ASSERT_FLOAT_EQ(255.538528442, msg->heading);
00439   ASSERT_FLOAT_EQ(0.006041416, msg->pitch);
00440   ASSERT_FLOAT_EQ(0.043859947, msg->heading_sigma);
00441   ASSERT_FLOAT_EQ(0.052394450, msg->pitch_sigma);
00442   ASSERT_EQ("\"J56X\"", msg->station_id);
00443   ASSERT_EQ(24, msg->num_satellites_tracked);
00444   ASSERT_EQ(18, msg->num_satellites_used_in_solution);
00445   ASSERT_EQ(18, msg->num_satellites_above_elevation_mask_angle);
00446   ASSERT_EQ(17, msg->num_satellites_above_elevation_mask_angle_l2);
00447   ASSERT_EQ(1, msg->solution_source);
00448   ASSERT_EQ(1, msg->extended_solution_status.original_mask);
00449 }
00450 
00451 int main(int argc, char **argv)
00452 {
00453   testing::InitGoogleTest(&argc, argv);
00454 
00455   return RUN_ALL_TESTS();
00456 }


novatel_gps_driver
Author(s):
autogenerated on Wed Jul 3 2019 19:40:37