node.h

Go to the documentation of this file.

//==============================================================================
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
// 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 the Flight Systems and Automatic Control group,
//       TU Darmstadt, 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 THE COPYRIGHT HOLDER 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 UBLOX_GPS_NODE_H
#define UBLOX_GPS_NODE_H

// STL
#include <vector>
#include <set>
// Boost
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>
// ROS includes
#include <ros/ros.h>
#include <ros/console.h>
#include <ros/serialization.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
// ROS messages
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/TimeReference.h>
#include <sensor_msgs/Imu.h>
// Other U-Blox package includes
#include <ublox_msgs/ublox_msgs.h>
// Ublox GPS includes
#include <ublox_gps/gps.h>
#include <ublox_gps/utils.h>
#include <ublox_gps/raw_data_pa.h>

// This file declares the ComponentInterface which acts as a high level
// interface for u-blox firmware, product categories, etc. It contains methods
// to configure the u-blox and subscribe to u-blox messages.
//
// This file also declares UbloxNode which implements ComponentInterface and is
// the main class and ros node. it implements functionality which applies to
// any u-blox device, regardless of the firmware version or product type.
// The class is designed in compositional style; it contains ComponentInterfaces
// which implement features specific to the device based on its firmware version
// and product category. UbloxNode calls the public methods of each component.
//
// This file declares UbloxFirmware is an abstract class which implements
// ComponentInterface and functions generic to all firmware (such as the
// initializing the fix diagnostics). Subclasses of UbloxFirmware for firmware
// versions 6-8 are also declared in this file.
//
// Lastly, this file declares classes for each product category which also
// implement u-blox interface, currently only the class for High Precision
// GNSS devices has been fully implemented and tested.

namespace ublox_node {

constexpr static uint32_t kROSQueueSize = 1;
constexpr static uint16_t kDefaultMeasPeriod = 250;
constexpr static uint32_t kSubscribeRate = 1;
constexpr static uint32_t kNavSvInfoSubscribeRate = 20;

// ROS objects
boost::shared_ptr<diagnostic_updater::Updater> updater;
boost::shared_ptr<ros::NodeHandle> nh;

ublox_gps::Gps gps;
std::set<std::string> supported;

std::map<std::string, bool> enabled;
std::string frame_id;
int fix_status_service;
uint16_t meas_rate;
uint16_t nav_rate;
std::vector<uint8_t> rtcm_ids;
std::vector<uint8_t> rtcm_rates;
bool config_on_startup_flag_;


struct UbloxTopicDiagnostic {
  UbloxTopicDiagnostic() {}

  UbloxTopicDiagnostic (std::string topic, double freq_tol, int freq_window) {
    const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz
    min_freq = target_freq;
    max_freq = target_freq;
    diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq,
                                                        freq_tol, freq_window);
    diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic,
                                                                   *updater,
                                                                   freq_param);
  }

  UbloxTopicDiagnostic (std::string topic, double freq_min, double freq_max,
                   double freq_tol, int freq_window) {
    min_freq = freq_min;
    max_freq = freq_max;
    diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq,
                                                        freq_tol, freq_window);
    diagnostic = new diagnostic_updater::HeaderlessTopicDiagnostic(topic,
                                                                   *updater,
                                                                   freq_param);
  }

  diagnostic_updater::HeaderlessTopicDiagnostic *diagnostic;
  double min_freq;
  double max_freq;
};

struct FixDiagnostic {
  FixDiagnostic() {}

  FixDiagnostic (std::string name, double freq_tol, int freq_window,
                 double stamp_min) {
    const double target_freq = 1.0 / (meas_rate * 1e-3 * nav_rate); // Hz
    min_freq = target_freq;
    max_freq = target_freq;
    diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq,
                                                        freq_tol, freq_window);
    double stamp_max = meas_rate * 1e-3 * (1 + freq_tol);
    diagnostic_updater::TimeStampStatusParam time_param(stamp_min, stamp_max);
    diagnostic = new diagnostic_updater::TopicDiagnostic(name,
                                                         *updater,
                                                         freq_param,
                                                         time_param);
  }

  diagnostic_updater::TopicDiagnostic *diagnostic;
  double min_freq;
  double max_freq;
};

FixDiagnostic freq_diag;

uint8_t modelFromString(const std::string& model);

