.. _program_listing_file__tmp_ws_src_novatel_gps_driver_novatel_gps_driver_include_novatel_gps_driver_novatel_gps.h:

Program Listing for File novatel_gps.h
======================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_novatel_gps_driver_novatel_gps_driver_include_novatel_gps_driver_novatel_gps.h>` (``/tmp/ws/src/novatel_gps_driver/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // *****************************************************************************
   //
   // Copyright (c) 2019, Southwest Research Institute® (SwRI®)
   // All rights reserved.
   //
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions are met:
   //     * Redistributions of source code must retain the above copyright
   //       notice, this list of conditions and the following disclaimer.
   //     * Redistributions in binary form must reproduce the above copyright
   //       notice, this list of conditions and the following disclaimer in the
   //       documentation and/or other materials provided with the distribution.
   //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
   //       names of its contributors may be used to endorse or promote products
   //       derived from this software without specific prior written permission.
   //
   // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
   // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
   // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
   // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   //
   // *****************************************************************************
   
   #ifndef NOVATEL_GPS_DRIVER_NOVATEL_GPS_H_
   #define NOVATEL_GPS_DRIVER_NOVATEL_GPS_H_
   
   // std libraries
   #include <chrono>
   #include <map>
   #include <queue>
   #include <string>
   #include <vector>
   
   // Boost
   #include <boost/asio.hpp>
   #include <boost/circular_buffer.hpp>
   
   // ROS
   #include <rclcpp/rclcpp.hpp>
   #include <rclcpp/time.hpp>
   
   // Messages
   #include <gps_msgs/msg/gps_fix.hpp>
   
   #include <novatel_gps_msgs/msg/gpgga.hpp>
   #include <novatel_gps_msgs/msg/gprmc.hpp>
   
   #include <sensor_msgs/msg/imu.hpp>
   
   // Other libraries
   #include <swri_serial_util/serial_port.h>
   
   #include <pcap.h>
   
   // Local include files
   #include <novatel_gps_driver/novatel_message_extractor.h>
   #include <novatel_gps_driver/parsers/bestpos.h>
   #include <novatel_gps_driver/parsers/bestxyz.h>
   #include <novatel_gps_driver/parsers/bestutm.h>
   #include <novatel_gps_driver/parsers/bestvel.h>
   #include <novatel_gps_driver/parsers/clocksteering.h>
   #include <novatel_gps_driver/parsers/corrimudata.h>
   #include <novatel_gps_driver/parsers/gpgga.h>
   #include <novatel_gps_driver/parsers/gpgsa.h>
   #include <novatel_gps_driver/parsers/gpgsv.h>
   #include <novatel_gps_driver/parsers/gphdt.h>
   #include <novatel_gps_driver/parsers/gprmc.h>
   #include <novatel_gps_driver/parsers/heading2.h>
   #include <novatel_gps_driver/parsers/dual_antenna_heading.h>
   #include <novatel_gps_driver/parsers/inscov.h>
   #include <novatel_gps_driver/parsers/inspva.h>
   #include <novatel_gps_driver/parsers/inspvax.h>
   #include <novatel_gps_driver/parsers/insstdev.h>
   #include <novatel_gps_driver/parsers/range.h>
   #include <novatel_gps_driver/parsers/psrdop2.h>
   #include <novatel_gps_driver/parsers/time.h>
   #include <novatel_gps_driver/parsers/trackstat.h>
   
   namespace novatel_gps_driver
   {
     typedef std::map<std::string, double> NovatelMessageOpts;
   
     class NovatelGps
     {
       public:
         enum ConnectionType { SERIAL, TCP, UDP, PCAP, INVALID };
   
         enum ReadResult
         {
           READ_SUCCESS = 0,
           READ_INSUFFICIENT_DATA = 1,
           READ_TIMEOUT = 2,
           READ_INTERRUPTED = 3,
           READ_ERROR = -1,
           READ_PARSE_FAILED = -2
         };
   
         explicit NovatelGps(rclcpp::Node& Node);
         ~NovatelGps();
   
         bool Connect(const std::string& device, ConnectionType connection);
   
         bool Connect(const std::string& device, ConnectionType connection, NovatelMessageOpts const& opts);
   
         void Disconnect();
   
         std::string ErrorMsg() const { return error_msg_; }
   
         void GetFixMessages(std::vector<gps_msgs::msg::GPSFix::UniquePtr>& fix_messages);
         void GetGpggaMessages(std::vector<novatel_gps_driver::GpggaParser::MessageType>& gpgga_messages);
         void GetGpgsaMessages(std::vector<novatel_gps_driver::GpgsaParser::MessageType>& gpgsa_messages);
         void GetGpgsvMessages(std::vector<novatel_gps_driver::GpgsvParser::MessageType>& gpgsv_messages);
         void GetGphdtMessages(std::vector<novatel_gps_driver::GphdtParser::MessageType>& gphdt_messages);
         void GetGprmcMessages(std::vector<novatel_gps_driver::GprmcParser::MessageType>& gprmc_messages);
         void GetNovatelHeading2Messages(std::vector<novatel_gps_driver::Heading2Parser::MessageType>& headings);
         void GetNovatelDualAntennaHeadingMessages(std::vector<novatel_gps_driver::DualAntennaHeadingParser::MessageType>& headings);
          void GetNovatelPsrdop2Messages(std::vector<novatel_gps_driver::Psrdop2Parser::MessageType>& psrdop2_messages);
         void GetImuMessages(std::vector<sensor_msgs::msg::Imu::SharedPtr>& imu_messages);
         void GetInscovMessages(std::vector<novatel_gps_driver::InscovParser::MessageType>& inscov_messages);
         void GetInspvaMessages(std::vector<novatel_gps_driver::InspvaParser::MessageType>& inspva_messages);
         void GetInspvaxMessages(std::vector<novatel_gps_driver::InspvaxParser::MessageType>& inspvax_messages);
         void GetInsstdevMessages(std::vector<novatel_gps_driver::InsstdevParser::MessageType>& insstdev_messages);
         void GetNovatelCorrectedImuData(std::vector<novatel_gps_driver::CorrImuDataParser::MessageType>& imu_messages);
         void GetNovatelPositions(std::vector<novatel_gps_driver::BestposParser::MessageType>& positions);
         void GetNovatelXYZPositions(std::vector<novatel_gps_driver::BestxyzParser::MessageType>& positions);
         void GetNovatelUtmPositions(std::vector<novatel_gps_driver::BestutmParser::MessageType>& utm_positions);
         void GetNovatelVelocities(std::vector<novatel_gps_driver::BestvelParser::MessageType>& velocities);
         void GetRangeMessages(std::vector<novatel_gps_driver::RangeParser::MessageType>& range_messages);
         void GetTimeMessages(std::vector<novatel_gps_driver::TimeParser::MessageType>& time_messages);
         void GetTrackstatMessages(std::vector<novatel_gps_driver::TrackstatParser::MessageType>& trackstat_msgs);
         void GetClockSteeringMessages(std::vector<novatel_gps_driver::ClockSteeringParser::MessageType>& clocksteering_msgs);
   
         bool IsConnected() { return is_connected_; }
   
         static ConnectionType ParseConnection(const std::string& connection);
   
         void ApplyVehicleBodyRotation(const bool& apply_rotation);
   
         ReadResult ProcessData();
   
         void SetImuRate(double imu_rate, bool force = true);
   
         void SetSerialBaud(int32_t serial_baud);
   
         bool Write(const std::string& command);
   
         double gpsfix_sync_tol_; //seconds
         bool wait_for_sync_; // wait until a bestvel has arrived before publishing bestpos
   
       private:
         bool Configure(NovatelMessageOpts const& opts);
   
         bool CreateIpConnection(const std::string& endpoint, NovatelMessageOpts const& opts);
   
         bool CreateSerialConnection(const std::string& device, NovatelMessageOpts const& opts);
   
         bool CreatePcapConnection(const std::string& device, NovatelMessageOpts const& opts);
   
         template<typename Input, typename Output>
         inline void DrainQueue(Input& in, Output& out)
         {
           out.clear();
           std::move(std::make_move_iterator(in.begin()),
               std::make_move_iterator(in.end()),
               std::back_inserter(out));
           in.clear();
         }
   
         void GenerateImuMessages();
   
         NovatelGps::ReadResult ParseBinaryMessage(const BinaryMessage& msg,
                                                   const rclcpp::Time& stamp) noexcept(false);
         NovatelGps::ReadResult ParseNmeaSentence(const NmeaSentence& sentence,
                                                  const rclcpp::Time& stamp,
                                                  double most_recent_utc_time) noexcept(false);
         NovatelGps::ReadResult ParseNovatelSentence(const NovatelSentence& sentence,
                                                     const rclcpp::Time& stamp) noexcept(false);
   
         ReadResult ReadData();
   
         static constexpr uint16_t DEFAULT_TCP_PORT = 3001;
         static constexpr uint16_t DEFAULT_UDP_PORT = 3002;
         static constexpr size_t MAX_BUFFER_SIZE = 100;
         static constexpr size_t SYNC_BUFFER_SIZE = 10;
         static constexpr uint32_t SECONDS_PER_WEEK = 604800;
         static constexpr double IMU_TOLERANCE_S = 0.0002;
         static constexpr double DEGREES_TO_RADIANS = M_PI / 180.0;
   
   
         rclcpp::Node& node_;
   
         ConnectionType connection_;
   
         std::string error_msg_;
   
         bool is_connected_;
         bool imu_rate_forced_;
   
         double utc_offset_;
   
         // Serial connection
         int32_t serial_baud_;
         swri_serial_util::SerialPort serial_;
   
         // TCP / UDP connections
         boost::asio::io_service io_service_;
         boost::asio::ip::tcp::socket tcp_socket_;
         std::shared_ptr<boost::asio::ip::udp::socket> udp_socket_;
         std::shared_ptr<boost::asio::ip::udp::endpoint> udp_endpoint_;
   
         // Data buffers
         std::vector<uint8_t> data_buffer_;
         std::string nmea_buffer_;
         std::array<uint8_t, 10000> socket_buffer_;
   
         pcap_t* pcap_;
         bpf_program pcap_packet_filter_;
         char pcap_errbuf_[MAX_BUFFER_SIZE];
         std::vector<uint8_t> last_tcp_packet_;
   
         NovatelMessageExtractor extractor_;
   
         // Message parsers
         BestposParser bestpos_parser_;
         BestxyzParser bestxyz_parser_;
         BestutmParser bestutm_parser_;
         BestvelParser bestvel_parser_;
         Heading2Parser heading2_parser_;
         DualAntennaHeadingParser dual_antenna_heading_parser_;
         ClockSteeringParser clocksteering_parser_;
         CorrImuDataParser corrimudata_parser_;
         GpggaParser gpgga_parser_;
         GpgsaParser gpgsa_parser_;
         GpgsvParser gpgsv_parser_;
         GphdtParser gphdt_parser_;
         GprmcParser gprmc_parser_;
         InscovParser inscov_parser_;
         InspvaParser inspva_parser_;
         InspvaxParser inspvax_parser_;
         InsstdevParser insstdev_parser_;
         Psrdop2Parser psrdop2_parser_;
         RangeParser range_parser_;
         TimeParser time_parser_;
         TrackstatParser trackstat_parser_;
   
         // Message buffers
         boost::circular_buffer<novatel_gps_driver::ClockSteeringParser::MessageType> clocksteering_msgs_;
         boost::circular_buffer<novatel_gps_driver::CorrImuDataParser::MessageType> corrimudata_msgs_;
         boost::circular_buffer<novatel_gps_driver::GpggaParser::MessageType> gpgga_msgs_;
         boost::circular_buffer<novatel_gps_driver::GpgsaParser::MessageType> gpgsa_msgs_;
         boost::circular_buffer<novatel_gps_driver::GpgsvParser::MessageType> gpgsv_msgs_;
         boost::circular_buffer<novatel_gps_driver::GphdtParser::MessageType> gphdt_msgs_;
         boost::circular_buffer<novatel_gps_driver::GprmcParser::MessageType> gprmc_msgs_;
         boost::circular_buffer<sensor_msgs::msg::Imu::SharedPtr> imu_msgs_;
         boost::circular_buffer<novatel_gps_driver::InscovParser::MessageType> inscov_msgs_;
         boost::circular_buffer<novatel_gps_driver::InspvaParser::MessageType> inspva_msgs_;
         boost::circular_buffer<novatel_gps_driver::InspvaxParser::MessageType> inspvax_msgs_;
         boost::circular_buffer<novatel_gps_driver::InsstdevParser::MessageType> insstdev_msgs_;
         boost::circular_buffer<novatel_gps_driver::BestposParser::MessageType> novatel_positions_;
         boost::circular_buffer<novatel_gps_driver::BestxyzParser::MessageType> novatel_xyz_positions_;
         boost::circular_buffer<novatel_gps_driver::BestutmParser::MessageType> novatel_utm_positions_;
         boost::circular_buffer<novatel_gps_driver::BestvelParser::MessageType> novatel_velocities_;
         boost::circular_buffer<novatel_gps_driver::BestposParser::MessageType> bestpos_sync_buffer_;
         boost::circular_buffer<novatel_gps_driver::BestvelParser::MessageType> bestvel_sync_buffer_;
         boost::circular_buffer<novatel_gps_driver::Heading2Parser::MessageType> heading2_msgs_;
         boost::circular_buffer<novatel_gps_driver::DualAntennaHeadingParser::MessageType> dual_antenna_heading_msgs_;
         boost::circular_buffer<novatel_gps_driver::Psrdop2Parser::MessageType> psrdop2_msgs_;
         boost::circular_buffer<novatel_gps_driver::RangeParser::MessageType> range_msgs_;
         boost::circular_buffer<novatel_gps_driver::TimeParser::MessageType> time_msgs_;
         boost::circular_buffer<novatel_gps_driver::TrackstatParser::MessageType> trackstat_msgs_;
   
         novatel_gps_driver::Psrdop2Parser::MessageType latest_psrdop2_;
   
         // IMU data synchronization queues
         std::queue<novatel_gps_driver::CorrImuDataParser::MessageType> corrimudata_queue_;
         std::queue<novatel_gps_driver::InspvaParser::MessageType> inspva_queue_;
         novatel_gps_driver::InsstdevParser::MessageType latest_insstdev_;
         novatel_gps_driver::InscovParser::MessageType latest_inscov_;
         double imu_rate_;
   
         // Additional Options
         bool apply_vehicle_body_rotation_;
     };
   }
   
   #endif  //NOVATEL_GPS_DRIVER_NOVATEL_GPS_H_