novatel_gps.h

Go to the documentation of this file.

// *****************************************************************************
//
// Copyright (c) 2017, 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_OEM628_NOVATEL_GPS_H_
#define NOVATEL_OEM628_NOVATEL_GPS_H_

#include <map>
#include <queue>
#include <string>
#include <vector>

#include <boost/asio.hpp>
#include <boost/circular_buffer.hpp>

#include <pcap.h>

#include <swri_serial_util/serial_port.h>

#include <gps_common/GPSFix.h>

#include <novatel_gps_msgs/Gpgga.h>
#include <novatel_gps_msgs/Gpgsa.h>
#include <novatel_gps_msgs/Gphdt.h>
#include <novatel_gps_msgs/Gprmc.h>
#include <novatel_gps_msgs/Inspva.h>
#include <novatel_gps_msgs/Inspvax.h>
#include <novatel_gps_msgs/Insstdev.h>
#include <novatel_gps_msgs/NovatelCorrectedImuData.h>
#include <novatel_gps_msgs/NovatelPosition.h>
#include <novatel_gps_msgs/NovatelXYZ.h>
#include <novatel_gps_msgs/NovatelUtmPosition.h>
#include <novatel_gps_msgs/NovatelVelocity.h>
#include <novatel_gps_msgs/Range.h>
#include <novatel_gps_msgs/Time.h>
#include <novatel_gps_msgs/Trackstat.h>

#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/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>