uint8_t fixModeFromString(const std::string& mode);

template <typename V, typename T>
void checkMin(V val, T min, std::string name) {
  if(val < min) {
    std::stringstream oss;
    oss << "Invalid settings: " << name << " must be > " << min;
    throw std::runtime_error(oss.str());
  }
}

template <typename V, typename T>
void checkRange(V val, T min, T max, std::string name) {
  if(val < min || val > max) {
    std::stringstream oss;
    oss << "Invalid settings: " << name << " must be in range [" << min <<
        ", " << max << "].";
    throw std::runtime_error(oss.str());
  }
}

template <typename V, typename T>
void checkRange(std::vector<V> val, T min, T max, std::string name) {
  for(size_t i = 0; i < val.size(); i++)  {
    std::stringstream oss;
    oss << name << "[" << i << "]";
    checkRange(val[i], min, max, oss.str());
  }
}

template <typename U>
bool getRosUint(const std::string& key, U &u) {
  int param;
  if (!nh->getParam(key, param)) return false;
  // Check the bounds
  U min = std::numeric_limits<U>::lowest();
  U max = std::numeric_limits<U>::max();
  checkRange(param, min, max, key);
  // set the output
  u = (U) param;
  return true;
}

template <typename U, typename V>
void getRosUint(const std::string& key, U &u, V default_val) {
  if(!getRosUint(key, u))
    u = default_val;
}

template <typename U>
bool getRosUint(const std::string& key, std::vector<U> &u) {
  std::vector<int> param;
  if (!nh->getParam(key, param)) return false;

  // Check the bounds
  U min = std::numeric_limits<U>::lowest();
  U max = std::numeric_limits<U>::max();
  checkRange(param, min, max, key);

  // set the output
  u.insert(u.begin(), param.begin(), param.end());
  return true;
}

template <typename I>
bool getRosInt(const std::string& key, I &u) {
  int param;
  if (!nh->getParam(key, param)) return false;
  // Check the bounds
  I min = std::numeric_limits<I>::lowest();
  I max = std::numeric_limits<I>::max();
  checkRange(param, min, max, key);
  // set the output
  u = (I) param;
  return true;
}

template <typename U, typename V>
void getRosInt(const std::string& key, U &u, V default_val) {
  if(!getRosInt(key, u))
    u = default_val;
}

template <typename I>
bool getRosInt(const std::string& key, std::vector<I> &i) {
  std::vector<int> param;
  if (!nh->getParam(key, param)) return false;

  // Check the bounds
  I min = std::numeric_limits<I>::lowest();
  I max = std::numeric_limits<I>::max();
  checkRange(param, min, max, key);

  // set the output
  i.insert(i.begin(), param.begin(), param.end());
  return true;
}

template <typename MessageT>
void publish(const MessageT& m, const std::string& topic) {
  static ros::Publisher publisher = nh->advertise<MessageT>(topic,
                                                            kROSQueueSize);
  publisher.publish(m);
}

bool supportsGnss(std::string gnss) {
  return supported.count(gnss) > 0;
}

class ComponentInterface {
 public:
  virtual void getRosParams() = 0;

  virtual bool configureUblox() = 0;

  virtual void initializeRosDiagnostics() = 0;

  virtual void subscribe() = 0;
};

typedef boost::shared_ptr<ComponentInterface> ComponentPtr;

class UbloxNode : public virtual ComponentInterface {
 public:
  constexpr static int kResetWait = 10;
  constexpr static double kPollDuration = 1.0;
  // Constants used for diagnostic frequency updater
  constexpr static float kDiagnosticPeriod = 0.2;
  constexpr static double kFixFreqTol = 0.15;
  constexpr static double kFixFreqWindow = 10;
  constexpr static double kTimeStampStatusMin = 0;

  UbloxNode();

  void getRosParams();

  bool configureUblox();

  void subscribe();

  void initializeRosDiagnostics();

  void printInf(const ublox_msgs::Inf &m, uint8_t id);

 private:

  void initializeIo();

  void initialize();

  void shutdown();

  bool resetDevice();

  void processMonVer();

  void addFirmwareInterface();

  void addProductInterface(std::string product_category,
                           std::string ref_rov = "");

  void pollMessages(const ros::TimerEvent& event);

  void configureInf();


