Program Listing for File novatel_gps_node.h

↰ Return to documentation for file (/tmp/ws/src/novatel_gps_driver/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.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_NODE_H
#define NOVATEL_GPS_DRIVER_NOVATEL_GPS_NODE_H

#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <diagnostic_updater/diagnostic_updater.hpp>

#include <novatel_gps_driver/novatel_gps.h>

#include <novatel_gps_msgs/msg/clock_steering.hpp>
#include <novatel_gps_msgs/srv/novatel_freset.hpp>

#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/time_reference.hpp>

#include <swri_roscpp/subscriber.h>

#include <rclcpp/rclcpp.hpp>

#include <chrono>
#include <string>

using TimeParserMsgT = novatel_gps_driver::TimeParser::MessageType;

namespace novatel_gps_driver
{
  class NovatelGpsNode : public rclcpp::Node
  {
  public:
    explicit NovatelGpsNode(const rclcpp::NodeOptions& options);

    ~NovatelGpsNode() override;

    void SyncCallback(const builtin_interfaces::msg::Time::ConstSharedPtr& sync);

    void Spin();

  private:
    // Parameters
    std::string device_;
    std::string connection_type_;
    int32_t serial_baud_;
    double polling_period_;
    bool publish_gpgsa_;
    bool publish_gpgsv_;
    bool publish_gphdt_;
    double imu_rate_;
    double imu_sample_rate_;
    bool span_frame_to_ros_frame_;
    bool publish_clock_steering_;
    bool publish_imu_messages_;
    bool publish_novatel_positions_;
    bool publish_novatel_xyz_positions_;
    bool publish_novatel_utm_positions_;
    bool publish_novatel_velocity_;
    bool publish_novatel_heading2_;
    bool publish_novatel_dual_antenna_heading_;
    bool publish_novatel_psrdop2_;
    bool publish_nmea_messages_;
    bool publish_range_messages_;
    bool publish_time_messages_;
    bool publish_time_reference_;
    bool publish_trackstat_;
    bool publish_diagnostics_;
    bool publish_sync_diagnostic_;
    bool publish_invalid_gpsfix_;
    double reconnect_delay_s_;
    bool use_binary_messages_;

    rclcpp::Publisher<novatel_gps_msgs::msg::ClockSteering>::SharedPtr clocksteering_pub_;
    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr fix_pub_;
    rclcpp::Publisher<gps_msgs::msg::GPSFix>::SharedPtr gps_pub_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Inscov>::SharedPtr inscov_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Inspva>::SharedPtr inspva_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Inspvax>::SharedPtr inspvax_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Insstdev>::SharedPtr insstdev_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelCorrectedImuData>::SharedPtr novatel_imu_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelPosition>::SharedPtr novatel_position_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelXYZ>::SharedPtr novatel_xyz_position_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelUtmPosition>::SharedPtr novatel_utm_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelVelocity>::SharedPtr novatel_velocity_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelHeading2>::SharedPtr novatel_heading2_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelDualAntennaHeading>::SharedPtr novatel_dual_antenna_heading_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::NovatelPsrdop2>::SharedPtr novatel_psrdop2_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Gpgga>::SharedPtr gpgga_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Gpgsv>::SharedPtr gpgsv_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Gpgsa>::SharedPtr gpgsa_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Gphdt>::SharedPtr gphdt_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Gprmc>::SharedPtr gprmc_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Range>::SharedPtr range_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Time>::SharedPtr time_pub_;
    rclcpp::Publisher<sensor_msgs::msg::TimeReference>::SharedPtr time_ref_pub_;
    rclcpp::Publisher<novatel_gps_msgs::msg::Trackstat>::SharedPtr trackstat_pub_;

    rclcpp::Service<novatel_gps_msgs::srv::NovatelFRESET>::SharedPtr reset_service_;

    NovatelGps::ConnectionType connection_;
    NovatelGps gps_;

    boost::thread thread_;
    boost::mutex mutex_;

    swri::Subscriber sync_sub_;
    rclcpp::Time last_sync_;
    boost::circular_buffer<rclcpp::Time> sync_times_;
    boost::circular_buffer<rclcpp::Time> msg_times_;
    boost::accumulators::accumulator_set<float, boost::accumulators::stats<
        boost::accumulators::tag::max,
        boost::accumulators::tag::min,
        boost::accumulators::tag::mean,
        boost::accumulators::tag::variance> > offset_stats_;
    boost::accumulators::accumulator_set<float, boost::accumulators::stats<boost::accumulators::tag::rolling_mean> > rolling_offset_;

    // ROS diagnostics
    std::string error_msg_;
    diagnostic_updater::Updater diagnostic_updater_;
    std::string hw_id_;
    double expected_rate_;
    int32_t device_timeouts_;
    int32_t device_interrupts_;
    int32_t device_errors_;
    int32_t gps_parse_failures_;
    int32_t gps_insufficient_data_warnings_;
    int32_t publish_rate_warnings_;
    int32_t measurement_count_;
    rclcpp::Time last_published_;
    novatel_gps_msgs::msg::NovatelPosition::SharedPtr last_novatel_position_;

    std::string imu_frame_id_;
    std::string frame_id_;

    bool resetService(std::shared_ptr<rmw_request_id_t> request_header,
                      novatel_gps_msgs::srv::NovatelFRESET::Request::SharedPtr req,
                      novatel_gps_msgs::srv::NovatelFRESET::Response::SharedPtr res);

    void CheckDeviceForData();

    sensor_msgs::msg::NavSatFix::UniquePtr ConvertGpsFixToNavSatFix(const gps_msgs::msg::GPSFix::UniquePtr& msg);

    void CalculateTimeSync();

    void FixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);

    void SyncDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);

    void DeviceDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);

    void GpsDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);

    void DataDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);

    void RateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status);

    rclcpp::Time NovatelTimeToLocalTime(const TimeParserMsgT & utc_time);
  };
}

#endif //NOVATEL_GPS_DRIVER_NOVATEL_GPS_NODE_H