#include <sensor_msgs/Imu.h>
#include <novatel_gps_driver/parsers/inscov.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
      };

      NovatelGps();
      ~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_common::GPSFixPtr>& fix_messages);
      void GetGpggaMessages(std::vector<novatel_gps_msgs::GpggaPtr>& gpgga_messages);
      void GetGpgsaMessages(std::vector<novatel_gps_msgs::GpgsaPtr>& gpgsa_messages);
      void GetGpgsvMessages(std::vector<novatel_gps_msgs::GpgsvPtr>& gpgsv_messages);
      void GetGphdtMessages(std::vector<novatel_gps_msgs::GphdtPtr>& gphdt_messages);
      void GetGprmcMessages(std::vector<novatel_gps_msgs::GprmcPtr>& gprmc_messages);
      void GetNovatelHeading2Messages(std::vector<novatel_gps_msgs::NovatelHeading2Ptr>& headings);
      void GetNovatelDualAntennaHeadingMessages(std::vector<novatel_gps_msgs::NovatelDualAntennaHeadingPtr>& headings);
       void GetNovatelPsrdop2Messages(std::vector<novatel_gps_msgs::NovatelPsrdop2Ptr>& psrdop2_messages);
      void GetImuMessages(std::vector<sensor_msgs::ImuPtr>& imu_messages);
      void GetInscovMessages(std::vector<novatel_gps_msgs::InscovPtr>& inscov_messages);
      void GetInspvaMessages(std::vector<novatel_gps_msgs::InspvaPtr>& inspva_messages);
      void GetInspvaxMessages(std::vector<novatel_gps_msgs::InspvaxPtr>& inspvax_messages);
      void GetInsstdevMessages(std::vector<novatel_gps_msgs::InsstdevPtr>& insstdev_messages);
      void GetNovatelCorrectedImuData(std::vector<novatel_gps_msgs::NovatelCorrectedImuDataPtr>& imu_messages);
      void GetNovatelPositions(std::vector<novatel_gps_msgs::NovatelPositionPtr>& positions);
      void GetNovatelXYZPositions(std::vector<novatel_gps_msgs::NovatelXYZPtr>& positions);
      void GetNovatelUtmPositions(std::vector<novatel_gps_msgs::NovatelUtmPositionPtr>& utm_positions);
      void GetNovatelVelocities(std::vector<novatel_gps_msgs::NovatelVelocityPtr>& velocities);
      void GetRangeMessages(std::vector<novatel_gps_msgs::RangePtr>& range_messages);
      void GetTimeMessages(std::vector<novatel_gps_msgs::TimePtr>& time_messages);
      void GetTrackstatMessages(std::vector<novatel_gps_msgs::TrackstatPtr>& trackstat_msgs);
      void GetClockSteeringMessages(std::vector<novatel_gps_msgs::ClockSteeringPtr>& 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);

      void GenerateImuMessages();

      NovatelGps::ReadResult ParseBinaryMessage(const BinaryMessage& msg,
                                                const ros::Time& stamp) noexcept(false);
      NovatelGps::ReadResult ParseNmeaSentence(const NmeaSentence& sentence,
                                               const ros::Time& stamp,
                                               double most_recent_utc_time) noexcept(false);
      NovatelGps::ReadResult ParseNovatelSentence(const NovatelSentence& sentence,
                                                  const ros::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;

      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_;
      boost::shared_ptr<boost::asio::ip::udp::socket> udp_socket_;
      boost::shared_ptr<boost::asio::ip::udp::endpoint> udp_endpoint_;

      // Data buffers
      std::vector<uint8_t> data_buffer_;
      std::string nmea_buffer_;
      boost::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_msgs::ClockSteeringPtr> clocksteering_msgs_;
      boost::circular_buffer<novatel_gps_msgs::NovatelCorrectedImuDataPtr> corrimudata_msgs_;
      boost::circular_buffer<novatel_gps_msgs::GpggaPtr> gpgga_msgs_;
      boost::circular_buffer<novatel_gps_msgs::GpgsaPtr> gpgsa_msgs_;
      boost::circular_buffer<novatel_gps_msgs::GpgsvPtr> gpgsv_msgs_;
      boost::circular_buffer<novatel_gps_msgs::GphdtPtr> gphdt_msgs_;
      boost::circular_buffer<novatel_gps_msgs::GprmcPtr> gprmc_msgs_;
      boost::circular_buffer<sensor_msgs::ImuPtr> imu_msgs_;
      boost::circular_buffer<novatel_gps_msgs::InscovPtr> inscov_msgs_;
      boost::circular_buffer<novatel_gps_msgs::InspvaPtr> inspva_msgs_;
      boost::circular_buffer<novatel_gps_msgs::InspvaxPtr> inspvax_msgs_;
      boost::circular_buffer<novatel_gps_msgs::InsstdevPtr> insstdev_msgs_;
      boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> novatel_positions_;
      boost::circular_buffer<novatel_gps_msgs::NovatelXYZPtr> novatel_xyz_positions_;
      boost::circular_buffer<novatel_gps_msgs::NovatelUtmPositionPtr> novatel_utm_positions_;
      boost::circular_buffer<novatel_gps_msgs::NovatelVelocityPtr> novatel_velocities_;
      boost::circular_buffer<novatel_gps_msgs::NovatelPositionPtr> bestpos_sync_buffer_;
      boost::circular_buffer<novatel_gps_msgs::NovatelVelocityPtr> bestvel_sync_buffer_;
      boost::circular_buffer<novatel_gps_msgs::NovatelHeading2Ptr> heading2_msgs_;
      boost::circular_buffer<novatel_gps_msgs::NovatelDualAntennaHeadingPtr> dual_antenna_heading_msgs_;
      boost::circular_buffer<novatel_gps_msgs::NovatelPsrdop2Ptr> psrdop2_msgs_;
      boost::circular_buffer<novatel_gps_msgs::RangePtr> range_msgs_;
      boost::circular_buffer<novatel_gps_msgs::TimePtr> time_msgs_;
      boost::circular_buffer<novatel_gps_msgs::TrackstatPtr> trackstat_msgs_;

      novatel_gps_msgs::NovatelPsrdop2Ptr latest_psrdop2_;

      // IMU data synchronization queues
      std::queue<novatel_gps_msgs::NovatelCorrectedImuDataPtr> corrimudata_queue_;
      std::queue<novatel_gps_msgs::InspvaPtr> inspva_queue_;
      novatel_gps_msgs::InsstdevPtr latest_insstdev_;
      novatel_gps_msgs::InscovPtr latest_inscov_;
      double imu_rate_;

      // Additional Options
      bool apply_vehicle_body_rotation_;
  };
}

#endif  // NOVATEL_OEM628_NOVATEL_GPS_H_