  std::vector<boost::shared_ptr<ComponentInterface> > components_;

  float protocol_version_ = 0;
  // Variables set from parameter server
  std::string device_;
  std::string dynamic_model_;
  std::string fix_mode_;
  uint8_t dmodel_;
  uint8_t fmode_;
  uint32_t baudrate_;
  uint16_t uart_in_;
  uint16_t uart_out_;
  uint16_t usb_tx_;

  bool set_usb_;
  uint16_t usb_in_;
  uint16_t usb_out_ ;
  double rate_;
  bool set_dat_;
  ublox_msgs::CfgDAT cfg_dat_;
  bool enable_sbas_;
  bool enable_ppp_;
  uint8_t sbas_usage_;
  uint8_t max_sbas_;
  uint8_t dr_limit_;
  ublox_msgs::CfgCFG load_;
  ublox_msgs::CfgCFG save_;
  uint8_t tim_rate_;

  RawDataStreamPa rawDataStreamPa_;
};

class UbloxFirmware : public virtual ComponentInterface {
 public:
  void initializeRosDiagnostics();

 protected:
  virtual void fixDiagnostic(
      diagnostic_updater::DiagnosticStatusWrapper& stat) = 0;
};

class UbloxFirmware6 : public UbloxFirmware {
 public:
  UbloxFirmware6();

  void getRosParams();

  bool configureUblox();

  void subscribe();

 protected:
  void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);

 private:
  void callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m);

  void callbackNavVelNed(const ublox_msgs::NavVELNED& m);

  void callbackNavSol(const ublox_msgs::NavSOL& m);

  ublox_msgs::NavPOSLLH last_nav_pos_;
  ublox_msgs::NavVELNED last_nav_vel_;
  ublox_msgs::NavSOL last_nav_sol_;
  sensor_msgs::NavSatFix fix_;
  geometry_msgs::TwistWithCovarianceStamped velocity_;

  ublox_msgs::CfgNMEA6 cfg_nmea_;
  bool set_nmea_;
};

template<typename NavPVT>
class UbloxFirmware7Plus : public UbloxFirmware {
 public:
  void callbackNavPvt(const NavPVT& m) {
    if(enabled["nav_pvt"]) {
      // NavPVT publisher
      static ros::Publisher publisher = nh->advertise<NavPVT>("navpvt",
                                                              kROSQueueSize);
      publisher.publish(m);
    }

    //
    // NavSatFix message
    //
    static ros::Publisher fixPublisher =
        nh->advertise<sensor_msgs::NavSatFix>("fix", kROSQueueSize);

    sensor_msgs::NavSatFix fix;
    fix.header.frame_id = frame_id;
    // set the timestamp
    uint8_t valid_time = m.VALID_DATE | m.VALID_TIME | m.VALID_FULLY_RESOLVED;
    if (((m.valid & valid_time) == valid_time) &&
        (m.flags2 & m.FLAGS2_CONFIRMED_AVAILABLE)) {
      // Use NavPVT timestamp since it is valid
      // The time in nanoseconds from the NavPVT message can be between -1e9 and 1e9
      //  The ros time uses only unsigned values, so a negative nano seconds must be
      //  converted to a positive value
      if (m.nano < 0) {
        fix.header.stamp.sec = toUtcSeconds(m) - 1;
        fix.header.stamp.nsec = (uint32_t)(m.nano + 1e9);
      }
      else {
        fix.header.stamp.sec = toUtcSeconds(m);
        fix.header.stamp.nsec = (uint32_t)(m.nano);
      }
    } else {
      // Use ROS time since NavPVT timestamp is not valid
      fix.header.stamp = ros::Time::now();
    }
    // Set the LLA
    fix.latitude = m.lat * 1e-7; // to deg
    fix.longitude = m.lon * 1e-7; // to deg
    fix.altitude = m.height * 1e-3; // to [m]
    // Set the Fix status
    bool fixOk = m.flags & m.FLAGS_GNSS_FIX_OK;
    if (fixOk && m.fixType >= m.FIX_TYPE_2D) {
      fix.status.status = fix.status.STATUS_FIX;
      if(m.flags & m.CARRIER_PHASE_FIXED)
        fix.status.status = fix.status.STATUS_GBAS_FIX;
    }
    else {
      fix.status.status = fix.status.STATUS_NO_FIX;
    }
    // Set the service based on GNSS configuration
    fix.status.service = fix_status_service;

    // Set the position covariance
    const double varH = pow(m.hAcc / 1000.0, 2); // to [m^2]
    const double varV = pow(m.vAcc / 1000.0, 2); // to [m^2]
    fix.position_covariance[0] = varH;
    fix.position_covariance[4] = varH;
    fix.position_covariance[8] = varV;
    fix.position_covariance_type =
        sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;

    fixPublisher.publish(fix);

    //
    // Twist message
    //
    static ros::Publisher velocityPublisher =
        nh->advertise<geometry_msgs::TwistWithCovarianceStamped>("fix_velocity",
                                                                 kROSQueueSize);
    geometry_msgs::TwistWithCovarianceStamped velocity;
    velocity.header.stamp = fix.header.stamp;
    velocity.header.frame_id = frame_id;

    // convert to XYZ linear velocity [m/s] in ENU
    velocity.twist.twist.linear.x = m.velE * 1e-3;
    velocity.twist.twist.linear.y = m.velN * 1e-3;
    velocity.twist.twist.linear.z = -m.velD * 1e-3;
    // Set the covariance
    const double covSpeed = pow(m.sAcc * 1e-3, 2);
    const int cols = 6;
    velocity.twist.covariance[cols * 0 + 0] = covSpeed;
    velocity.twist.covariance[cols * 1 + 1] = covSpeed;
    velocity.twist.covariance[cols * 2 + 2] = covSpeed;
    velocity.twist.covariance[cols * 3 + 3] = -1;  //  angular rate unsupported

    velocityPublisher.publish(velocity);

    //
    // Update diagnostics
    //
    last_nav_pvt_ = m;
    freq_diag.diagnostic->tick(fix.header.stamp);
    updater->update();
  }

