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/NovatelXYZ.h>
00055 #include <novatel_gps_msgs/NovatelUtmPosition.h>
00056 #include <novatel_gps_msgs/NovatelVelocity.h>
00057 #include <novatel_gps_msgs/Range.h>
00058 #include <novatel_gps_msgs/Time.h>
00059 #include <novatel_gps_msgs/Trackstat.h>
00060 
00061 #include <novatel_gps_driver/novatel_message_extractor.h>
00062 
00063 #include <novatel_gps_driver/parsers/bestpos.h>
00064 #include <novatel_gps_driver/parsers/bestxyz.h>
00065 #include <novatel_gps_driver/parsers/bestutm.h>
00066 #include <novatel_gps_driver/parsers/bestvel.h>
00067 #include <novatel_gps_driver/parsers/clocksteering.h>
00068 #include <novatel_gps_driver/parsers/corrimudata.h>
00069 #include <novatel_gps_driver/parsers/gpgga.h>
00070 #include <novatel_gps_driver/parsers/gpgsa.h>
00071 #include <novatel_gps_driver/parsers/gpgsv.h>
00072 #include <novatel_gps_driver/parsers/gprmc.h>
00073 #include <novatel_gps_driver/parsers/heading2.h>
00074 #include <novatel_gps_driver/parsers/dual_antenna_heading.h>
00075 #include <novatel_gps_driver/parsers/inspva.h>
00076 #include <novatel_gps_driver/parsers/insstdev.h>
00077 #include <novatel_gps_driver/parsers/range.h>
00078 #include <novatel_gps_driver/parsers/time.h>
00079 #include <novatel_gps_driver/parsers/trackstat.h>
00080 
00081 #include <sensor_msgs/Imu.h>
00082 #include <novatel_gps_driver/parsers/inscov.h>
00083 
00084 namespace novatel_gps_driver
00085 {
00087   typedef std::map<std::string, double> NovatelMessageOpts;
00088 
00089   class NovatelGps
00090   {
00091     public:
00092       enum ConnectionType { SERIAL, TCP, UDP, PCAP, INVALID };
00093 
00094       enum ReadResult
00095       {
00096         READ_SUCCESS = 0,
00097         READ_INSUFFICIENT_DATA = 1,
00098         READ_TIMEOUT = 2,
00099         READ_INTERRUPTED = 3,
00100         READ_ERROR = -1,
00101         READ_PARSE_FAILED = -2
00102       };
00103 
00104       NovatelGps();
00105       ~NovatelGps();
00106 
00115       bool Connect(const std::string& device, ConnectionType connection);
00116 
00126       bool Connect(const std::string& device, ConnectionType connection, NovatelMessageOpts const& opts);
00127 
00131       void Disconnect();
00132 
00136       std::string ErrorMsg() const { return error_msg_; }
00137 
00143       void GetFixMessages(std::vector<gps_common::GPSFixPtr>& fix_messages);
00149       void GetGpggaMessages(std::vector<novatel_gps_msgs::GpggaPtr>& gpgga_messages);
00155       void GetGpgsaMessages(std::vector<novatel_gps_msgs::GpgsaPtr>& gpgsa_messages);
00161       void GetGpgsvMessages(std::vector<novatel_gps_msgs::GpgsvPtr>& gpgsv_messages);
00167       void GetGprmcMessages(std::vector<novatel_gps_msgs::GprmcPtr>& gprmc_messages);
00173       void GetNovatelHeading2Messages(std::vector<novatel_gps_msgs::NovatelHeading2Ptr>& headings);
00179       void GetNovatelDualAntennaHeadingMessages(std::vector<novatel_gps_msgs::NovatelDualAntennaHeadingPtr>& headings);
00185       void GetImuMessages(std::vector<sensor_msgs::ImuPtr>& imu_messages);
00191       void GetInscovMessages(std::vector<novatel_gps_msgs::InscovPtr>& inscov_messages);
00197       void GetInspvaMessages(std::vector<novatel_gps_msgs::InspvaPtr>& inspva_messages);
00203       void GetInsstdevMessages(std::vector<novatel_gps_msgs::InsstdevPtr>& insstdev_messages);
00209       void GetNovatelCorrectedImuData(std::vector<novatel_gps_msgs::NovatelCorrectedImuDataPtr>& imu_messages);
00215       void GetNovatelPositions(std::vector<novatel_gps_msgs::NovatelPositionPtr>& positions);
00221       void GetNovatelXYZPositions(std::vector<novatel_gps_msgs::NovatelXYZPtr>& positions);
00227       void GetNovatelUtmPositions(std::vector<novatel_gps_msgs::NovatelUtmPositionPtr>& utm_positions);
00233       void GetNovatelVelocities(std::vector<novatel_gps_msgs::NovatelVelocityPtr>& velocities);
00239       void GetRangeMessages(std::vector<novatel_gps_msgs::RangePtr>& range_messages);
00245       void GetTimeMessages(std::vector<novatel_gps_msgs::TimePtr>& time_messages);
00251       void GetTrackstatMessages(std::vector<novatel_gps_msgs::TrackstatPtr>& trackstat_msgs);
00257       void GetClockSteeringMessages(std::vector<novatel_gps_msgs::ClockSteeringPtr>& clocksteering_msgs);
00258 
00262       bool IsConnected() { return is_connected_; }
00263 
00269       static ConnectionType ParseConnection(const std::string& connection);
00270 
00276       void ApplyVehicleBodyRotation(const bool& apply_rotation);
00277 
00284       ReadResult ProcessData();
00285 
00291       void SetImuRate(double imu_rate, bool force = true);
00292 
00297       void SetSerialBaud(int32_t serial_baud);
00298 
00304       bool Write(const std::string& command);
00305 
00306       //parameters
00307       double gpgga_gprmc_sync_tol_; //seconds
00308       double gpgga_position_sync_tol_; //seconds
00309       bool wait_for_position_; //if false, do not require position message to make gps fix message
00310       //added this because position message is sometimes > 1 s late.
00311 
00312     private:
00320       bool Configure(NovatelMessageOpts const& opts);
00321 
00341       bool CreateIpConnection(const std::string& endpoint, NovatelMessageOpts const& opts);
00342 
00355       bool CreateSerialConnection(const std::string& device, NovatelMessageOpts const& opts);
00356 
00364       bool CreatePcapConnection(const std::string& device, NovatelMessageOpts const& opts);
00365 
00370       void GenerateImuMessages();
00371 
00379       NovatelGps::ReadResult ParseBinaryMessage(const BinaryMessage& msg,
00380                                                 const ros::Time& stamp) throw(ParseException);
00390       NovatelGps::ReadResult ParseNmeaSentence(const NmeaSentence& sentence,
00391                                                const ros::Time& stamp,
00392                                                double most_recent_utc_time) throw(ParseException);
00400       NovatelGps::ReadResult ParseNovatelSentence(const NovatelSentence& sentence,
00401                                                   const ros::Time& stamp) throw(ParseException);
00402 
00408       ReadResult ReadData();
00409 
00410       static constexpr uint16_t DEFAULT_TCP_PORT = 3001;
00411       static constexpr uint16_t DEFAULT_UDP_PORT = 3002;
00412       static constexpr size_t MAX_BUFFER_SIZE = 100;
00413       static constexpr size_t SYNC_BUFFER_SIZE = 10;
00414       static constexpr uint32_t SECONDS_PER_WEEK = 604800;
00415       static constexpr double IMU_TOLERANCE_S = 0.0002;
00416       static constexpr double DEGREES_TO_RADIANS = M_PI / 180.0;
00417 
00418       ConnectionType connection_;
00419 
00420       std::string error_msg_;
00421 
00422       bool is_connected_;
00423       bool imu_rate_forced_;
00424 
00425       double utc_offset_;
00426 
00427       // Serial connection
00428       int32_t serial_baud_;
00429       swri_serial_util::SerialPort serial_;
00430 
00431       // TCP / UDP connections
00432       boost::asio::io_service io_service_;
00433       boost::asio::ip::tcp::socket tcp_socket_;
00434       boost::shared_ptr<boost::asio::ip::udp::socket> udp_socket_;
00435       boost::shared_ptr<boost::asio::ip::udp::endpoint> udp_endpoint_;
00436 
00437       // Data buffers
00440       std::vector<uint8_t> data_buffer_;
00442       std::string nmea_buffer_;
00444       boost::array<uint8_t, 10000> socket_buffer_;
00445 
00447       pcap_t* pcap_;
00448       bpf_program pcap_packet_filter_;
00449       char pcap_errbuf_[MAX_BUFFER_SIZE];
00450       std::vector<uint8_t> last_tcp_packet_;
00451 
00453       NovatelMessageExtractor extractor_;
00454 
00455       // Message parsers
00456       BestposParser bestpos_parser_;
00457       BestxyzParser bestxyz_parser_;
00458       BestutmParser bestutm_parser_;
00459       BestvelParser bestvel_parser_;
00460       Heading2Parser heading2_parser_;
00461       DualAntennaHeadingParser dual_antenna_heading_parser_;
00462       ClockSteeringParser clocksteering_parser_;
00463       CorrImuDataParser corrimudata_parser_;
00464       GpggaParser gpgga_parser_;
00465       GpgsaParser gpgsa_parser_;
00466       GpgsvParser gpgsv_parser_;
00467       GprmcParser gprmc_parser_;
00468       InscovParser inscov_parser_;
00469       InspvaParser inspva_parser_;
00470       InsstdevParser insstdev_parser_;
00471       RangeParser range_parser_;
00472       TimeParser time_parser_;
00473       TrackstatParser trackstat_parser_;
00474 
00475       // Message buffers
00476       boost::circular_buffer<novatel_gps_msgs::ClockSteeringPtr> clocksteering_msgs_;
00477       boost::circular_buffer<novatel_gps_msgs::NovatelCorrectedImuDataPtr> corrimudata_msgs_;
00478       boost::circular_buffer<novatel_gps_msgs::GpggaPtr> gpgga_msgs_;
00479       boost::circular_buffer<novatel_gps_msgs::Gpgga> gpgga_sync_buffer_;
00480       boost::circular_buffer<novatel_gps_msgs::GpgsaPtr> gpgsa_msgs_;
00481       boost::circular_buffer<novatel_gps_msgs::GpgsvPtr> gpgsv_msgs_;
00482       boost::circular_buffer<novatel_gps_msgs::GprmcPtr> gprmc_msgs_;
00483       boost::circular_buffer<novatel_gps_msgs::Gprmc> gprmc_sync_buffer_;
00484       boost::circular_buffer<sensor_msgs::ImuPtr> imu_msgs_;
00485       boost::circular_buffer<novatel_gps_msgs::InscovPtr> inscov_msgs_;
00486       boost::circular_buffer<novatel_gps_msgs::InspvaPtr> inspva_msgs_;
00487       boost::circular_buffer<novatel_gps_msgs::InsstdevPtr> insstdev_msgs_;
00488       boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> novatel_positions_;
00489       boost::circular_buffer<novatel_gps_msgs::NovatelXYZPtr> novatel_xyz_positions_;
00490       boost::circular_buffer<novatel_gps_msgs::NovatelUtmPositionPtr> novatel_utm_positions_;
00491       boost::circular_buffer<novatel_gps_msgs::NovatelVelocityPtr> novatel_velocities_;
00492       boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> position_sync_buffer_;
00493       boost::circular_buffer<novatel_gps_msgs::NovatelHeading2Ptr> heading2_msgs_;
00494       boost::circular_buffer<novatel_gps_msgs::NovatelDualAntennaHeadingPtr> dual_antenna_heading_msgs_;
00495       boost::circular_buffer<novatel_gps_msgs::RangePtr> range_msgs_;
00496       boost::circular_buffer<novatel_gps_msgs::TimePtr> time_msgs_;
00497       boost::circular_buffer<novatel_gps_msgs::TrackstatPtr> trackstat_msgs_;
00498 
00499       // IMU data synchronization queues
00500       std::queue<novatel_gps_msgs::NovatelCorrectedImuDataPtr> corrimudata_queue_;
00501       std::queue<novatel_gps_msgs::InspvaPtr> inspva_queue_;
00502       novatel_gps_msgs::InsstdevPtr latest_insstdev_;
00503       novatel_gps_msgs::InscovPtr latest_inscov_;
00504       double imu_rate_;
00505 
00506       // Additional Options
00507       bool apply_vehicle_body_rotation_;
00508   };
00509 }
00510 
00511 #endif  // NOVATEL_OEM628_NOVATEL_GPS_H_