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>
// Other U-Blox package includes
#include <ublox_msgs/ublox_msgs.h>
// Ublox GPS includes
#include <ublox_gps/gps.h>
#include <ublox_gps/utils.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 {

const static uint32_t kROSQueueSize = 1; 
const static uint16_t kDefaultMeasPeriod = 250; 
const static uint32_t kSubscribeRate = 1; 
const 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;

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>
bool 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, name);
  }
}

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 = 0;
  U max = ~0;
  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 = 0;
  U max = ~0;
  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 = 1 << (sizeof(I) * 8 - 1);
  I max = (I)~(min);
  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 = 1 << (sizeof(I) * 8 - 1);
  I max = (I)~(min);
  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:
  const static double kResetWait = 10.0; 
  const static double kPollDuration = 1.0; 
  // Constants used for diagnostic frequency updater
  const static float kDiagnosticPeriod = 0.2; 
  const static double kFixFreqTol = 0.15; 
  const static double kFixFreqWindow = 10; 
  const 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_; 
  // 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_;
};

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
      fix.header.stamp.sec = toUtcSeconds(m);
      fix.header.stamp.nsec = 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 save_on_shutdown_; 
  bool clear_bbr_; 
};

class RawDataProduct: public virtual ComponentInterface {
 public:
  static const double kRtcmFreqTol = 0.15;
  static const 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_; 
};

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() {}

  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
  const static double kRtcmFreqMin = 1; 
  const static double kRtcmFreqMax = 10; 
  const static double kRtcmFreqTol = 0.1;
  const 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() {
    ROS_WARN("Functionality specific to u-blox TIM devices is only %s",
        "partially implemented. See TimProduct class in node.h & node.cpp.");
  }

  bool configureUblox() {}

  void subscribe();

  void initializeRosDiagnostics() {}
};

}

#endif