 protected:

  void fixDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) {
    // check the last message, convert to diagnostic
    if (last_nav_pvt_.fixType ==
        ublox_msgs::NavPVT::FIX_TYPE_DEAD_RECKONING_ONLY) {
      stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
      stat.message = "Dead reckoning only";
    } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_2D) {
      stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
      stat.message = "2D fix";
    } else if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_3D) {
      stat.level = diagnostic_msgs::DiagnosticStatus::OK;
      stat.message = "3D fix";
    } else if (last_nav_pvt_.fixType ==
               ublox_msgs::NavPVT::FIX_TYPE_GNSS_DEAD_RECKONING_COMBINED) {
      stat.level = diagnostic_msgs::DiagnosticStatus::OK;
      stat.message = "GPS and dead reckoning combined";
    } else if (last_nav_pvt_.fixType ==
               ublox_msgs::NavPVT::FIX_TYPE_TIME_ONLY) {
      stat.level = diagnostic_msgs::DiagnosticStatus::OK;
      stat.message = "Time only fix";
    }

    // If fix not ok (w/in DOP & Accuracy Masks), raise the diagnostic level
    if (!(last_nav_pvt_.flags & ublox_msgs::NavPVT::FLAGS_GNSS_FIX_OK)) {
      stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
      stat.message += ", fix not ok";
    }
    // Raise diagnostic level to error if no fix
    if (last_nav_pvt_.fixType == ublox_msgs::NavPVT::FIX_TYPE_NO_FIX) {
      stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
      stat.message = "No fix";
    }

    // append last fix position
    stat.add("iTOW [ms]", last_nav_pvt_.iTOW);
    stat.add("Latitude [deg]", last_nav_pvt_.lat * 1e-7);
    stat.add("Longitude [deg]", last_nav_pvt_.lon * 1e-7);
    stat.add("Altitude [m]", last_nav_pvt_.height * 1e-3);
    stat.add("Height above MSL [m]", last_nav_pvt_.hMSL * 1e-3);
    stat.add("Horizontal Accuracy [m]", last_nav_pvt_.hAcc * 1e-3);
    stat.add("Vertical Accuracy [m]", last_nav_pvt_.vAcc * 1e-3);
    stat.add("# SVs used", (int)last_nav_pvt_.numSV);
  }

  NavPVT last_nav_pvt_;
  // Whether or not to enable the given GNSS
  bool enable_gps_;
  bool enable_glonass_;
  bool enable_qzss_;
  bool enable_sbas_;
  uint32_t qzss_sig_cfg_;
};

