Program Listing for File novatel_gps.h
↰ Return to documentation for file (/tmp/ws/src/novatel_gps_driver/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h)
// *****************************************************************************
//
// 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_