novatel_gps_driver: novatel

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 #ifndef NOVATEL_OEM628_NOVATEL_GPS_H_
00031 #define NOVATEL_OEM628_NOVATEL_GPS_H_
00032 
00033 #include <map>
00034 #include <queue>
00035 #include <string>
00036 #include <vector>
00037 
00038 #include <boost/asio.hpp>
00039 #include <boost/circular_buffer.hpp>
00040 
00041 #include <pcap.h>
00042 
00043 #include <swri_serial_util/serial_port.h>
00044 
00045 #include <gps_common/GPSFix.h>
00046 
00047 #include <novatel_gps_msgs/Gpgga.h>
00048 #include <novatel_gps_msgs/Gpgsa.h>
00049 #include <novatel_gps_msgs/Gprmc.h>
00050 #include <novatel_gps_msgs/Inspva.h>
00051 #include <novatel_gps_msgs/Insstdev.h>
00052 #include <novatel_gps_msgs/NovatelCorrectedImuData.h>
00053 #include <novatel_gps_msgs/NovatelPosition.h>
00054 #include <novatel_gps_msgs/NovatelVelocity.h>
00055 #include <novatel_gps_msgs/Range.h>
00056 #include <novatel_gps_msgs/Time.h>
00057 #include <novatel_gps_msgs/Trackstat.h>
00058 
00059 #include <novatel_gps_driver/novatel_message_extractor.h>
00060 
00061 #include <novatel_gps_driver/parsers/bestpos.h>
00062 #include <novatel_gps_driver/parsers/bestvel.h>
00063 #include <novatel_gps_driver/parsers/corrimudata.h>
00064 #include <novatel_gps_driver/parsers/gpgga.h>
00065 #include <novatel_gps_driver/parsers/gpgsa.h>
00066 #include <novatel_gps_driver/parsers/gpgsv.h>
00067 #include <novatel_gps_driver/parsers/gprmc.h>
00068 #include <novatel_gps_driver/parsers/inspva.h>
00069 #include <novatel_gps_driver/parsers/insstdev.h>
00070 #include <novatel_gps_driver/parsers/range.h>
00071 #include <novatel_gps_driver/parsers/time.h>
00072 #include <novatel_gps_driver/parsers/trackstat.h>
00073 
00074 #include <sensor_msgs/Imu.h>
00075 #include <novatel_gps_driver/parsers/inscov.h>
00076 
00077 namespace novatel_gps_driver
00078 {
00080   typedef std::map<std::string, double> NovatelMessageOpts;
00081 
00082   class NovatelGps
00083   {
00084     public:
00085       enum ConnectionType { SERIAL, TCP, UDP, PCAP, INVALID };
00086 
00087       enum ReadResult
00088       {
00089         READ_SUCCESS = 0,
00090         READ_INSUFFICIENT_DATA = 1,
00091         READ_TIMEOUT = 2,
00092         READ_INTERRUPTED = 3,
00093         READ_ERROR = -1,
00094         READ_PARSE_FAILED = -2
00095       };
00096 
00097       NovatelGps();
00098       ~NovatelGps();
00099 
00108       bool Connect(const std::string& device, ConnectionType connection);
00109 
00119       bool Connect(const std::string& device, ConnectionType connection, NovatelMessageOpts const& opts);
00120 
00124       void Disconnect();
00125 
00129       std::string ErrorMsg() const { return error_msg_; }
00130 
00136       void GetFixMessages(std::vector<gps_common::GPSFixPtr>& fix_messages);
00142       void GetGpggaMessages(std::vector<novatel_gps_msgs::GpggaPtr>& gpgga_messages);
00148       void GetGpgsaMessages(std::vector<novatel_gps_msgs::GpgsaPtr>& gpgsa_messages);
00154       void GetGpgsvMessages(std::vector<novatel_gps_msgs::GpgsvPtr>& gpgsv_messages);
00160       void GetGprmcMessages(std::vector<novatel_gps_msgs::GprmcPtr>& gprmc_messages);
00166       void GetImuMessages(std::vector<sensor_msgs::ImuPtr>& imu_messages);
00172       void GetInscovMessages(std::vector<novatel_gps_msgs::InscovPtr>& inscov_messages);
00178       void GetInspvaMessages(std::vector<novatel_gps_msgs::InspvaPtr>& inspva_messages);
00184       void GetInsstdevMessages(std::vector<novatel_gps_msgs::InsstdevPtr>& insstdev_messages);
00190       void GetNovatelCorrectedImuData(std::vector<novatel_gps_msgs::NovatelCorrectedImuDataPtr>& imu_messages);
00196       void GetNovatelPositions(std::vector<novatel_gps_msgs::NovatelPositionPtr>& positions);
00202       void GetNovatelVelocities(std::vector<novatel_gps_msgs::NovatelVelocityPtr>& velocities);
00208       void GetRangeMessages(std::vector<novatel_gps_msgs::RangePtr>& range_messages);
00214       void GetTimeMessages(std::vector<novatel_gps_msgs::TimePtr>& time_messages);
00220       void GetTrackstatMessages(std::vector<novatel_gps_msgs::TrackstatPtr>& trackstat_msgs);
00221 
00225       bool IsConnected() { return is_connected_; }
00226 
00232       static ConnectionType ParseConnection(const std::string& connection);
00233 
00240       ReadResult ProcessData();
00241 
00246       void SetImuRate(double imu_rate);
00247 
00253       bool Write(const std::string& command);
00254 
00255       //parameters
00256       double gpgga_gprmc_sync_tol_; //seconds
00257       double gpgga_position_sync_tol_; //seconds
00258       bool wait_for_position_; //if false, do not require position message to make gps fix message
00259       //added this because position message is sometimes > 1 s late.
00260 
00261     private:
00269       bool Configure(NovatelMessageOpts const& opts);
00270 
00290       bool CreateIpConnection(const std::string& endpoint, NovatelMessageOpts const& opts);
00291 
00304       bool CreateSerialConnection(const std::string& device, NovatelMessageOpts const& opts);
00305 
00313       bool CreatePcapConnection(const std::string& device, NovatelMessageOpts const& opts);
00314 
00319       void GenerateImuMessages();
00320 
00328       NovatelGps::ReadResult ParseBinaryMessage(const BinaryMessage& msg,
00329                                                 const ros::Time& stamp) throw(ParseException);
00339       NovatelGps::ReadResult ParseNmeaSentence(const NmeaSentence& sentence,
00340                                                const ros::Time& stamp,
00341                                                double most_recent_utc_time) throw(ParseException);
00349       NovatelGps::ReadResult ParseNovatelSentence(const NovatelSentence& sentence,
00350                                                   const ros::Time& stamp) throw(ParseException);
00351 
00357       ReadResult ReadData();
00358 
00359       static constexpr uint16_t DEFAULT_TCP_PORT = 3001;
00360       static constexpr uint16_t DEFAULT_UDP_PORT = 3002;
00361       static constexpr size_t MAX_BUFFER_SIZE = 100;
00362       static constexpr size_t SYNC_BUFFER_SIZE = 10;
00363       static constexpr uint32_t SECONDS_PER_WEEK = 604800;
00364       static constexpr double IMU_TOLERANCE_S = 0.0002;
00365       static constexpr double DEGREES_TO_RADIANS = M_PI / 180.0;
00366 
00367       ConnectionType connection_;
00368 
00369       std::string error_msg_;
00370 
00371       bool is_connected_;
00372 
00373       double utc_offset_;
00374 
00375       // Serial connection
00376       swri_serial_util::SerialPort serial_;
00377 
00378       // TCP / UDP connections
00379       boost::asio::io_service io_service_;
00380       boost::asio::ip::tcp::socket tcp_socket_;
00381       boost::shared_ptr<boost::asio::ip::udp::socket> udp_socket_;
00382       boost::shared_ptr<boost::asio::ip::udp::endpoint> udp_endpoint_;
00383 
00384       // Data buffers
00387       std::vector<uint8_t> data_buffer_;
00389       std::string nmea_buffer_;
00391       boost::array<uint8_t, 10000> socket_buffer_;
00392 
00394       pcap_t* pcap_;
00395       bpf_program pcap_packet_filter_;
00396       char pcap_errbuf_[MAX_BUFFER_SIZE];
00397       std::vector<uint8_t> last_tcp_packet_;
00398 
00400       NovatelMessageExtractor extractor_;
00401 
00402       // Message parsers
00403       BestposParser bestpos_parser_;
00404       BestvelParser bestvel_parser_;
00405       CorrImuDataParser corrimudata_parser_;
00406       GpggaParser gpgga_parser_;
00407       GpgsaParser gpgsa_parser_;
00408       GpgsvParser gpgsv_parser_;
00409       GprmcParser gprmc_parser_;
00410       InscovParser inscov_parser_;
00411       InspvaParser inspva_parser_;
00412       InsstdevParser insstdev_parser_;
00413       RangeParser range_parser_;
00414       TimeParser time_parser_;
00415       TrackstatParser trackstat_parser_;
00416 
00417       // Message buffers
00418       boost::circular_buffer<novatel_gps_msgs::NovatelCorrectedImuDataPtr> corrimudata_msgs_;
00419       boost::circular_buffer<novatel_gps_msgs::GpggaPtr> gpgga_msgs_;
00420       boost::circular_buffer<novatel_gps_msgs::Gpgga> gpgga_sync_buffer_;
00421       boost::circular_buffer<novatel_gps_msgs::GpgsaPtr> gpgsa_msgs_;
00422       boost::circular_buffer<novatel_gps_msgs::GpgsvPtr> gpgsv_msgs_;
00423       boost::circular_buffer<novatel_gps_msgs::GprmcPtr> gprmc_msgs_;
00424       boost::circular_buffer<novatel_gps_msgs::Gprmc> gprmc_sync_buffer_;
00425       boost::circular_buffer<sensor_msgs::ImuPtr> imu_msgs_;
00426       boost::circular_buffer<novatel_gps_msgs::InscovPtr> inscov_msgs_;
00427       boost::circular_buffer<novatel_gps_msgs::InspvaPtr> inspva_msgs_;
00428       boost::circular_buffer<novatel_gps_msgs::InsstdevPtr> insstdev_msgs_;
00429       boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> novatel_positions_;
00430       boost::circular_buffer<novatel_gps_msgs::NovatelVelocityPtr> novatel_velocities_;
00431       boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> position_sync_buffer_;
00432       boost::circular_buffer<novatel_gps_msgs::RangePtr> range_msgs_;
00433       boost::circular_buffer<novatel_gps_msgs::TimePtr> time_msgs_;
00434       boost::circular_buffer<novatel_gps_msgs::TrackstatPtr> trackstat_msgs_;
00435 
00436       // IMU data synchronization queues
00437       std::queue<novatel_gps_msgs::NovatelCorrectedImuDataPtr> corrimudata_queue_;
00438       std::queue<novatel_gps_msgs::InspvaPtr> inspva_queue_;
00439       novatel_gps_msgs::InsstdevPtr latest_insstdev_;
00440       novatel_gps_msgs::InscovPtr latest_inscov_;
00441       double imu_rate_;
00442   };
00443 }
00444 
00445 #endif  // NOVATEL_OEM628_NOVATEL_GPS_H_