class UbloxFirmware7 : public UbloxFirmware7Plus<ublox_msgs::NavPVT7> {
 public:
  UbloxFirmware7();

  void getRosParams();

  bool configureUblox();

  void subscribe();

  private:

    ublox_msgs::CfgNMEA7 cfg_nmea_;
    bool set_nmea_;
};

class UbloxFirmware8 : public UbloxFirmware7Plus<ublox_msgs::NavPVT> {
 public:
  UbloxFirmware8();

  void getRosParams();

  bool configureUblox();

  void subscribe();

 private:
  // Set from ROS parameters
  bool enable_galileo_;
  bool enable_beidou_;
  bool enable_imes_;
  bool set_nmea_;
  ublox_msgs::CfgNMEA cfg_nmea_;
  bool clear_bbr_;
};

class RawDataProduct: public virtual ComponentInterface {
 public:
  static constexpr double kRtcmFreqTol = 0.15;
  static constexpr int kRtcmFreqWindow = 25;

  void getRosParams() {}

  bool configureUblox() { return true; }

  void subscribe();

  void initializeRosDiagnostics();

 private:
  std::vector<UbloxTopicDiagnostic> freq_diagnostics_;
};

class AdrUdrProduct: public virtual ComponentInterface {
 public:
  void getRosParams();

  bool configureUblox();

  void subscribe();

  void initializeRosDiagnostics() {
    ROS_WARN("ROS Diagnostics specific to u-blox ADR/UDR devices is %s",
             "unimplemented. See AdrUdrProduct class in node.h & node.cpp.");
  }

 protected:
  bool use_adr_;

   
  sensor_msgs::Imu imu_;
  sensor_msgs::TimeReference t_ref_;
  ublox_msgs::TimTM2 timtm2;

  void callbackEsfMEAS(const ublox_msgs::EsfMEAS &m);
};

class FtsProduct: public virtual ComponentInterface {
  void getRosParams() {
    ROS_WARN("Functionality specific to u-blox FTS devices is %s",
             "unimplemented. See FtsProduct class in node.h & node.cpp.");
  }

  bool configureUblox() { return false; }

  void subscribe() {}

  void initializeRosDiagnostics() {}
};

class HpgRefProduct: public virtual ComponentInterface {
 public:
  void getRosParams();

  bool configureUblox();

  void subscribe();

  void initializeRosDiagnostics();

  void callbackNavSvIn(ublox_msgs::NavSVIN m);

 protected:
  void tmode3Diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);

  bool setTimeMode();

  ublox_msgs::NavSVIN last_nav_svin_;

  uint8_t tmode3_;

  // TMODE3 = Fixed mode settings

  bool lla_flag_;

  std::vector<float> arp_position_;

  std::vector<int8_t> arp_position_hp_;

  float fixed_pos_acc_;

  // Settings for TMODE3 = Survey-in

  bool svin_reset_;

  uint32_t sv_in_min_dur_;

  float sv_in_acc_lim_;

  enum {
    INIT, 
    FIXED, 
    DISABLED, 
    SURVEY_IN, 
    TIME 
  } mode_;
};

class HpgRovProduct: public virtual ComponentInterface {
 public:
  // Constants for diagnostic updater
  constexpr static double kRtcmFreqMin = 1;
  constexpr static double kRtcmFreqMax = 10;
  constexpr static double kRtcmFreqTol = 0.1;
  constexpr static int kRtcmFreqWindow = 25;
  void getRosParams();

  bool configureUblox();

  void subscribe();

  void initializeRosDiagnostics();

 protected:
  void carrierPhaseDiagnostics(
      diagnostic_updater::DiagnosticStatusWrapper& stat);

  void callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m);


  ublox_msgs::NavRELPOSNED last_rel_pos_;


  uint8_t dgnss_mode_;

  UbloxTopicDiagnostic freq_rtcm_;
};

class TimProduct: public virtual ComponentInterface {
  void getRosParams(); 
 
  bool configureUblox(); 

  void subscribe();

  void initializeRosDiagnostics();

 protected:  
  void callbackTimTM2(const ublox_msgs::TimTM2 &m);
 
  sensor_msgs::TimeReference t_ref_;
};

}

#endif