Beispiel #1
0
int main(int argc, char **argv) {
  boost::asio::io_service io_service;
  ros::Timer poller;
  boost::shared_ptr<boost::asio::ip::tcp::socket> tcp_handle;
  boost::shared_ptr<boost::asio::serial_port> serial_handle;
  bool setup_ok = true;
  
  ros::init(argc, argv, "ublox_gps");
  nh.reset(new ros::NodeHandle("~"));
  if (!nh->hasParam("diagnostic_period")) {
    nh->setParam("diagnostic_period", 0.2); //  5Hz diagnostic period 
  }
  updater.reset(new diagnostic_updater::Updater());
  updater->setHardwareID("ublox");
  
  std::string device;
  int baudrate;
  int rate, meas_rate;
  bool enable_sbas, enable_glonass, enable_beidou, enable_ppp;
  std::string dynamic_model, fix_mode;
  int dr_limit;
  int ublox_version;
  ros::NodeHandle param("~");
  param.param("device", device, std::string("/dev/ttyUSB0"));
  param.param("frame_id", frame_id, std::string("gps"));
  param.param("baudrate", baudrate, 9600);
  param.param("rate", rate, 4); //  in Hz
  param.param("enable_sbas", enable_sbas, false);
  param.param("enable_glonass", enable_glonass, false);
  param.param("enable_beidou", enable_beidou, false);
  param.param("enable_ppp", enable_ppp, false);
  param.param("dynamic_model", dynamic_model, std::string("portable"));
  param.param("fix_mode", fix_mode, std::string("both"));
  param.param("dr_limit", dr_limit, 0);
  param.param("ublox_version", ublox_version, 6); /// @todo: temporary workaround
    
  if (enable_ppp) {
    ROS_WARN("Warning: PPP is enabled - this is an expert setting.");
  }
  
  if (rate <= 0) {
    ROS_ERROR("Invalid settings: rate must be > 0");
    return 1;
  }
  //  measurement rate param for ublox, units of ms
  meas_rate = 1000 / rate;
  
  if (dr_limit < 0 || dr_limit > 255) {
    ROS_ERROR("Invalid settings: dr_limit must be between 0 and 255");
    return 1;
  }
  
  DynamicModel dmodel;
  FixMode fmode;
  try {
    dmodel = ublox_gps::modelFromString(dynamic_model);
    fmode = ublox_gps::fixModeFromString(fix_mode);
  } catch (std::exception& e) {
    ROS_ERROR("Invalid settings: %s", e.what());
    return 1;
  }
  
  //  configure diagnostic updater for frequency
  updater->add("fix", &fix_diagnostic);
  updater->force_update();
  
  const double target_freq = 1000.0 / meas_rate;  //  actual update frequency
  double min_freq = target_freq;
  double max_freq = target_freq;
  diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, 0.05, 10);
  diagnostic_updater::TimeStampStatusParam time_param(0,meas_rate * 1e-3 * 0.05);
  freq_diag.reset(new diagnostic_updater::TopicDiagnostic(std::string("fix"), 
                                                          *updater, 
                                                          freq_param,
                                                          time_param));
  
  boost::smatch match;
  if (boost::regex_match(device, match, boost::regex("(tcp|udp)://(.+):(\\d+)"))) {
    std::string proto(match[1]);
    std::string host(match[2]);
    std::string port(match[3]);
    ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str());

    if (proto == "tcp") {
      boost::asio::ip::tcp::resolver::iterator endpoint;

      try {
        boost::asio::ip::tcp::resolver resolver(io_service);
        endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port));
      } catch(std::runtime_error& e) {
        ROS_ERROR("Could not resolve %s:%s: %s", host.c_str(), port.c_str(), e.what());
        return 1; //  exit
      }

      boost::asio::ip::tcp::socket *socket = new boost::asio::ip::tcp::socket(io_service);
      tcp_handle.reset(socket);

      try {
        socket->connect(*endpoint);
      } catch(std::runtime_error& e) {
        ROS_ERROR("Could not connect to %s:%s: %s", endpoint->host_name().c_str(), endpoint->service_name().c_str(), e.what());
        return 1; //  exit
      }
    
      ROS_INFO("Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str());
      gps.initialize(*socket, io_service);
    } else {
      ROS_ERROR("Protocol '%s' is unsupported", proto.c_str());
      return 1; //  exit
    }
  } else {
    boost::asio::serial_port *serial = new boost::asio::serial_port(io_service);
    serial_handle.reset(serial);

    // open serial port
    try {
      serial->open(device);
    } catch (std::runtime_error& e) {
      ROS_ERROR("Could not open serial port %s: %s", device.c_str(), e.what());
      return 1; //  exit
    }

    ROS_INFO("Opened serial port %s", device.c_str());
    gps.setBaudrate(baudrate);
    gps.initialize(*serial, io_service);
  }

  //  apply all requested settings
  try {
    if (!gps.isInitialized()) {
      throw std::runtime_error("Failed to initialize.");
    }
    if (!gps.setMeasRate(meas_rate)) {
      std::stringstream ss;
      ss << "Failed to set measurement rate to " << meas_rate << "ms.";
      throw std::runtime_error(ss.str());
    }
    if (!gps.enableSBAS(enable_sbas)) {
      throw std::runtime_error(std::string("Failed to ") + 
                               ((enable_sbas) ? "enable" : "disable") + " SBAS.");
    }
    if (!gps.setPPPEnabled(enable_ppp)) {
      throw std::runtime_error(std::string("Failed to ") +
                               ((enable_ppp) ? "enable" : "disable") + " PPP.");
    }
    if (!gps.setDynamicModel(dmodel)) {
      throw std::runtime_error("Failed to set model: " + 
                               dynamic_model + ".");
    }
    if (!gps.setFixMode(fmode)) {
      throw std::runtime_error("Failed to set fix mode: " + fix_mode + ".");
    }
    if (!gps.setDeadReckonLimit(dr_limit)) {
      std::stringstream ss;
      ss << "Failed to set dead reckoning limit: " << dr_limit << ".";
      throw std::runtime_error(ss.str());
    }
    
    if (ublox_version >= 7) {
      ublox_msgs::CfgGNSS cfgGNSS;
      if (gps.poll(cfgGNSS)) {
        ROS_INFO("Read GNSS config.");
        ROS_INFO("Num. tracking channels in hardware: %i",cfgGNSS.numTrkChHw);
        ROS_INFO("Num. tracking channels to use: %i",cfgGNSS.numTrkChUse);
      } else {
        throw std::runtime_error("Failed to read the GNSS config.");
      }
      
      cfgGNSS.numConfigBlocks = 1;  //  do services one by one
      cfgGNSS.msgVer = 0;
      cfgGNSS.resTrkCh = 8;   //  taken as defaults from ublox manual
      cfgGNSS.maxTrkCh = 16;
      
      //  configure glonass
      cfgGNSS.gnssId = ublox_msgs::CfgGNSS::GNSS_ID_GLONASS;
      cfgGNSS.flags = enable_glonass;
      if (!gps.configure(cfgGNSS)) {
        throw std::runtime_error(std::string("Failed to ") +
                                 ((enable_glonass) ? "enable" : "disable") + " GLONASS.");
      }
      if (ublox_version >= 8) {
        //  configure beidou
        cfgGNSS.gnssId = ublox_msgs::CfgGNSS::GNSS_ID_BEIDOU;
        cfgGNSS.flags = enable_beidou;
        if (!gps.configure(cfgGNSS)) {
          throw std::runtime_error(std::string("Failed to ") +
                                   ((enable_beidou) ? "enable" : "disable") + " BeiDou.");
        }
      } else {
        ROS_WARN("ublox_version < 8, ignoring BeiDou Settings");
      }
    } else {
      ROS_WARN("ublox_version < 7, ignoring GNSS settings");
    }
  } catch (std::exception& e) {
    setup_ok = false;
    ROS_ERROR("Error configuring device: %s", e.what());
  }

  if (setup_ok) {
    ROS_INFO("U-Blox configured successfully.");
    
    // subscribe messages
    param.param("all", enabled["all"], false);
    param.param("rxm", enabled["rxm"], false);
    param.param("aid", enabled["aid"], false);
    
    param.param("nav_sol", enabled["nav_sol"], true);
    if (enabled["nav_sol"]) gps.subscribe<ublox_msgs::NavSOL>(boost::bind(&publish<ublox_msgs::NavSOL>, _1, "navsol"), 1);
    param.param("nav_status", enabled["nav_status"], true);
    if (enabled["nav_status"]) gps.subscribe<ublox_msgs::NavSTATUS>(&publishNavStatus, 1);
    param.param("nav_svinfo", enabled["nav_svinfo"], enabled["all"]);
    if (enabled["nav_svinfo"]) gps.subscribe<ublox_msgs::NavSVINFO>(&publishNavSVINFO, 20);
    param.param("nav_clk", enabled["nav_clk"], enabled["all"]);
    if (enabled["nav_clk"]) gps.subscribe<ublox_msgs::NavCLOCK>(&publishNavCLK, 1);
    param.param("rxm_raw", enabled["rxm_raw"], enabled["all"] || enabled["rxm"]);
    if (enabled["rxm_raw"]) gps.subscribe<ublox_msgs::RxmRAW>(&publishRxmRAW, 1);
    param.param("rxm_sfrb", enabled["rxm_sfrb"], enabled["all"] || enabled["rxm"]);
    if (enabled["rxm_sfrb"]) gps.subscribe<ublox_msgs::RxmSFRB>(&publishRxmSFRB, 1);
    param.param("nav_posllh", enabled["nav_posllh"], true);
    if (enabled["nav_posllh"]) gps.subscribe<ublox_msgs::NavPOSLLH>(&publishNavPosLLH, 1);
    param.param("nav_velned", enabled["nav_velned"], true);
    if (enabled["nav_velned"]) gps.subscribe<ublox_msgs::NavVELNED>(&publishNavVelNED, 1);
    param.param("aid_alm", enabled["aid_alm"], enabled["all"] || enabled["aid"]);
    if (enabled["aid_alm"]) gps.subscribe<ublox_msgs::AidALM>(&publishAidALM);
    param.param("aid_eph", enabled["aid_eph"], enabled["all"] || enabled["aid"]);
    if (enabled["aid_eph"]) gps.subscribe<ublox_msgs::AidEPH>(&publishAidEPH);
    param.param("aid_hui", enabled["aid_hui"], enabled["all"] || enabled["aid"]);
    if (enabled["aid_hui"]) gps.subscribe<ublox_msgs::AidHUI>(&publishAidHUI);
    
    poller = nh->createTimer(ros::Duration(1.0), &pollMessages);
    poller.start();
    ros::spin();
  }

  if (gps.isInitialized()) {
    gps.close();
    ROS_INFO("Closed connection to %s.", device.c_str());
  }
  return 0;
}
Beispiel #2
0
int main(int argc, char **argv) {
  boost::asio::io_service io_service;
  ros::Timer poller;
  boost::shared_ptr<boost::asio::ip::tcp::socket> tcp_handle;
  boost::shared_ptr<boost::asio::serial_port> serial_handle;
  bool setup_ok = true;
  
  ros::init(argc, argv, "ublox_gps");
  nh.reset(new ros::NodeHandle("~"));
  if (!nh->hasParam("diagnostic_period")) {
    nh->setParam("diagnostic_period", 0.2); //  5Hz diagnostic period 
  }
  updater.reset(new diagnostic_updater::Updater());
  updater->setHardwareID("ublox");
  
  std::string device;
  int baudrate;
  int meas_rate;
  bool enable_sbas;
  std::string dynamic_model, fix_mode;
  int dr_limit;

  ros::NodeHandle param("~");
  param.param("device", device, std::string("/dev/ttyUSB0"));
  param.param("frame_id", frame_id, std::string("gps"));
  param.param("baudrate", baudrate, 9600);
  param.param("meas_rate", meas_rate, 250);        //  default to 4Hz
  param.param("enable_sbas", enable_sbas, false);
  param.param("dynamic_model", dynamic_model, std::string("portable"));
  param.param("fix_mode", fix_mode, std::string("both"));
  param.param("dr_limit", dr_limit, 0);
  if (meas_rate <= 0) {
    ROS_ERROR("Invalid settings: meas_rate must be > 0");
    return 1;
  }
  if (dr_limit < 0 || dr_limit > 255) {
    ROS_ERROR("Invalid settings: dr_limit must be between 0 and 255");
    return 1;
  }
  
  DynamicModel dmodel;
  FixMode fmode;
  try {
    dmodel = ublox_gps::modelFromString(dynamic_model);
    fmode = ublox_gps::fixModeFromString(fix_mode);
  } catch (std::exception& e) {
    ROS_ERROR("Invalid settings: %s", e.what());
    return 1;
  }
  
  //  configure diagnostic updater for frequency
  updater->add("fix", &fix_diagnostic);
  updater->force_update();
  
  const double target_freq = 1000.0 / meas_rate;
  double min_freq = target_freq;
  double max_freq = target_freq;
  diagnostic_updater::FrequencyStatusParam freq_param(&min_freq, &max_freq, 0.05, 10);
  diagnostic_updater::TimeStampStatusParam time_param(0,meas_rate * 1e-3 * 0.05);
  freq_diag.reset(new diagnostic_updater::TopicDiagnostic(std::string("fix"), 
                                                          *updater, 
                                                          freq_param,
                                                          time_param));
  
  boost::smatch match;
  if (boost::regex_match(device, match, boost::regex("(tcp|udp)://(.+):(\\d+)"))) {
    std::string proto(match[1]);
    std::string host(match[2]);
    std::string port(match[3]);
    ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str());

    if (proto == "tcp") {
      boost::asio::ip::tcp::resolver::iterator endpoint;

      try {
        boost::asio::ip::tcp::resolver resolver(io_service);
        endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port));
      } catch(std::runtime_error& e) {
        ROS_ERROR("Could not resolve %s:%s: %s", host.c_str(), port.c_str(), e.what());
        return 1; //  exit
      }

      boost::asio::ip::tcp::socket *socket = new boost::asio::ip::tcp::socket(io_service);
      tcp_handle.reset(socket);

      try {
        socket->connect(*endpoint);
      } catch(std::runtime_error& e) {
        ROS_ERROR("Could not connect to %s:%s: %s", endpoint->host_name().c_str(), endpoint->service_name().c_str(), e.what());
        return 1; //  exit
      }
    
      ROS_INFO("Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str());
      gps.initialize(*socket, io_service);
    } else {
      ROS_ERROR("Protocol '%s' is unsupported", proto.c_str());
      return 1; //  exit
    }
  } else {
    boost::asio::serial_port *serial = new boost::asio::serial_port(io_service);
    serial_handle.reset(serial);

    // open serial port
    try {
      serial->open(device);
    } catch (std::runtime_error& e) {
      ROS_ERROR("Could not open serial port %s: %s", device.c_str(), e.what());
      return 1; //  exit
    }

    ROS_INFO("Opened serial port %s", device.c_str());
    gps.setBaudrate(baudrate);
    gps.initialize(*serial, io_service);
  }

  //  apply all requested settings
  try {
    if (!gps.isInitialized()) {
      throw std::runtime_error("Failed to initialize.");
    }
    if (!gps.setMeasRate(meas_rate)) {
      std::stringstream ss;
      ss << "Failed to set measurement rate " << meas_rate << ".";
      throw std::runtime_error(ss.str());
    }
    if (!gps.enableSBAS(enable_sbas)) {
      throw std::runtime_error(std::string("Failed to ") + 
                               ((enable_sbas) ? "enable" : "disable") + " SBAS.");
    }
    if (!gps.setDynamicModel(dmodel)) {
      throw std::runtime_error("Failed to set model: " + 
                               dynamic_model + ".");
    }
    if (!gps.setFixMode(fmode)) {
      throw std::runtime_error("Failed to set fix mode: " + fix_mode + ".");
    }
    if (!gps.setDeadReckonLimit(dr_limit)) {
      std::stringstream ss;
      ss << "Failed to set dead reckoning limit: " << dr_limit << ".";
      throw std::runtime_error(ss.str());
    }
    
  } catch (std::exception& e) {
    setup_ok = false;
    ROS_ERROR("Error configuring device: %s", e.what());
  }

  if (setup_ok) {
    ROS_INFO("U-Blox configured successfully.");
    
    // subscribe messages
    param.param("all", enabled["all"], false);
    param.param("rxm", enabled["rxm"], false);
    param.param("aid", enabled["aid"], false);
    
    param.param("nav_sol", enabled["nav_sol"], enabled["all"]);
    if (enabled["nav_sol"]) gps.subscribe<ublox_msgs::NavSOL>(boost::bind(&publish<ublox_msgs::NavSOL>, _1, "navsol"), 1);
    param.param("nav_status", enabled["nav_status"], true);
    if (enabled["nav_status"]) gps.subscribe<ublox_msgs::NavSTATUS>(&publishNavStatus, 1);
    param.param("nav_svinfo", enabled["nav_svinfo"], enabled["all"]);
    if (enabled["nav_svinfo"]) gps.subscribe<ublox_msgs::NavSVINFO>(&publishNavSVINFO, 20);
    param.param("nav_clk", enabled["nav_clk"], enabled["all"]);
    if (enabled["nav_clk"]) gps.subscribe<ublox_msgs::NavCLOCK>(&publishNavCLK, 1);
    param.param("rxm_raw", enabled["rxm_raw"], enabled["all"] || enabled["rxm"]);
    if (enabled["rxm_raw"]) gps.subscribe<ublox_msgs::RxmRAW>(&publishRxmRAW, 1);
    param.param("rxm_sfrb", enabled["rxm_sfrb"], enabled["all"] || enabled["rxm"]);
    if (enabled["rxm_sfrb"]) gps.subscribe<ublox_msgs::RxmSFRB>(&publishRxmSFRB, 1);
    param.param("nav_posllh", enabled["nav_posllh"], true);
    if (enabled["nav_posllh"]) gps.subscribe<ublox_msgs::NavPOSLLH>(&publishNavPosLLH, 1);
    param.param("nav_velned", enabled["nav_velned"], true);
    if (enabled["nav_velned"]) gps.subscribe<ublox_msgs::NavVELNED>(&publishNavVelNED, 1);
    param.param("aid_alm", enabled["aid_alm"], enabled["all"] || enabled["aid"]);
    if (enabled["aid_alm"]) gps.subscribe<ublox_msgs::AidALM>(&publishAidALM);
    param.param("aid_eph", enabled["aid_eph"], enabled["all"] || enabled["aid"]);
    if (enabled["aid_eph"]) gps.subscribe<ublox_msgs::AidEPH>(&publishAidEPH);
    param.param("aid_hui", enabled["aid_hui"], enabled["all"] || enabled["aid"]);
    if (enabled["aid_hui"]) gps.subscribe<ublox_msgs::AidHUI>(&publishAidHUI);
    
    poller = nh->createTimer(ros::Duration(1.0), &pollMessages);
    poller.start();
    ros::spin();
  }

  if (gps.isInitialized()) {
    gps.close();
    ROS_INFO("Closed connection to %s.", device.c_str());
  }
  return 0;
}