novatel_message_extractor.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/novatel_message_extractor.h>
00031 
00032 #include <limits>
00033 #include <sstream>
00034 
00035 #include <boost/algorithm/string/split.hpp>
00036 #include <boost/algorithm/string/classification.hpp>
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <swri_string_util/string_util.h>
00041 #include <novatel_gps_driver/parsers/header.h>
00042 #include <novatel_gps_driver/parsers/gpgga.h>
00043 #include <novatel_gps_driver/parsers/gprmc.h>
00044 
00045 namespace novatel_gps_driver
00046 {
00047   const std::string NovatelMessageExtractor::CHECKSUM_FLAG = "*";
00048   const std::string NovatelMessageExtractor::FIELD_SEPARATOR = ",";
00049   const std::string NovatelMessageExtractor::HEADER_SEPARATOR = ";";
00050   const std::string NovatelMessageExtractor::NMEA_SENTENCE_FLAG = "$";
00051   const std::string NovatelMessageExtractor::NOVATEL_SENTENCE_FLAG = "#";
00052   const std::string NovatelMessageExtractor::NOVATEL_ASCII_FLAGS = "$#";
00053   const std::string NovatelMessageExtractor::NOVATEL_BINARY_SYNC_BYTES = "\xAA\x44\x12";
00054   const std::string NovatelMessageExtractor::NOVATEL_ENDLINE = "\r\n";
00055   
00056   uint32_t NovatelMessageExtractor::CRC32Value(int32_t i)
00057   {
00058     int32_t j;
00059     uint32_t ulCRC;
00060     ulCRC = static_cast<uint32_t>(i);
00061     for ( j = 8 ; j > 0; j-- )
00062     {
00063       if ( ulCRC & 1 )
00064         ulCRC = static_cast<uint32_t>(( ulCRC >> 1 ) ^ NOVATEL_CRC32_POLYNOMIAL);
00065       else
00066         ulCRC >>= 1;
00067     }
00068     return ulCRC;
00069   }
00070 
00071   uint32_t NovatelMessageExtractor::CalculateBlockCRC32(
00072       uint32_t ulCount,          // Number of bytes in the data block
00073       const uint8_t* ucBuffer )  // Data block
00074   {
00075     uint32_t ulTemp1;
00076     uint32_t ulTemp2;
00077     uint32_t ulCRC = 0;
00078     while ( ulCount-- != 0 )
00079     {
00080       ulTemp1 = static_cast<uint32_t>(( ulCRC >> 8 ) & 0x00FFFFFFL);
00081       ulTemp2 = CRC32Value( ((int32_t) ulCRC ^ *ucBuffer++ ) & 0xff );
00082       ulCRC = ulTemp1 ^ ulTemp2;
00083     }
00084     return( ulCRC );
00085   }
00086 
00087   uint8_t NovatelMessageExtractor::NmeaChecksum(const std::string& sentence)
00088   {
00089     uint8_t checksum = 0;
00090     std::string::const_iterator it = sentence.begin();
00091     for (; it != sentence.end(); ++it)
00092     {
00093       checksum ^= *it;
00094     }
00095     return checksum;
00096   }
00097 
00098   size_t NovatelMessageExtractor::GetSentenceChecksumStart(const std::string& str, size_t start_idx)
00099   {
00100     return str.find(CHECKSUM_FLAG, start_idx);
00101   }
00102 
00103   void NovatelMessageExtractor::VectorizeString(
00104       const std::string& str,
00105       std::vector<std::string>& vectorized_message,
00106       const std::string& delimiters)
00107   {
00108     boost::algorithm::split(vectorized_message, str, boost::algorithm::is_any_of(delimiters));
00109   }
00110 
00111   bool NovatelMessageExtractor::GetNovatelMessageParts(
00112       const std::string& sentence,
00113       std::string& message_id,
00114       std::vector<std::string>& header,
00115       std::vector<std::string>& body)
00116   {
00117     message_id.clear();
00118     header.clear();
00119     body.clear();
00120 
00121     std::vector<std::string> vectorized_message;
00122     VectorizeString(sentence, vectorized_message, HEADER_SEPARATOR);
00123 
00124     if (vectorized_message.size() != 2)
00125     {
00126       return false;
00127     }
00128 
00129     VectorizeString(vectorized_message[0], header, FIELD_SEPARATOR);
00130 
00131     VectorizeString(vectorized_message[1], body, FIELD_SEPARATOR);
00132 
00133     if (!header.empty())
00134     {
00135       message_id = header.front();
00136     }
00137     else
00138     {
00139       return false;
00140     }
00141     return true;
00142   }
00143 
00144   int32_t NovatelMessageExtractor::GetBinaryMessage(const std::string& str,
00145                            size_t start_idx,
00146                            BinaryMessage& msg)
00147   {
00148     if (str.length() < HeaderParser::BINARY_HEADER_LENGTH + 4)
00149     {
00150       // The shortest a binary message can be (header + no data + CRC)
00151       // is 32 bytes, so just return if we don't have at least that many.
00152       ROS_DEBUG("Binary message was too short to parse.");
00153       return -1;
00154     }
00155 
00156     ROS_DEBUG("Reading binary header.");
00157     msg.header_.ParseHeader(reinterpret_cast<const uint8_t*>(&str[start_idx]));
00158     uint16_t data_start = static_cast<uint16_t>(msg.header_.header_length_ + start_idx);
00159     uint16_t data_length = msg.header_.message_length_;
00160 
00161     if (msg.header_.sync0_ != static_cast<uint8_t>(NOVATEL_BINARY_SYNC_BYTES[0]) ||
00162         msg.header_.sync1_ != static_cast<uint8_t>(NOVATEL_BINARY_SYNC_BYTES[1]) ||
00163         msg.header_.sync2_ != static_cast<uint8_t>(NOVATEL_BINARY_SYNC_BYTES[2]))
00164     {
00165       ROS_ERROR("Sync bytes were incorrect; this should never happen and is definitely a bug: %x %x %x",
00166                msg.header_.sync0_, msg.header_.sync1_, msg.header_.sync2_);
00167       return -2;
00168     }
00169 
00170     if (msg.header_.header_length_ != HeaderParser::BINARY_HEADER_LENGTH)
00171     {
00172       ROS_WARN("Binary header length was unexpected: %u (expected %u)",
00173                msg.header_.header_length_, HeaderParser::BINARY_HEADER_LENGTH);
00174     }
00175 
00176     ROS_DEBUG("Msg ID: %u    Data start / length: %u / %u",
00177               msg.header_.message_id_, data_start, data_length);
00178 
00179     if (data_start + data_length + 4 > str.length())
00180     {
00181       ROS_DEBUG("Not enough data.");
00182       return -1;
00183     }
00184 
00185     ROS_DEBUG("Reading binary message data.");
00186     msg.data_.resize(data_length);
00187     std::copy(&str[data_start], &str[data_start+data_length], reinterpret_cast<char*>(&msg.data_[0]));
00188 
00189     ROS_DEBUG("Calculating CRC.");
00190 
00191     uint32_t crc = CalculateBlockCRC32(static_cast<uint32_t>(msg.header_.header_length_) + data_length,
00192                                        reinterpret_cast<const uint8_t*>(&str[start_idx]));
00193 
00194     ROS_DEBUG("Reading CRC.");
00195     msg.crc_ = ParseUInt32(reinterpret_cast<const uint8_t*>(&str[data_start+data_length]));
00196 
00197     if (crc != msg.crc_)
00198     {
00199       // Invalid CRC
00200       ROS_DEBUG("Invalid CRC;  Calc: %u    In msg: %u", crc, msg.crc_);
00201       return -2;
00202     }
00203 
00204     // On success, return how many bytes we read
00205 
00206     ROS_DEBUG("Finishing reading binary message.");
00207     return static_cast<int32_t>(msg.header_.header_length_ + data_length + 4);
00208   }
00209 
00210   int32_t NovatelMessageExtractor::GetNovatelSentence(
00211       const std::string& str,
00212       size_t start_idx,
00213       std::string& sentence)
00214   {
00215     sentence.clear();
00216 
00217     size_t checksum_start = GetSentenceChecksumStart(str, start_idx);
00218     if (checksum_start == std::string::npos)
00219     {
00220       // Sentence not complete. Just return.
00221       return -1;
00222     }
00223     else if (checksum_start + 8 >= str.size())
00224     {
00225       // Sentence not complete. Just return.
00226       return -1;
00227     }
00228     else
00229     {
00230       // Compare the checksums
00231       sentence = str.substr(start_idx + 1, checksum_start - start_idx - 1);
00232       std::string checksum_str = str.substr(checksum_start + 1, 8);
00233       uint64_t checksum = std::strtoul(checksum_str.c_str(), 0, 16);
00234       uint64_t calculated_checksum = CalculateBlockCRC32(
00235           static_cast<uint32_t>(sentence.size()),
00236           reinterpret_cast<const uint8_t*>(sentence.c_str()));
00237 
00238       if (checksum == ULONG_MAX)
00239       {
00240         // Invalid checksum -- strtoul failed
00241         return 1;
00242       }
00243       else if(static_cast<uint32_t>(checksum) == calculated_checksum)
00244       {
00245         return 0;
00246       }
00247       else
00248       {
00249         ROS_WARN("Expected checksum: [%lx]", calculated_checksum);
00250         // Invalid checksum
00251         return 1;
00252       }
00253     }
00254   }
00255 
00256   int32_t NovatelMessageExtractor::GetNmeaSentence(
00257       const std::string& str,
00258       size_t start_idx,
00259       std::string& sentence,
00260       bool keep_container)
00261   {
00262     sentence.clear();
00263 
00264     size_t checksum_start = GetSentenceChecksumStart(str, start_idx);
00265     if (checksum_start == std::string::npos)
00266     {
00267       // Sentence not complete. Just return.
00268       return -1;
00269     }
00270     else if (checksum_start + 2 >= str.size())
00271     {
00272       // Sentence not complete. Just return.
00273       return -1;
00274     }
00275     else
00276     {
00277       // Compare the checksums
00278       sentence = str.substr(start_idx + 1, checksum_start - start_idx - 1);
00279       std::string checksum_str = str.substr(checksum_start + 1, 2);
00280       uint64_t checksum = std::strtoul(checksum_str.c_str(), 0, 16);
00281       uint64_t calculated_checksum = NmeaChecksum(sentence);
00282       if (checksum == ULONG_MAX)
00283       {
00284         // Invalid checksum
00285         return 1;
00286       }
00287       else if(static_cast<uint32_t>(checksum) == calculated_checksum)
00288       {
00289         if (keep_container)
00290         {
00291           sentence.insert(0, "$");
00292           std::string recreated_checksum_str("*");
00293           recreated_checksum_str += checksum_str;
00294           sentence.insert(sentence.end(),
00295               recreated_checksum_str.begin(),
00296               recreated_checksum_str.end());
00297         }
00298         return 0;
00299       }
00300       else
00301       {
00302         ROS_WARN("Expected: [%lx]", calculated_checksum);
00303         // Invalid checksum
00304         return 1;
00305       }
00306     }
00307   }
00308 
00309   void NovatelMessageExtractor::FindAsciiSentence(const std::string& sentence,
00310                          size_t current_idx,
00311                          size_t& start_idx,
00312                          size_t& end_idx,
00313                          size_t& invalid_char_idx)
00314   {
00315     start_idx = sentence.find_first_of(NOVATEL_ASCII_FLAGS, current_idx);
00316     end_idx = std::string::npos;
00317     invalid_char_idx = std::string::npos;
00318 
00319     if (start_idx == std::string::npos)
00320     {
00321       return;
00322     }
00323 
00324     end_idx = sentence.find(NOVATEL_ENDLINE, start_idx);
00325 
00326     size_t search_stop_idx = std::min(end_idx, sentence.length());
00327     for (size_t i = start_idx; i < search_stop_idx; i++)
00328     {
00329       if (sentence[i] == 9 || sentence[i] == 10 || sentence[i] == 11 || sentence[i] == 13 ||
00330           (sentence[i] >= 32 && sentence[i] <= 126))
00331       {
00332         continue;
00333       }
00334 
00335       invalid_char_idx = i;
00336       break;
00337     }
00338   }
00339 
00340   bool NovatelMessageExtractor::VectorizeNovatelSentence(
00341       const std::string& data,
00342       NovatelSentence& sentence)
00343   {
00344     return GetNovatelMessageParts(
00345         data, sentence.id, sentence.header, sentence.body);
00346   }
00347 
00348   void NovatelMessageExtractor::VectorizeNmeaSentence(
00349     const std::string& sentence,
00350     NmeaSentence& vectorized_message)
00351   {
00352     VectorizeString(sentence, vectorized_message.body, FIELD_SEPARATOR);
00353     if (!vectorized_message.body.empty())
00354     {
00355       vectorized_message.id = vectorized_message.body.front();
00356     }
00357   }
00358 
00359   bool NovatelMessageExtractor::ExtractCompleteMessages(
00360       const std::string input,
00361       std::vector<NmeaSentence>& nmea_sentences,
00362       std::vector<NovatelSentence>& novatel_sentences,
00363       std::vector<BinaryMessage>& binary_messages,
00364       std::string& remaining,
00365       bool keep_nmea_container)
00366   {
00367     bool parse_error = false;
00368 
00369     size_t sentence_start = 0;
00370     while(sentence_start != std::string::npos && sentence_start < input.size())
00371     {
00372       size_t ascii_start_idx;
00373       size_t ascii_end_idx;
00374       size_t invalid_ascii_idx;
00375       size_t binary_start_idx = input.find(NOVATEL_BINARY_SYNC_BYTES, sentence_start);
00376 
00377       FindAsciiSentence(input, sentence_start, ascii_start_idx, ascii_end_idx, invalid_ascii_idx);
00378 
00379       ROS_DEBUG("Binary start: %lu   ASCII start / end / invalid: %lu / %lu / %lu",
00380                 binary_start_idx, ascii_start_idx, ascii_end_idx, invalid_ascii_idx);
00381 
00382       if (binary_start_idx == std::string::npos && ascii_start_idx == std::string::npos)
00383       {
00384         // If we don't see either a binary or an ASCII message, just give up.
00385         break;
00386       }
00387 
00388       if (ascii_start_idx == std::string::npos ||
00389           (binary_start_idx != std::string::npos && binary_start_idx < ascii_start_idx))
00390       {
00391         // If we see a binary header or if there's both a binary and ASCII header but
00392         // the binary one comes first, try to parse it.
00393         BinaryMessage cur_msg;
00394         int32_t result = GetBinaryMessage(input, binary_start_idx, cur_msg);
00395         if (result > 0)
00396         {
00397           binary_messages.push_back(cur_msg);
00398           sentence_start += binary_start_idx + result;
00399           ROS_DEBUG("Parsed a binary message with %u bytes.", result);
00400         }
00401         else if (result == -1)
00402         {
00403           // Sentence is not complete, add it to the remaining and break;
00404           remaining = input.substr(binary_start_idx);
00405           ROS_DEBUG("Binary message was incomplete, waiting for more.");
00406           break;
00407         }
00408         else
00409         {
00410           // Sentence had an invalid checksum, just iterate to the next sentence
00411           sentence_start += 1;
00412           ROS_WARN("Invalid binary message checksum");
00413           parse_error = true;
00414         }
00415       }
00416       else
00417       {
00418         // If we saw the beginning of an ASCII message, try to parse it.
00419         size_t ascii_len = ascii_end_idx - ascii_start_idx;
00420         if (invalid_ascii_idx != std::string::npos)
00421         {
00422           // If we see an invalid character, don't even bother trying to parse
00423           // the rest of the message.  By this point we also know there's no
00424           // binary header before this point, so just skip the data.
00425           ROS_WARN("Invalid ASCII char: [%s]", input.substr(ascii_start_idx, ascii_len).c_str());
00426           ROS_WARN("                     %s^", std::string(invalid_ascii_idx-ascii_start_idx-1, ' ').c_str());
00427           sentence_start += invalid_ascii_idx + 1;
00428         }
00429         else if (ascii_end_idx != std::string::npos)
00430         {
00431           // If we've got a start, an end, and no invalid characters, we've
00432           // got a valid ASCII message.
00433           ROS_DEBUG("ASCII sentence:\n[%s]", input.substr(ascii_start_idx, ascii_len).c_str());
00434           if (input[ascii_start_idx] == NMEA_SENTENCE_FLAG[0])
00435           {
00436             std::string cur_sentence;
00437             int32_t result = GetNmeaSentence(
00438                 input,
00439                 ascii_start_idx,
00440                 cur_sentence,
00441                 keep_nmea_container);
00442             if (result == 0)
00443             {
00444               nmea_sentences.push_back(NmeaSentence());
00445               VectorizeNmeaSentence(cur_sentence, nmea_sentences.back());
00446               sentence_start = ascii_end_idx;
00447             }
00448             else if (result < 0)
00449             {
00450               // Sentence is not complete, add it to the remaining and break.
00451               // This is legacy code from before FindAsciiSentence was implemented,
00452               // and it will probably never happen, but it doesn't hurt anything to
00453               // have it here.
00454               remaining = input.substr(ascii_start_idx);
00455               ROS_DEBUG("Waiting for more NMEA data.");
00456               break;
00457             }
00458             else
00459             {
00460               ROS_WARN("Invalid NMEA checksum for: [%s]",
00461                        input.substr(ascii_start_idx, ascii_len).c_str());
00462               // Sentence had an invalid checksum, just iterate to the next sentence
00463               sentence_start += 1;
00464               parse_error = true;
00465             }
00466           }
00467           else if (input[ascii_start_idx] == NOVATEL_SENTENCE_FLAG[0])
00468           {
00469             std::string cur_sentence;
00470             int32_t result = GetNovatelSentence(input, ascii_start_idx, cur_sentence);
00471             if (result == 0)
00472             {
00473               // Send to parser for testing:
00474               novatel_sentences.push_back(NovatelSentence());
00475               if (!VectorizeNovatelSentence(cur_sentence, novatel_sentences.back()))
00476               {
00477                 novatel_sentences.pop_back();
00478                 parse_error = true;
00479                 ROS_ERROR_THROTTLE(1.0, "Unable to vectorize novatel sentence");
00480               }
00481               sentence_start = ascii_end_idx;
00482             }
00483             else if (result < 0)
00484             {
00485 
00486               // Sentence is not complete, add it to the remaining and break.
00487               // This is legacy code from before FindAsciiSentence was implemented,
00488               // and it will probably never happen, but it doesn't hurt anything to
00489               // have it here.
00490               remaining = input.substr(ascii_start_idx);
00491               ROS_DEBUG("Waiting for more NovAtel data.");
00492               break;
00493             }
00494             else
00495             {
00496               // Sentence had an invalid checksum, just iterate to the next sentence
00497               sentence_start += 1;
00498               ROS_WARN("Invalid NovAtel checksum for: [%s]",
00499                        input.substr(ascii_start_idx, ascii_len).c_str());
00500               parse_error = true;
00501             }
00502           }
00503         }
00504         else
00505         {
00506           ROS_DEBUG("Incomplete ASCII sentence, waiting for more.");
00507           remaining = input.substr(ascii_start_idx);
00508           break;
00509         }
00510       }
00511     }
00512 
00513     return !parse_error;
00514   }
00515 
00516   double NovatelMessageExtractor::GetMostRecentUtcTime(const std::vector<NmeaSentence>& sentences)
00517   {
00518     std::vector<NmeaSentence>::const_reverse_iterator iter;
00519     for (iter = sentences.rbegin(); iter != sentences.rend(); iter++)
00520     {
00521       if (iter->id == GpggaParser::MESSAGE_NAME || iter->id == GprmcParser::MESSAGE_NAME)
00522       {
00523         if (iter->body.size() > 1)
00524         {
00525           if (iter->body[1].empty() || iter->body[1] == "0")
00526           {
00527             return 0;
00528           }
00529           else
00530           {
00531             double utc_float;
00532             if (swri_string_util::ToDouble(iter->body[1], utc_float))
00533             {
00534               return UtcFloatToSeconds(utc_float);
00535             }
00536             return 0;
00537           }
00538         }
00539       }
00540     }
00541 
00542     return 0;
00543   }
00544 
00545   void NovatelMessageExtractor::GetGpsFixMessage(
00546       const novatel_gps_msgs::Gprmc& gprmc,
00547       const novatel_gps_msgs::Gpgga& gpgga,
00548       gps_common::GPSFixPtr gps_fix)
00549   {
00550     gps_fix->header.stamp = gpgga.header.stamp;
00551     gps_fix->altitude = gpgga.alt;
00552     // gps_fix.climb // We don't get this information from the receiver
00553     // gps_fix.dip // We don't get this information from the receiver
00554     // gps_fix.err // To be filled in later from BestPos messages
00555     // gps_fix.err_climb // We don't get this information from the receiver
00556     // gps_fix.err_dip // We don't get this information from the receiver
00557     // gps_fix.err_horz // To be filled in later from BestPos messages
00558     // gps_fix.err_pitch // We don't get this information from the receiver
00559     // gps_fix.err_roll // We don't get this information from the receiver
00560     // gps_fix.err_speed = ERR_INIT_HIGH;
00561     // gps_fix.err_time = ERR_INIT_HIGH;
00562     // gps_fix.err_track = ERR_INIT_HIGH;
00563     // gps_fix.err_vert = ERR_INIT_HIGH;
00564     // gps_fix.gdop = ERR_INIT_HIGH
00565     gps_fix->hdop = gpgga.hdop;
00566     gps_fix->latitude = gprmc.lat;
00567     if (gpgga.lat_dir == "S")
00568     {
00569       gps_fix->latitude *= -1;
00570     }
00571 
00572     gps_fix->longitude = gprmc.lon;
00573     if (gpgga.lon_dir == "W")
00574     {
00575       gps_fix->longitude *= -1;
00576     }
00577     // gps_fix.pdop = ERR_INIT_HIGH;
00578     // gps_fix.pitch = 0.0;
00579     // gps_fix.roll = 0.0;
00580     gps_fix->speed = gprmc.speed;
00581     // gps_fix.tdop = ERR_INIT_HIGH;
00582     gps_fix->time = gpgga.utc_seconds;
00583     gps_fix->track = gprmc.track;
00584     // gps_fix.vdop = ERR_INIT_HIGH;
00585 
00586     gps_fix->status.status = gps_common::GPSStatus::STATUS_FIX;
00587     gps_fix->status.satellites_used = static_cast<uint16_t>(gpgga.num_sats);
00588 
00589   }
00590 
00591 }


novatel_gps_driver
Author(s):
autogenerated on Sun Oct 8 2017 02:40:29