00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "ublox_gps/node.h"
00031 #include <cmath>
00032 #include <string>
00033 #include <sstream>
00034
00035 using namespace ublox_node;
00036
00037
00038
00039
00040 uint8_t ublox_node::modelFromString(const std::string& model) {
00041 std::string lower = model;
00042 std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
00043 if(lower == "portable") {
00044 return ublox_msgs::CfgNAV5::DYN_MODEL_PORTABLE;
00045 } else if(lower == "stationary") {
00046 return ublox_msgs::CfgNAV5::DYN_MODEL_STATIONARY;
00047 } else if(lower == "pedestrian") {
00048 return ublox_msgs::CfgNAV5::DYN_MODEL_PEDESTRIAN;
00049 } else if(lower == "automotive") {
00050 return ublox_msgs::CfgNAV5::DYN_MODEL_AUTOMOTIVE;
00051 } else if(lower == "sea") {
00052 return ublox_msgs::CfgNAV5::DYN_MODEL_SEA;
00053 } else if(lower == "airborne1") {
00054 return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_1G;
00055 } else if(lower == "airborne2") {
00056 return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_2G;
00057 } else if(lower == "airborne4") {
00058 return ublox_msgs::CfgNAV5::DYN_MODEL_AIRBORNE_4G;
00059 } else if(lower == "wristwatch") {
00060 return ublox_msgs::CfgNAV5::DYN_MODEL_WRIST_WATCH;
00061 }
00062
00063 throw std::runtime_error("Invalid settings: " + lower +
00064 " is not a valid dynamic model.");
00065 }
00066
00067 uint8_t ublox_node::fixModeFromString(const std::string& mode) {
00068 std::string lower = mode;
00069 std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
00070 if (lower == "2d") {
00071 return ublox_msgs::CfgNAV5::FIX_MODE_2D_ONLY;
00072 } else if (lower == "3d") {
00073 return ublox_msgs::CfgNAV5::FIX_MODE_3D_ONLY;
00074 } else if (lower == "auto") {
00075 return ublox_msgs::CfgNAV5::FIX_MODE_AUTO;
00076 }
00077
00078 throw std::runtime_error("Invalid settings: " + mode +
00079 " is not a valid fix mode.");
00080 }
00081
00082
00083
00084
00085 UbloxNode::UbloxNode() {
00086 initialize();
00087 }
00088
00089 void UbloxNode::addFirmwareInterface() {
00090 int ublox_version;
00091 if (protocol_version_ < 14) {
00092 components_.push_back(ComponentPtr(new UbloxFirmware6));
00093 ublox_version = 6;
00094 } else if (protocol_version_ >= 14 && protocol_version_ <= 15) {
00095 components_.push_back(ComponentPtr(new UbloxFirmware7));
00096 ublox_version = 7;
00097 } else {
00098 components_.push_back(ComponentPtr(new UbloxFirmware8));
00099 ublox_version = 8;
00100 }
00101 ROS_INFO("U-Blox Firmware Version: %d", ublox_version);
00102 }
00103
00104
00105 void UbloxNode::addProductInterface(std::string product_category,
00106 std::string ref_rov) {
00107 if (product_category.compare("HPG") == 0 && ref_rov.compare("REF") == 0)
00108 components_.push_back(ComponentPtr(new HpgRefProduct));
00109 else if (product_category.compare("HPG") == 0 && ref_rov.compare("ROV") == 0)
00110 components_.push_back(ComponentPtr(new HpgRovProduct));
00111 else if (product_category.compare("TIM") == 0)
00112 components_.push_back(ComponentPtr(new TimProduct));
00113 else if (product_category.compare("ADR") == 0 ||
00114 product_category.compare("UDR") == 0)
00115 components_.push_back(ComponentPtr(new AdrUdrProduct));
00116 else if (product_category.compare("FTS") == 0)
00117 components_.push_back(ComponentPtr(new FtsProduct));
00118 else if(product_category.compare("SPG") != 0)
00119 ROS_WARN("Product category %s %s from MonVER message not recognized %s",
00120 product_category.c_str(), ref_rov.c_str(),
00121 "options are HPG REF, HPG ROV, TIM, ADR, UDR, FTS, SPG");
00122 }
00123
00124 void UbloxNode::getRosParams() {
00125 nh->param("device", device_, std::string("/dev/ttyACM0"));
00126 nh->param("frame_id", frame_id, std::string("gps"));
00127
00128
00129 getRosUint("load/mask", load_.loadMask, 0);
00130 getRosUint("load/device", load_.deviceMask, 0);
00131 getRosUint("save/mask", save_.saveMask, 0);
00132 getRosUint("save/device", save_.deviceMask, 0);
00133
00134
00135 getRosUint("uart1/baudrate", baudrate_, 9600);
00136 getRosUint("uart1/in", uart_in_, ublox_msgs::CfgPRT::PROTO_UBX
00137 | ublox_msgs::CfgPRT::PROTO_NMEA
00138 | ublox_msgs::CfgPRT::PROTO_RTCM);
00139 getRosUint("uart1/out", uart_out_, ublox_msgs::CfgPRT::PROTO_UBX);
00140
00141 set_usb_ = false;
00142 if (nh->hasParam("usb/in") || nh->hasParam("usb/out")) {
00143 set_usb_ = true;
00144 if(!getRosUint("usb/in", usb_in_)) {
00145 throw std::runtime_error(std::string("usb/out is set, therefore ") +
00146 "usb/in must be set");
00147 }
00148 if(!getRosUint("usb/out", usb_out_)) {
00149 throw std::runtime_error(std::string("usb/in is set, therefore ") +
00150 "usb/out must be set");
00151 }
00152 getRosUint("usb/tx_ready", usb_tx_, 0);
00153 }
00154
00155 nh->param("rate", rate_, 4.0);
00156 getRosUint("nav_rate", nav_rate, 1);
00157
00158 getRosUint("rtcm/ids", rtcm_ids);
00159 getRosUint("rtcm/rates", rtcm_rates);
00160
00161 nh->param("enable_ppp", enable_ppp_, false);
00162
00163 nh->param("sbas", enable_sbas_, false);
00164 getRosUint("sbas/max", max_sbas_, 0);
00165 getRosUint("sbas/usage", sbas_usage_, 0);
00166 nh->param("dynamic_model", dynamic_model_, std::string("portable"));
00167 nh->param("fix_mode", fix_mode_, std::string("auto"));
00168 getRosUint("dr_limit", dr_limit_, 0);
00169
00170 if (enable_ppp_)
00171 ROS_WARN("Warning: PPP is enabled - this is an expert setting.");
00172
00173 checkMin(rate_, 0, "rate");
00174
00175 if(rtcm_ids.size() != rtcm_rates.size())
00176 throw std::runtime_error(std::string("Invalid settings: size of rtcm_ids") +
00177 " must match size of rtcm_rates");
00178
00179 dmodel_ = modelFromString(dynamic_model_);
00180 fmode_ = fixModeFromString(fix_mode_);
00181
00182 nh->param("dat/set", set_dat_, false);
00183 if(set_dat_) {
00184 std::vector<float> shift, rot;
00185 if (!nh->getParam("dat/majA", cfg_dat_.majA)
00186 || nh->getParam("dat/flat", cfg_dat_.flat)
00187 || nh->getParam("dat/shift", shift)
00188 || nh->getParam("dat/rot", rot)
00189 || nh->getParam("dat/scale", cfg_dat_.scale))
00190 throw std::runtime_error(std::string("dat/set is true, therefore ") +
00191 "dat/majA, dat/flat, dat/shift, dat/rot, & dat/scale must be set");
00192 if(shift.size() != 3 || rot.size() != 3)
00193 throw std::runtime_error(std::string("size of dat/shift & dat/rot ") +
00194 "must be 3");
00195 checkRange(cfg_dat_.majA, 6300000.0, 6500000.0, "dat/majA");
00196 checkRange(cfg_dat_.flat, 0.0, 500.0, "dat/flat");
00197
00198 checkRange(shift, 0.0, 500.0, "dat/shift");
00199 cfg_dat_.dX = shift[0];
00200 cfg_dat_.dY = shift[1];
00201 cfg_dat_.dZ = shift[2];
00202
00203 checkRange(rot, -5000.0, 5000.0, "dat/rot");
00204 cfg_dat_.rotX = rot[0];
00205 cfg_dat_.rotY = rot[1];
00206 cfg_dat_.rotZ = rot[2];
00207
00208 checkRange(cfg_dat_.scale, 0.0, 50.0, "scale");
00209 }
00210
00211
00212 meas_rate = 1000 / rate_;
00213
00214
00215 nh->param("config_on_startup", config_on_startup_flag_, true);
00216
00217
00218 rawDataStreamPa_.getRosParams();
00219 }
00220
00221 void UbloxNode::pollMessages(const ros::TimerEvent& event) {
00222 static std::vector<uint8_t> payload(1, 1);
00223 if (enabled["aid_alm"])
00224 gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload);
00225 if (enabled["aid_eph"])
00226 gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload);
00227 if (enabled["aid_hui"])
00228 gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI);
00229
00230 payload[0]++;
00231 if (payload[0] > 32) {
00232 payload[0] = 1;
00233 }
00234 }
00235
00236 void UbloxNode::printInf(const ublox_msgs::Inf &m, uint8_t id) {
00237 if (id == ublox_msgs::Message::INF::ERROR)
00238 ROS_ERROR_STREAM("INF: " << std::string(m.str.begin(), m.str.end()));
00239 else if (id == ublox_msgs::Message::INF::WARNING)
00240 ROS_WARN_STREAM("INF: " << std::string(m.str.begin(), m.str.end()));
00241 else if (id == ublox_msgs::Message::INF::DEBUG)
00242 ROS_DEBUG_STREAM("INF: " << std::string(m.str.begin(), m.str.end()));
00243 else
00244 ROS_INFO_STREAM("INF: " << std::string(m.str.begin(), m.str.end()));
00245 }
00246
00247 void UbloxNode::subscribe() {
00248 ROS_DEBUG("Subscribing to U-Blox messages");
00249
00250 nh->param("publish/all", enabled["all"], false);
00251 nh->param("inf/all", enabled["inf"], true);
00252 nh->param("publish/nav/all", enabled["nav"], enabled["all"]);
00253 nh->param("publish/rxm/all", enabled["rxm"], enabled["all"]);
00254 nh->param("publish/aid/all", enabled["aid"], enabled["all"]);
00255 nh->param("publish/mon/all", enabled["mon"], enabled["all"]);
00256
00257
00258 nh->param("publish/nav/status", enabled["nav_status"], enabled["nav"]);
00259 if (enabled["nav_status"])
00260 gps.subscribe<ublox_msgs::NavSTATUS>(boost::bind(
00261 publish<ublox_msgs::NavSTATUS>, _1, "navstatus"), kSubscribeRate);
00262
00263 nh->param("publish/nav/posecef", enabled["nav_posecef"], enabled["nav"]);
00264 if (enabled["nav_posecef"])
00265 gps.subscribe<ublox_msgs::NavPOSECEF>(boost::bind(
00266 publish<ublox_msgs::NavPOSECEF>, _1, "navposecef"), kSubscribeRate);
00267
00268 nh->param("publish/nav/clock", enabled["nav_clock"], enabled["nav"]);
00269 if (enabled["nav_clock"])
00270 gps.subscribe<ublox_msgs::NavCLOCK>(boost::bind(
00271 publish<ublox_msgs::NavCLOCK>, _1, "navclock"), kSubscribeRate);
00272
00273
00274 nh->param("inf/debug", enabled["inf_debug"], false);
00275 if (enabled["inf_debug"])
00276 gps.subscribeId<ublox_msgs::Inf>(
00277 boost::bind(&UbloxNode::printInf, this, _1,
00278 ublox_msgs::Message::INF::DEBUG),
00279 ublox_msgs::Message::INF::DEBUG);
00280
00281 nh->param("inf/error", enabled["inf_error"], enabled["inf"]);
00282 if (enabled["inf_error"])
00283 gps.subscribeId<ublox_msgs::Inf>(
00284 boost::bind(&UbloxNode::printInf, this, _1,
00285 ublox_msgs::Message::INF::ERROR),
00286 ublox_msgs::Message::INF::ERROR);
00287
00288 nh->param("inf/notice", enabled["inf_notice"], enabled["inf"]);
00289 if (enabled["inf_notice"])
00290 gps.subscribeId<ublox_msgs::Inf>(
00291 boost::bind(&UbloxNode::printInf, this, _1,
00292 ublox_msgs::Message::INF::NOTICE),
00293 ublox_msgs::Message::INF::NOTICE);
00294
00295 nh->param("inf/test", enabled["inf_test"], enabled["inf"]);
00296 if (enabled["inf_test"])
00297 gps.subscribeId<ublox_msgs::Inf>(
00298 boost::bind(&UbloxNode::printInf, this, _1,
00299 ublox_msgs::Message::INF::TEST),
00300 ublox_msgs::Message::INF::TEST);
00301
00302 nh->param("inf/warning", enabled["inf_warning"], enabled["inf"]);
00303 if (enabled["inf_warning"])
00304 gps.subscribeId<ublox_msgs::Inf>(
00305 boost::bind(&UbloxNode::printInf, this, _1,
00306 ublox_msgs::Message::INF::WARNING),
00307 ublox_msgs::Message::INF::WARNING);
00308
00309
00310 nh->param("publish/aid/alm", enabled["aid_alm"], enabled["aid"]);
00311 if (enabled["aid_alm"])
00312 gps.subscribe<ublox_msgs::AidALM>(boost::bind(
00313 publish<ublox_msgs::AidALM>, _1, "aidalm"), kSubscribeRate);
00314
00315 nh->param("publish/aid/eph", enabled["aid_eph"], enabled["aid"]);
00316 if (enabled["aid_eph"])
00317 gps.subscribe<ublox_msgs::AidEPH>(boost::bind(
00318 publish<ublox_msgs::AidEPH>, _1, "aideph"), kSubscribeRate);
00319
00320 nh->param("publish/aid/hui", enabled["aid_hui"], enabled["aid"]);
00321 if (enabled["aid_hui"])
00322 gps.subscribe<ublox_msgs::AidHUI>(boost::bind(
00323 publish<ublox_msgs::AidHUI>, _1, "aidhui"), kSubscribeRate);
00324
00325 for(int i = 0; i < components_.size(); i++)
00326 components_[i]->subscribe();
00327 }
00328
00329 void UbloxNode::initializeRosDiagnostics() {
00330 if (!nh->hasParam("diagnostic_period"))
00331 nh->setParam("diagnostic_period", kDiagnosticPeriod);
00332
00333 updater.reset(new diagnostic_updater::Updater());
00334 updater->setHardwareID("ublox");
00335
00336
00337 freq_diag = FixDiagnostic(std::string("fix"), kFixFreqTol,
00338 kFixFreqWindow, kTimeStampStatusMin);
00339 for(int i = 0; i < components_.size(); i++)
00340 components_[i]->initializeRosDiagnostics();
00341 }
00342
00343
00344 void UbloxNode::processMonVer() {
00345 ublox_msgs::MonVER monVer;
00346 if (!gps.poll(monVer))
00347 throw std::runtime_error("Failed to poll MonVER & set relevant settings");
00348
00349 ROS_DEBUG("%s, HW VER: %s", monVer.swVersion.c_array(),
00350 monVer.hwVersion.c_array());
00351
00352 std::vector<std::string> extension;
00353 extension.reserve(monVer.extension.size());
00354 for(std::size_t i = 0; i < monVer.extension.size(); ++i) {
00355 ROS_DEBUG("%s", monVer.extension[i].field.c_array());
00356
00357 unsigned char* end = std::find(monVer.extension[i].field.begin(),
00358 monVer.extension[i].field.end(), '\0');
00359 extension.push_back(std::string(monVer.extension[i].field.begin(), end));
00360 }
00361
00362
00363 for(std::size_t i = 0; i < extension.size(); ++i) {
00364 std::size_t found = extension[i].find("PROTVER");
00365 if (found != std::string::npos) {
00366 protocol_version_ = ::atof(
00367 extension[i].substr(8, extension[i].size()-8).c_str());
00368 break;
00369 }
00370 }
00371 if (protocol_version_ == 0)
00372 ROS_WARN("Failed to parse MonVER and determine protocol version. %s",
00373 "Defaulting to firmware version 6.");
00374 addFirmwareInterface();
00375
00376 if(protocol_version_ < 18) {
00377
00378 std::vector<std::string> strs;
00379 if(extension.size() > 0)
00380 boost::split(strs, extension[extension.size()-1], boost::is_any_of(";"));
00381 for(size_t i = 0; i < strs.size(); i++)
00382 supported.insert(strs[i]);
00383 } else {
00384 for(std::size_t i = 0; i < extension.size(); ++i) {
00385 std::vector<std::string> strs;
00386
00387 if(i <= extension.size() - 2) {
00388 boost::split(strs, extension[i], boost::is_any_of("="));
00389 if(strs.size() > 1) {
00390 if (strs[0].compare(std::string("FWVER")) == 0) {
00391 if(strs[1].length() > 8)
00392 addProductInterface(strs[1].substr(0, 3), strs[1].substr(8, 10));
00393 else
00394 addProductInterface(strs[1].substr(0, 3));
00395 continue;
00396 }
00397 }
00398 }
00399
00400 if(i >= extension.size() - 2) {
00401 boost::split(strs, extension[i], boost::is_any_of(";"));
00402 for(size_t i = 0; i < strs.size(); i++)
00403 supported.insert(strs[i]);
00404 }
00405 }
00406 }
00407 }
00408
00409 bool UbloxNode::configureUblox() {
00410 try {
00411 if (!gps.isInitialized())
00412 throw std::runtime_error("Failed to initialize.");
00413 if (load_.loadMask != 0) {
00414 ROS_DEBUG("Loading u-blox configuration from memory. %u", load_.loadMask);
00415 if (!gps.configure(load_))
00416 throw std::runtime_error(std::string("Failed to load configuration ") +
00417 "from memory");
00418 if (load_.loadMask & load_.MASK_IO_PORT) {
00419 ROS_DEBUG("Loaded I/O configuration from memory, resetting serial %s",
00420 "communications.");
00421 boost::posix_time::seconds wait(kResetWait);
00422 gps.reset(wait);
00423 if (!gps.isConfigured())
00424 throw std::runtime_error(std::string("Failed to reset serial I/O") +
00425 "after loading I/O configurations from device memory.");
00426 }
00427 }
00428
00429 if (config_on_startup_flag_) {
00430 if (set_usb_) {
00431 gps.configUsb(usb_tx_, usb_in_, usb_out_);
00432 }
00433 if (!gps.configRate(meas_rate, nav_rate)) {
00434 std::stringstream ss;
00435 ss << "Failed to set measurement rate to " << meas_rate
00436 << "ms and navigation rate to " << nav_rate;
00437 throw std::runtime_error(ss.str());
00438 }
00439
00440 if(supportsGnss("SBAS")) {
00441 if (!gps.configSbas(enable_sbas_, sbas_usage_, max_sbas_)) {
00442 throw std::runtime_error(std::string("Failed to ") +
00443 ((enable_sbas_) ? "enable" : "disable") +
00444 " SBAS.");
00445 }
00446 }
00447 if (!gps.setPpp(enable_ppp_))
00448 throw std::runtime_error(std::string("Failed to ") +
00449 ((enable_ppp_) ? "enable" : "disable")
00450 + " PPP.");
00451 if (!gps.setDynamicModel(dmodel_))
00452 throw std::runtime_error("Failed to set model: " + dynamic_model_ + ".");
00453 if (!gps.setFixMode(fmode_))
00454 throw std::runtime_error("Failed to set fix mode: " + fix_mode_ + ".");
00455 if (!gps.setDeadReckonLimit(dr_limit_)) {
00456 std::stringstream ss;
00457 ss << "Failed to set dead reckoning limit: " << dr_limit_ << ".";
00458 throw std::runtime_error(ss.str());
00459 }
00460 if (set_dat_ && !gps.configure(cfg_dat_))
00461 throw std::runtime_error("Failed to set user-defined datum.");
00462
00463 for (int i = 0; i < components_.size(); i++) {
00464 if(!components_[i]->configureUblox())
00465 return false;
00466 }
00467 }
00468 if (save_.saveMask != 0) {
00469 ROS_DEBUG("Saving the u-blox configuration, mask %u, device %u",
00470 save_.saveMask, save_.deviceMask);
00471 if(!gps.configure(save_))
00472 ROS_ERROR("u-blox unable to save configuration to non-volatile memory");
00473 }
00474 } catch (std::exception& e) {
00475 ROS_FATAL("Error configuring u-blox: %s", e.what());
00476 return false;
00477 }
00478 return true;
00479 }
00480
00481 void UbloxNode::configureInf() {
00482 ublox_msgs::CfgINF msg;
00483
00484 ublox_msgs::CfgINF_Block block;
00485 block.protocolID = block.PROTOCOL_ID_UBX;
00486
00487 uint8_t mask = (enabled["inf_error"] ? block.INF_MSG_ERROR : 0) |
00488 (enabled["inf_warning"] ? block.INF_MSG_WARNING : 0) |
00489 (enabled["inf_notice"] ? block.INF_MSG_NOTICE : 0) |
00490 (enabled["inf_test"] ? block.INF_MSG_TEST : 0) |
00491 (enabled["inf_debug"] ? block.INF_MSG_DEBUG : 0);
00492 for (int i = 0; i < block.infMsgMask.size(); i++)
00493 block.infMsgMask[i] = mask;
00494
00495 msg.blocks.push_back(block);
00496
00497
00498 if (uart_in_ & ublox_msgs::CfgPRT::PROTO_NMEA) {
00499 ublox_msgs::CfgINF_Block block;
00500 block.protocolID = block.PROTOCOL_ID_NMEA;
00501
00502 for (int i = 0; i < block.infMsgMask.size(); i++)
00503 block.infMsgMask[i] = mask;
00504 msg.blocks.push_back(block);
00505 }
00506
00507 ROS_DEBUG("Configuring INF messages");
00508 if (!gps.configure(msg))
00509 ROS_WARN("Failed to configure INF messages");
00510 }
00511
00512 void UbloxNode::initializeIo() {
00513 gps.setConfigOnStartup(config_on_startup_flag_);
00514
00515 boost::smatch match;
00516 if (boost::regex_match(device_, match,
00517 boost::regex("(tcp|udp)://(.+):(\\d+)"))) {
00518 std::string proto(match[1]);
00519 if (proto == "tcp") {
00520 std::string host(match[2]);
00521 std::string port(match[3]);
00522 ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(),
00523 port.c_str());
00524 gps.initializeTcp(host, port);
00525 } else {
00526 throw std::runtime_error("Protocol '" + proto + "' is unsupported");
00527 }
00528 } else {
00529 gps.initializeSerial(device_, baudrate_, uart_in_, uart_out_);
00530 }
00531
00532
00533 if (rawDataStreamPa_.isEnabled()) {
00534 gps.setRawDataCallback(
00535 boost::bind(&RawDataStreamPa::ubloxCallback,&rawDataStreamPa_, _1, _2));
00536 rawDataStreamPa_.initialize();
00537 }
00538 }
00539
00540 void UbloxNode::initialize() {
00541
00542 getRosParams();
00543 initializeIo();
00544
00545 processMonVer();
00546 if(protocol_version_ <= 14) {
00547 if(nh->param("raw_data", false))
00548 components_.push_back(ComponentPtr(new RawDataProduct));
00549 }
00550
00551 for (int i = 0; i < components_.size(); i++)
00552 components_[i]->getRosParams();
00553
00554 initializeRosDiagnostics();
00555
00556 if (configureUblox()) {
00557 ROS_INFO("U-Blox configured successfully.");
00558
00559 subscribe();
00560
00561 configureInf();
00562
00563 ros::Timer poller;
00564 poller = nh->createTimer(ros::Duration(kPollDuration),
00565 &UbloxNode::pollMessages,
00566 this);
00567 poller.start();
00568 ros::spin();
00569 }
00570 shutdown();
00571 }
00572
00573 void UbloxNode::shutdown() {
00574 if (gps.isInitialized()) {
00575 gps.close();
00576 ROS_INFO("Closed connection to %s.", device_.c_str());
00577 }
00578 }
00579
00580
00581
00582
00583 void UbloxFirmware::initializeRosDiagnostics() {
00584 updater->add("fix", this, &UbloxFirmware::fixDiagnostic);
00585 updater->force_update();
00586 }
00587
00588
00589
00590
00591 UbloxFirmware6::UbloxFirmware6() {}
00592
00593 void UbloxFirmware6::getRosParams() {
00594
00595 fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS;
00596
00597 nh->param("nmea/set", set_nmea_, false);
00598 if (set_nmea_) {
00599 bool compat, consider;
00600
00601 if (!getRosUint("nmea/version", cfg_nmea_.version))
00602 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00603 "true, therefore nmea/version must be set");
00604 if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV))
00605 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00606 "true, therefore nmea/num_sv must be set");
00607 if (!nh->getParam("nmea/compat", compat))
00608 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00609 "true, therefore nmea/compat must be set");
00610 if (!nh->getParam("nmea/consider", consider))
00611 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00612 "true, therefore nmea/consider must be set");
00613
00614
00615 cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0;
00616 cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0;
00617
00618
00619 bool temp;
00620 nh->param("nmea/filter/pos", temp, false);
00621 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0;
00622 nh->param("nmea/filter/msk_pos", temp, false);
00623 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0;
00624 nh->param("nmea/filter/time", temp, false);
00625 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0;
00626 nh->param("nmea/filter/date", temp, false);
00627 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0;
00628 nh->param("nmea/filter/sbas", temp, false);
00629 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_SBAS_FILT : 0;
00630 nh->param("nmea/filter/track", temp, false);
00631 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0;
00632 }
00633 }
00634
00635 bool UbloxFirmware6::configureUblox() {
00636 ROS_WARN("ublox_version < 7, ignoring GNSS settings");
00637
00638 if (set_nmea_ && !gps.configure(cfg_nmea_))
00639 throw std::runtime_error("Failed to configure NMEA");
00640
00641 return true;
00642 }
00643
00644 void UbloxFirmware6::subscribe() {
00645
00646 nh->param("publish/nav/posllh", enabled["nav_posllh"], enabled["nav"]);
00647 nh->param("publish/nav/sol", enabled["nav_sol"], enabled["nav"]);
00648 nh->param("publish/nav/velned", enabled["nav_velned"], enabled["nav"]);
00649
00650
00651
00652 gps.subscribe<ublox_msgs::NavPOSLLH>(boost::bind(
00653 &UbloxFirmware6::callbackNavPosLlh, this, _1), kSubscribeRate);
00654 gps.subscribe<ublox_msgs::NavSOL>(boost::bind(
00655
00656 &UbloxFirmware6::callbackNavSol, this, _1), kSubscribeRate);
00657
00658 gps.subscribe<ublox_msgs::NavVELNED>(boost::bind(
00659 &UbloxFirmware6::callbackNavVelNed, this, _1), kSubscribeRate);
00660
00661
00662 nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]);
00663 if (enabled["nav_svinfo"])
00664 gps.subscribe<ublox_msgs::NavSVINFO>(boost::bind(
00665 publish<ublox_msgs::NavSVINFO>, _1, "navsvinfo"),
00666 kNavSvInfoSubscribeRate);
00667
00668
00669 nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]);
00670 if (enabled["mon_hw"])
00671 gps.subscribe<ublox_msgs::MonHW6>(boost::bind(
00672 publish<ublox_msgs::MonHW6>, _1, "monhw"), kSubscribeRate);
00673 }
00674
00675 void UbloxFirmware6::fixDiagnostic(
00676 diagnostic_updater::DiagnosticStatusWrapper& stat) {
00677
00678 if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_DEAD_RECKONING_ONLY) {
00679 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
00680 stat.message = "Dead reckoning only";
00681 } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_2D_FIX) {
00682 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00683 stat.message = "2D fix";
00684 } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_3D_FIX) {
00685 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00686 stat.message = "3D fix";
00687 } else if (last_nav_sol_.gpsFix ==
00688 ublox_msgs::NavSOL::GPS_GPS_DEAD_RECKONING_COMBINED) {
00689 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00690 stat.message = "GPS and dead reckoning combined";
00691 } else if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_TIME_ONLY_FIX) {
00692 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00693 stat.message = "Time fix only";
00694 }
00695
00696 if (!(last_nav_sol_.flags & ublox_msgs::NavSOL::FLAGS_GPS_FIX_OK)) {
00697 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
00698 stat.message += ", fix not ok";
00699 }
00700
00701 if (last_nav_sol_.gpsFix == ublox_msgs::NavSOL::GPS_NO_FIX) {
00702 stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00703 stat.message = "No fix";
00704 }
00705
00706
00707 stat.add("iTOW [ms]", last_nav_pos_.iTOW);
00708 stat.add("Latitude [deg]", last_nav_pos_.lat * 1e-7);
00709 stat.add("Longitude [deg]", last_nav_pos_.lon * 1e-7);
00710 stat.add("Altitude [m]", last_nav_pos_.height * 1e-3);
00711 stat.add("Height above MSL [m]", last_nav_pos_.hMSL * 1e-3);
00712 stat.add("Horizontal Accuracy [m]", last_nav_pos_.hAcc * 1e-3);
00713 stat.add("Vertical Accuracy [m]", last_nav_pos_.vAcc * 1e-3);
00714 stat.add("# SVs used", (int)last_nav_sol_.numSV);
00715 }
00716
00717 void UbloxFirmware6::callbackNavPosLlh(const ublox_msgs::NavPOSLLH& m) {
00718 if(enabled["nav_posllh"]) {
00719 static ros::Publisher publisher =
00720 nh->advertise<ublox_msgs::NavPOSLLH>("navposllh", kROSQueueSize);
00721 publisher.publish(m);
00722 }
00723
00724
00725 static ros::Publisher fixPublisher =
00726 nh->advertise<sensor_msgs::NavSatFix>("fix", kROSQueueSize);
00727 if (m.iTOW == last_nav_vel_.iTOW)
00728 fix_.header.stamp = velocity_.header.stamp;
00729 else
00730 fix_.header.stamp = ros::Time::now();
00731
00732 fix_.header.frame_id = frame_id;
00733 fix_.latitude = m.lat * 1e-7;
00734 fix_.longitude = m.lon * 1e-7;
00735 fix_.altitude = m.height * 1e-3;
00736
00737 if (last_nav_sol_.gpsFix >= last_nav_sol_.GPS_2D_FIX)
00738 fix_.status.status = fix_.status.STATUS_FIX;
00739 else
00740 fix_.status.status = fix_.status.STATUS_NO_FIX;
00741
00742
00743 const double varH = pow(m.hAcc / 1000.0, 2);
00744 const double varV = pow(m.vAcc / 1000.0, 2);
00745
00746 fix_.position_covariance[0] = varH;
00747 fix_.position_covariance[4] = varH;
00748 fix_.position_covariance[8] = varV;
00749 fix_.position_covariance_type =
00750 sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
00751
00752 fix_.status.service = fix_.status.SERVICE_GPS;
00753 fixPublisher.publish(fix_);
00754 last_nav_pos_ = m;
00755
00756 freq_diag.diagnostic->tick(fix_.header.stamp);
00757 updater->update();
00758 }
00759
00760 void UbloxFirmware6::callbackNavVelNed(const ublox_msgs::NavVELNED& m) {
00761 if(enabled["nav_velned"]) {
00762 static ros::Publisher publisher =
00763 nh->advertise<ublox_msgs::NavVELNED>("navvelned", kROSQueueSize);
00764 publisher.publish(m);
00765 }
00766
00767
00768 static ros::Publisher velocityPublisher =
00769 nh->advertise<geometry_msgs::TwistWithCovarianceStamped>("fix_velocity",
00770 kROSQueueSize);
00771 if (m.iTOW == last_nav_pos_.iTOW)
00772 velocity_.header.stamp = fix_.header.stamp;
00773 else
00774 velocity_.header.stamp = ros::Time::now();
00775 velocity_.header.frame_id = frame_id;
00776
00777
00778 velocity_.twist.twist.linear.x = m.velE / 100.0;
00779 velocity_.twist.twist.linear.y = m.velN / 100.0;
00780 velocity_.twist.twist.linear.z = -m.velD / 100.0;
00781
00782 const double varSpeed = pow(m.sAcc / 100.0, 2);
00783
00784 const int cols = 6;
00785 velocity_.twist.covariance[cols * 0 + 0] = varSpeed;
00786 velocity_.twist.covariance[cols * 1 + 1] = varSpeed;
00787 velocity_.twist.covariance[cols * 2 + 2] = varSpeed;
00788 velocity_.twist.covariance[cols * 3 + 3] = -1;
00789
00790 velocityPublisher.publish(velocity_);
00791 last_nav_vel_ = m;
00792 }
00793
00794 void UbloxFirmware6::callbackNavSol(const ublox_msgs::NavSOL& m) {
00795 if(enabled["nav_sol"]) {
00796 static ros::Publisher publisher =
00797 nh->advertise<ublox_msgs::NavSOL>("navsol", kROSQueueSize);
00798 publisher.publish(m);
00799 }
00800 last_nav_sol_ = m;
00801 }
00802
00803
00804
00805
00806 UbloxFirmware7::UbloxFirmware7() {}
00807
00808 void UbloxFirmware7::getRosParams() {
00809
00810
00811
00812
00813 nh->param("gnss/gps", enable_gps_, true);
00814 nh->param("gnss/glonass", enable_glonass_, false);
00815 nh->param("gnss/qzss", enable_qzss_, false);
00816 getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_,
00817 ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA);
00818 nh->param("gnss/sbas", enable_sbas_, false);
00819
00820 if(enable_gps_ && !supportsGnss("GPS"))
00821 ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by this device");
00822 if(enable_glonass_ && !supportsGnss("GLO"))
00823 ROS_WARN("gnss/glonass is true, but GLONASS is not %s",
00824 "supported by this device");
00825 if(enable_qzss_ && !supportsGnss("QZSS"))
00826 ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device");
00827 if(enable_sbas_ && !supportsGnss("SBAS"))
00828 ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device");
00829
00830 if(nh->hasParam("gnss/galileo"))
00831 ROS_WARN("ublox_version < 8, ignoring Galileo GNSS Settings");
00832 if(nh->hasParam("gnss/beidou"))
00833 ROS_WARN("ublox_version < 8, ignoring BeiDou Settings");
00834 if(nh->hasParam("gnss/imes"))
00835 ROS_WARN("ublox_version < 8, ignoring IMES GNSS Settings");
00836
00837
00838 fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS
00839 + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS;
00840
00841
00842
00843
00844 nh->param("nmea/set", set_nmea_, false);
00845 if (set_nmea_) {
00846 bool compat, consider;
00847
00848 if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion))
00849 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00850 "true, therefore nmea/version must be set");
00851 if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV))
00852 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00853 "true, therefore nmea/num_sv must be set");
00854 if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering))
00855 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00856 "true, therefore nmea/sv_numbering must be set");
00857 if (!nh->getParam("nmea/compat", compat))
00858 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00859 "true, therefore nmea/compat must be set");
00860 if (!nh->getParam("nmea/consider", consider))
00861 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
00862 "true, therefore nmea/consider must be set");
00863
00864
00865 cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0;
00866 cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0;
00867
00868 bool temp;
00869 nh->param("nmea/filter/pos", temp, false);
00870 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0;
00871 nh->param("nmea/filter/msk_pos", temp, false);
00872 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0;
00873 nh->param("nmea/filter/time", temp, false);
00874 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0;
00875 nh->param("nmea/filter/date", temp, false);
00876 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0;
00877 nh->param("nmea/filter/gps_only", temp, false);
00878 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_GPS_ONLY : 0;
00879 nh->param("nmea/filter/track", temp, false);
00880 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0;
00881
00882 nh->param("nmea/gnssToFilter/gps", temp, false);
00883 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0;
00884 nh->param("nmea/gnssToFilter/sbas", temp, false);
00885 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0;
00886 nh->param("nmea/gnssToFilter/qzss", temp, false);
00887 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0;
00888 nh->param("nmea/gnssToFilter/glonass", temp, false);
00889 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0;
00890
00891 getRosUint("nmea/main_talker_id", cfg_nmea_.mainTalkerId);
00892 getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsvTalkerId);
00893 }
00894 }
00895
00896 bool UbloxFirmware7::configureUblox() {
00898 ublox_msgs::CfgGNSS cfgGNSSRead;
00899 if (gps.poll(cfgGNSSRead)) {
00900 ROS_DEBUG("Read GNSS config.");
00901 ROS_DEBUG("Num. tracking channels in hardware: %i", cfgGNSSRead.numTrkChHw);
00902 ROS_DEBUG("Num. tracking channels to use: %i", cfgGNSSRead.numTrkChUse);
00903 } else {
00904 throw std::runtime_error("Failed to read the GNSS config.");
00905 }
00906
00907 ublox_msgs::CfgGNSS cfgGNSSWrite;
00908 cfgGNSSWrite.numConfigBlocks = 1;
00909 cfgGNSSWrite.numTrkChHw = cfgGNSSRead.numTrkChHw;
00910 cfgGNSSWrite.numTrkChUse = cfgGNSSRead.numTrkChUse;
00911 cfgGNSSWrite.msgVer = 0;
00912
00913
00914 if(supportsGnss("GLO")) {
00915 ublox_msgs::CfgGNSS_Block block;
00916 block.gnssId = block.GNSS_ID_GLONASS;
00917 block.resTrkCh = block.RES_TRK_CH_GLONASS;
00918 block.maxTrkCh = block.MAX_TRK_CH_GLONASS;
00919 block.flags = enable_glonass_ ? block.SIG_CFG_GLONASS_L1OF : 0;
00920 cfgGNSSWrite.blocks.push_back(block);
00921 if (!gps.configure(cfgGNSSWrite)) {
00922 throw std::runtime_error(std::string("Failed to ") +
00923 ((enable_glonass_) ? "enable" : "disable") +
00924 " GLONASS.");
00925 }
00926 }
00927
00928 if(supportsGnss("QZSS")) {
00929
00930 ublox_msgs::CfgGNSS_Block block;
00931 block.gnssId = block.GNSS_ID_QZSS;
00932 block.resTrkCh = block.RES_TRK_CH_QZSS;
00933 block.maxTrkCh = block.MAX_TRK_CH_QZSS;
00934 block.flags = enable_qzss_ ? qzss_sig_cfg_ : 0;
00935 cfgGNSSWrite.blocks[0] = block;
00936 if (!gps.configure(cfgGNSSWrite)) {
00937 throw std::runtime_error(std::string("Failed to ") +
00938 ((enable_glonass_) ? "enable" : "disable") +
00939 " QZSS.");
00940 }
00941 }
00942
00943 if(supportsGnss("SBAS")) {
00944
00945 ublox_msgs::CfgGNSS_Block block;
00946 block.gnssId = block.GNSS_ID_SBAS;
00947 block.resTrkCh = block.RES_TRK_CH_SBAS;
00948 block.maxTrkCh = block.MAX_TRK_CH_SBAS;
00949 block.flags = enable_sbas_ ? block.SIG_CFG_SBAS_L1CA : 0;
00950 cfgGNSSWrite.blocks[0] = block;
00951 if (!gps.configure(cfgGNSSWrite)) {
00952 throw std::runtime_error(std::string("Failed to ") +
00953 ((enable_sbas_) ? "enable" : "disable") +
00954 " SBAS.");
00955 }
00956 }
00957
00958 if(set_nmea_ && !gps.configure(cfg_nmea_))
00959 throw std::runtime_error("Failed to configure NMEA");
00960
00961 return true;
00962 }
00963
00964 void UbloxFirmware7::subscribe() {
00965
00966 nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]);
00967
00968
00969 gps.subscribe<ublox_msgs::NavPVT7>(boost::bind(
00970 &UbloxFirmware7Plus::callbackNavPvt, this, _1),
00971 kSubscribeRate);
00972
00973
00974 nh->param("publish/nav/svinfo", enabled["nav_svinfo"], enabled["nav"]);
00975 if (enabled["nav_svinfo"])
00976 gps.subscribe<ublox_msgs::NavSVINFO>(boost::bind(
00977 publish<ublox_msgs::NavSVINFO>, _1, "navsvinfo"),
00978 kNavSvInfoSubscribeRate);
00979
00980
00981 nh->param("publish/mon_hw", enabled["mon_hw"], enabled["mon"]);
00982 if (enabled["mon_hw"])
00983 gps.subscribe<ublox_msgs::MonHW>(boost::bind(
00984 publish<ublox_msgs::MonHW>, _1, "monhw"), kSubscribeRate);
00985 }
00986
00987
00988
00989
00990 UbloxFirmware8::UbloxFirmware8() {}
00991
00992 void UbloxFirmware8::getRosParams() {
00993
00994 nh->param("clear_bbr", clear_bbr_, false);
00995 gps.setSaveOnShutdown(nh->param("save_on_shutdown", false));
00996
00997
00998 nh->param("gnss/gps", enable_gps_, true);
00999 nh->param("gnss/galileo", enable_galileo_, false);
01000 nh->param("gnss/beidou", enable_beidou_, false);
01001 nh->param("gnss/imes", enable_imes_, false);
01002 nh->param("gnss/glonass", enable_glonass_, false);
01003 nh->param("gnss/qzss", enable_qzss_, false);
01004 nh->param("gnss/sbas", enable_sbas_, false);
01005
01006 getRosUint("gnss/qzss_sig_cfg", qzss_sig_cfg_,
01007 ublox_msgs::CfgGNSS_Block::SIG_CFG_QZSS_L1CA);
01008
01009 if (enable_gps_ && !supportsGnss("GPS"))
01010 ROS_WARN("gnss/gps is true, but GPS GNSS is not supported by %s",
01011 "this device");
01012 if (enable_glonass_ && !supportsGnss("GLO"))
01013 ROS_WARN("gnss/glonass is true, but GLONASS is not supported by %s",
01014 "this device");
01015 if (enable_galileo_ && !supportsGnss("GAL"))
01016 ROS_WARN("gnss/galileo is true, but Galileo GNSS is not supported %s",
01017 "by this device");
01018 if (enable_beidou_ && !supportsGnss("BDS"))
01019 ROS_WARN("gnss/beidou is true, but Beidou GNSS is not supported %s",
01020 "by this device");
01021 if (enable_imes_ && !supportsGnss("IMES"))
01022 ROS_WARN("gnss/imes is true, but IMES GNSS is not supported by %s",
01023 "this device");
01024 if (enable_qzss_ && !supportsGnss("QZSS"))
01025 ROS_WARN("gnss/qzss is true, but QZSS is not supported by this device");
01026 if (enable_sbas_ && !supportsGnss("SBAS"))
01027 ROS_WARN("gnss/sbas is true, but SBAS is not supported by this device");
01028
01029
01030 fix_status_service = sensor_msgs::NavSatStatus::SERVICE_GPS
01031 + (enable_glonass_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GLONASS
01032 + (enable_beidou_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_COMPASS
01033 + (enable_galileo_ ? 1 : 0) * sensor_msgs::NavSatStatus::SERVICE_GALILEO;
01034
01035
01036
01037
01038 nh->param("nmea/set", set_nmea_, false);
01039 if (set_nmea_) {
01040 bool compat, consider;
01041 cfg_nmea_.version = cfg_nmea_.VERSION;
01042
01043
01044 if (!getRosUint("nmea/version", cfg_nmea_.nmeaVersion))
01045 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
01046 "true, therefore nmea/version must be set");
01047 if (!getRosUint("nmea/num_sv", cfg_nmea_.numSV))
01048 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
01049 "true, therefore nmea/num_sv must be set");
01050 if (!getRosUint("nmea/sv_numbering", cfg_nmea_.svNumbering))
01051 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
01052 "true, therefore nmea/sv_numbering must be set");
01053 if (!nh->getParam("nmea/compat", compat))
01054 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
01055 "true, therefore nmea/compat must be set");
01056 if (!nh->getParam("nmea/consider", consider))
01057 throw std::runtime_error(std::string("Invalid settings: nmea/set is ") +
01058 "true, therefore nmea/consider must be set");
01059
01060
01061 cfg_nmea_.flags = compat ? cfg_nmea_.FLAGS_COMPAT : 0;
01062 cfg_nmea_.flags |= consider ? cfg_nmea_.FLAGS_CONSIDER : 0;
01063 bool temp;
01064 nh->param("nmea/limit82", temp, false);
01065 cfg_nmea_.flags |= temp ? cfg_nmea_.FLAGS_LIMIT82 : 0;
01066 nh->param("nmea/high_prec", temp, false);
01067 cfg_nmea_.flags |= temp ? cfg_nmea_.FLAGS_HIGH_PREC : 0;
01068
01069 nh->param("nmea/filter/pos", temp, false);
01070 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_POS : 0;
01071 nh->param("nmea/filter/msk_pos", temp, false);
01072 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_MSK_POS : 0;
01073 nh->param("nmea/filter/time", temp, false);
01074 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TIME : 0;
01075 nh->param("nmea/filter/date", temp, false);
01076 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_DATE : 0;
01077 nh->param("nmea/filter/gps_only", temp, false);
01078 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_GPS_ONLY : 0;
01079 nh->param("nmea/filter/track", temp, false);
01080 cfg_nmea_.filter |= temp ? cfg_nmea_.FILTER_TRACK : 0;
01081
01082 nh->param("nmea/gnssToFilter/gps", temp, false);
01083 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GPS : 0;
01084 nh->param("nmea/gnssToFilter/sbas", temp, false);
01085 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_SBAS : 0;
01086 nh->param("nmea/gnssToFilter/qzss", temp, false);
01087 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_QZSS : 0;
01088 nh->param("nmea/gnssToFilter/glonass", temp, false);
01089 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_GLONASS : 0;
01090 nh->param("nmea/gnssToFilter/beidou", temp, false);
01091 cfg_nmea_.gnssToFilter |= temp ? cfg_nmea_.GNSS_TO_FILTER_BEIDOU : 0;
01092
01093 getRosUint("nmea/main_talker_id", cfg_nmea_.mainTalkerId);
01094 getRosUint("nmea/gsv_talker_id", cfg_nmea_.gsvTalkerId);
01095
01096 std::vector<uint8_t> bdsTalkerId;
01097 getRosUint("nmea/bds_talker_id", bdsTalkerId);
01098 cfg_nmea_.bdsTalkerId[0] = bdsTalkerId[0];
01099 cfg_nmea_.bdsTalkerId[1] = bdsTalkerId[1];
01100 }
01101 }
01102
01103
01104 bool UbloxFirmware8::configureUblox() {
01105 if(clear_bbr_) {
01106
01107 if(!gps.clearBbr())
01108 ROS_ERROR("u-blox failed to clear flash memory");
01109 }
01110
01111
01112
01113
01114 ublox_msgs::CfgGNSS cfg_gnss;
01115 if (gps.poll(cfg_gnss)) {
01116 ROS_DEBUG("Read GNSS config.");
01117 ROS_DEBUG("Num. tracking channels in hardware: %i", cfg_gnss.numTrkChHw);
01118 ROS_DEBUG("Num. tracking channels to use: %i", cfg_gnss.numTrkChUse);
01119 } else {
01120 throw std::runtime_error("Failed to read the GNSS config.");
01121 }
01122
01123
01124 bool correct = true;
01125 for (int i = 0; i < cfg_gnss.blocks.size(); i++) {
01126 ublox_msgs::CfgGNSS_Block block = cfg_gnss.blocks[i];
01127 if (block.gnssId == block.GNSS_ID_GPS
01128 && enable_gps_ != (block.flags & block.FLAGS_ENABLE)) {
01129 correct = false;
01130 cfg_gnss.blocks[i].flags =
01131 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_gps_;
01132 ROS_DEBUG("GPS Configuration is different");
01133 } else if (block.gnssId == block.GNSS_ID_SBAS
01134 && enable_sbas_ != (block.flags & block.FLAGS_ENABLE)) {
01135 correct = false;
01136 cfg_gnss.blocks[i].flags =
01137 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_sbas_;
01138 ROS_DEBUG("SBAS Configuration is different");
01139 } else if (block.gnssId == block.GNSS_ID_GALILEO
01140 && enable_galileo_ != (block.flags & block.FLAGS_ENABLE)) {
01141 correct = false;
01142 cfg_gnss.blocks[i].flags =
01143 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_galileo_;
01144 ROS_DEBUG("Galileo GNSS Configuration is different");
01145 } else if (block.gnssId == block.GNSS_ID_BEIDOU
01146 && enable_beidou_ != (block.flags & block.FLAGS_ENABLE)) {
01147 correct = false;
01148 cfg_gnss.blocks[i].flags =
01149 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_beidou_;
01150 ROS_DEBUG("BeiDou Configuration is different");
01151 } else if (block.gnssId == block.GNSS_ID_IMES
01152 && enable_imes_ != (block.flags & block.FLAGS_ENABLE)) {
01153 correct = false;
01154 cfg_gnss.blocks[i].flags =
01155 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_imes_;
01156 } else if (block.gnssId == block.GNSS_ID_QZSS
01157 && (enable_qzss_ != (block.flags & block.FLAGS_ENABLE)
01158 || (enable_qzss_
01159 && qzss_sig_cfg_ != (block.flags & block.FLAGS_SIG_CFG_MASK)))) {
01160 ROS_DEBUG("QZSS Configuration is different %u, %u",
01161 block.flags & block.FLAGS_ENABLE,
01162 enable_qzss_);
01163 correct = false;
01164 ROS_DEBUG("QZSS Configuration: %u", block.flags);
01165 cfg_gnss.blocks[i].flags =
01166 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_qzss_;
01167 ROS_DEBUG("QZSS Configuration: %u", cfg_gnss.blocks[i].flags);
01168 if (enable_qzss_)
01169
01170 cfg_gnss.blocks[i].flags |= qzss_sig_cfg_;
01171 } else if (block.gnssId == block.GNSS_ID_GLONASS
01172 && enable_glonass_ != (block.flags & block.FLAGS_ENABLE)) {
01173 correct = false;
01174 cfg_gnss.blocks[i].flags =
01175 (cfg_gnss.blocks[i].flags & ~block.FLAGS_ENABLE) | enable_glonass_;
01176 ROS_DEBUG("GLONASS Configuration is different");
01177 }
01178 }
01179
01180
01181
01182 if (correct)
01183 ROS_DEBUG("U-Blox GNSS configuration is correct. GNSS not re-configured.");
01184 else if (!gps.configGnss(cfg_gnss, boost::posix_time::seconds(15)))
01185 throw std::runtime_error(std::string("Failed to cold reset device ") +
01186 "after configuring GNSS");
01187
01188
01189
01190
01191 if (set_nmea_ && !gps.configure(cfg_nmea_))
01192 throw std::runtime_error("Failed to configure NMEA");
01193
01194 return true;
01195 }
01196
01197 void UbloxFirmware8::subscribe() {
01198
01199 nh->param("publish/nav/pvt", enabled["nav_pvt"], enabled["nav"]);
01200
01201 gps.subscribe<ublox_msgs::NavPVT>(
01202 boost::bind(&UbloxFirmware7Plus::callbackNavPvt, this, _1), kSubscribeRate);
01203
01204
01205 nh->param("publish/nav/sat", enabled["nav_sat"], enabled["nav"]);
01206 if (enabled["nav_sat"])
01207 gps.subscribe<ublox_msgs::NavSAT>(boost::bind(
01208 publish<ublox_msgs::NavSAT>, _1, "navsat"), kNavSvInfoSubscribeRate);
01209
01210
01211 nh->param("publish/mon/hw", enabled["mon_hw"], enabled["mon"]);
01212 if (enabled["mon_hw"])
01213 gps.subscribe<ublox_msgs::MonHW>(boost::bind(
01214 publish<ublox_msgs::MonHW>, _1, "monhw"), kSubscribeRate);
01215
01216
01217 nh->param("publish/rxm/rtcm", enabled["rxm_rtcm"], enabled["rxm"]);
01218 if (enabled["rxm_rtcm"])
01219 gps.subscribe<ublox_msgs::RxmRTCM>(boost::bind(
01220 publish<ublox_msgs::RxmRTCM>, _1, "rxmrtcm"), kSubscribeRate);
01221 }
01222
01223
01224
01225
01226 void RawDataProduct::subscribe() {
01227
01228 nh->param("publish/rxm/all", enabled["rxm"], true);
01229
01230
01231 nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]);
01232 if (enabled["rxm_raw"])
01233 gps.subscribe<ublox_msgs::RxmRAW>(boost::bind(
01234 publish<ublox_msgs::RxmRAW>, _1, "rxmraw"), kSubscribeRate);
01235
01236
01237 nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]);
01238 if (enabled["rxm_sfrb"])
01239 gps.subscribe<ublox_msgs::RxmSFRB>(boost::bind(
01240 publish<ublox_msgs::RxmSFRB>, _1, "rxmsfrb"), kSubscribeRate);
01241
01242
01243 nh->param("publish/rxm/eph", enabled["rxm_eph"], enabled["rxm"]);
01244 if (enabled["rxm_eph"])
01245 gps.subscribe<ublox_msgs::RxmEPH>(boost::bind(
01246 publish<ublox_msgs::RxmEPH>, _1, "rxmeph"), kSubscribeRate);
01247
01248
01249 nh->param("publish/rxm/almRaw", enabled["rxm_alm"], enabled["rxm"]);
01250 if (enabled["rxm_alm"])
01251 gps.subscribe<ublox_msgs::RxmALM>(boost::bind(
01252 publish<ublox_msgs::RxmALM>, _1, "rxmalm"), kSubscribeRate);
01253 }
01254
01255 void RawDataProduct::initializeRosDiagnostics() {
01256 if (enabled["rxm_raw"])
01257 freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmraw", kRtcmFreqTol,
01258 kRtcmFreqWindow));
01259 if (enabled["rxm_sfrb"])
01260 freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmsfrb", kRtcmFreqTol,
01261 kRtcmFreqWindow));
01262 if (enabled["rxm_eph"])
01263 freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmeph", kRtcmFreqTol,
01264 kRtcmFreqWindow));
01265 if (enabled["rxm_alm"])
01266 freq_diagnostics_.push_back(UbloxTopicDiagnostic("rxmalm", kRtcmFreqTol,
01267 kRtcmFreqWindow));
01268 }
01269
01270
01271
01272
01273 void AdrUdrProduct::getRosParams() {
01274 nh->param("use_adr", use_adr_, true);
01275
01276 float nav_rate_hz = 1000 / (meas_rate * nav_rate);
01277 if(nav_rate_hz != 1)
01278 ROS_WARN("Nav Rate recommended to be 1 Hz");
01279 }
01280
01281 bool AdrUdrProduct::configureUblox() {
01282 if(!gps.setUseAdr(use_adr_))
01283 throw std::runtime_error(std::string("Failed to ")
01284 + (use_adr_ ? "enable" : "disable") + "use_adr");
01285 return true;
01286 }
01287
01288 void AdrUdrProduct::subscribe() {
01289 nh->param("publish/esf/all", enabled["esf"], true);
01290
01291
01292 nh->param("publish/nav/att", enabled["nav_att"], enabled["nav"]);
01293 if (enabled["nav_att"])
01294 gps.subscribe<ublox_msgs::NavATT>(boost::bind(
01295 publish<ublox_msgs::NavATT>, _1, "navatt"), kSubscribeRate);
01296
01297
01298 nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]);
01299 if (enabled["esf_ins"])
01300 gps.subscribe<ublox_msgs::EsfINS>(boost::bind(
01301 publish<ublox_msgs::EsfINS>, _1, "esfins"), kSubscribeRate);
01302
01303
01304 nh->param("publish/esf/meas", enabled["esf_meas"], enabled["esf"]);
01305 if (enabled["esf_meas"])
01306 gps.subscribe<ublox_msgs::EsfMEAS>(boost::bind(
01307 publish<ublox_msgs::EsfMEAS>, _1, "esfmeas"), kSubscribeRate);
01308
01309 gps.subscribe<ublox_msgs::EsfMEAS>(boost::bind(
01310 &AdrUdrProduct::callbackEsfMEAS, this, _1), kSubscribeRate);
01311
01312
01313 nh->param("publish/esf/raw", enabled["esf_raw"], enabled["esf"]);
01314 if (enabled["esf_raw"])
01315 gps.subscribe<ublox_msgs::EsfRAW>(boost::bind(
01316 publish<ublox_msgs::EsfRAW>, _1, "esfraw"), kSubscribeRate);
01317
01318
01319 nh->param("publish/esf/status", enabled["esf_status"], enabled["esf"]);
01320 if (enabled["esf_status"])
01321 gps.subscribe<ublox_msgs::EsfSTATUS>(boost::bind(
01322 publish<ublox_msgs::EsfSTATUS>, _1, "esfstatus"), kSubscribeRate);
01323
01324
01325 nh->param("publish/hnr/pvt", enabled["hnr_pvt"], true);
01326 if (enabled["hnr_pvt"])
01327 gps.subscribe<ublox_msgs::HnrPVT>(boost::bind(
01328 publish<ublox_msgs::HnrPVT>, _1, "hnrpvt"), kSubscribeRate);
01329 }
01330
01331 void AdrUdrProduct::callbackEsfMEAS(const ublox_msgs::EsfMEAS &m) {
01332 if (enabled["esf_meas"]) {
01333 static ros::Publisher imu_pub =
01334 nh->advertise<sensor_msgs::Imu>("imu_meas", kROSQueueSize);
01335 static ros::Publisher time_ref_pub =
01336 nh->advertise<sensor_msgs::TimeReference>("interrupt_time", kROSQueueSize);
01337
01338 imu_.header.stamp = ros::Time::now();
01339 imu_.header.frame_id = frame_id;
01340
01341 float deg_per_sec = pow(2, -12);
01342 float m_per_sec_sq = pow(2, -10);
01343 float deg_c = 1e-2;
01344
01345 std::vector<unsigned int> imu_data = m.data;
01346 for (int i=0; i < imu_data.size(); i++){
01347 unsigned int data_type = imu_data[i] >> 24;
01348 double data_sign = (imu_data[i] & (1 << 23));
01349 unsigned int data_value = imu_data[i] & 0x7FFFFF;
01350
01351 if (data_sign == 0) {
01352 data_sign = -1;
01353 } else {
01354 data_sign = 1;
01355 }
01356
01357
01358
01359 imu_.orientation_covariance[0] = -1;
01360 imu_.linear_acceleration_covariance[0] = -1;
01361 imu_.angular_velocity_covariance[0] = -1;
01362
01363 if (data_type == 14) {
01364 if (data_sign == 1) {
01365 imu_.angular_velocity.x = 2048 - data_value * deg_per_sec;
01366 } else {
01367 imu_.angular_velocity.x = data_sign * data_value * deg_per_sec;
01368 }
01369 } else if (data_type == 16) {
01370
01371
01372 if (data_sign == 1) {
01373 imu_.linear_acceleration.x = 8191 - data_value * m_per_sec_sq;
01374 } else {
01375 imu_.linear_acceleration.x = data_sign * data_value * m_per_sec_sq;
01376 }
01377 } else if (data_type == 13) {
01378 if (data_sign == 1) {
01379 imu_.angular_velocity.y = 2048 - data_value * deg_per_sec;
01380 } else {
01381 imu_.angular_velocity.y = data_sign * data_value * deg_per_sec;
01382 }
01383 } else if (data_type == 17) {
01384 if (data_sign == 1) {
01385 imu_.linear_acceleration.y = 8191 - data_value * m_per_sec_sq;
01386 } else {
01387 imu_.linear_acceleration.y = data_sign * data_value * m_per_sec_sq;
01388 }
01389 } else if (data_type == 5) {
01390 if (data_sign == 1) {
01391 imu_.angular_velocity.z = 2048 - data_value * deg_per_sec;
01392 } else {
01393 imu_.angular_velocity.z = data_sign * data_value * deg_per_sec;
01394 }
01395 } else if (data_type == 18) {
01396 if (data_sign == 1) {
01397 imu_.linear_acceleration.z = 8191 - data_value * m_per_sec_sq;
01398 } else {
01399 imu_.linear_acceleration.z = data_sign * data_value * m_per_sec_sq;
01400 }
01401 } else if (data_type == 12) {
01402
01403 } else {
01404 ROS_INFO("data_type: %u", data_type);
01405 ROS_INFO("data_value: %u", data_value);
01406 }
01407
01408
01409
01410
01411
01412
01413
01414
01415
01416
01417
01418
01419 t_ref_.header.stamp = ros::Time::now();
01420 t_ref_.header.frame_id = frame_id;
01421
01422 time_ref_pub.publish(t_ref_);
01423 imu_pub.publish(imu_);
01424 }
01425 }
01426
01427 updater->force_update();
01428 }
01429
01430
01431
01432 void HpgRefProduct::getRosParams() {
01433 if (config_on_startup_flag_) {
01434 if(nav_rate * meas_rate != 1000)
01435 ROS_WARN("For HPG Ref devices, nav_rate should be exactly 1 Hz.");
01436
01437 if(!getRosUint("tmode3", tmode3_))
01438 throw std::runtime_error("Invalid settings: TMODE3 must be set");
01439
01440 if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) {
01441 if(!nh->getParam("arp/position", arp_position_))
01442 throw std::runtime_error(std::string("Invalid settings: arp/position ")
01443 + "must be set if TMODE3 is fixed");
01444 if(!getRosInt("arp/position_hp", arp_position_hp_))
01445 throw std::runtime_error(std::string("Invalid settings: arp/position_hp ")
01446 + "must be set if TMODE3 is fixed");
01447 if(!nh->getParam("arp/acc", fixed_pos_acc_))
01448 throw std::runtime_error(std::string("Invalid settings: arp/acc ")
01449 + "must be set if TMODE3 is fixed");
01450 if(!nh->getParam("arp/lla_flag", lla_flag_)) {
01451 ROS_WARN("arp/lla_flag param not set, assuming ARP coordinates are %s",
01452 "in ECEF");
01453 lla_flag_ = false;
01454 }
01455 } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) {
01456 nh->param("sv_in/reset", svin_reset_, true);
01457 if(!getRosUint("sv_in/min_dur", sv_in_min_dur_))
01458 throw std::runtime_error(std::string("Invalid settings: sv_in/min_dur ")
01459 + "must be set if TMODE3 is survey-in");
01460 if(!nh->getParam("sv_in/acc_lim", sv_in_acc_lim_))
01461 throw std::runtime_error(std::string("Invalid settings: sv_in/acc_lim ")
01462 + "must be set if TMODE3 is survey-in");
01463 } else if(tmode3_ != ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) {
01464 throw std::runtime_error(std::string("tmode3 param invalid. See CfgTMODE3")
01465 + " flag constants for possible values.");
01466 }
01467 }
01468 }
01469
01470 bool HpgRefProduct::configureUblox() {
01471
01472 if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_DISABLED) {
01473 if(!gps.disableTmode3())
01474 throw std::runtime_error("Failed to disable TMODE3.");
01475 mode_ = DISABLED;
01476 } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_FIXED) {
01477 if(!gps.configTmode3Fixed(lla_flag_, arp_position_, arp_position_hp_,
01478 fixed_pos_acc_))
01479 throw std::runtime_error("Failed to set TMODE3 to fixed.");
01480 if(!gps.configRtcm(rtcm_ids, rtcm_rates))
01481 throw std::runtime_error("Failed to set RTCM rates");
01482 mode_ = FIXED;
01483 } else if(tmode3_ == ublox_msgs::CfgTMODE3::FLAGS_MODE_SURVEY_IN) {
01484 if(!svin_reset_) {
01485 ublox_msgs::NavSVIN nav_svin;
01486 if(!gps.poll(nav_svin))
01487 throw std::runtime_error(std::string("Failed to poll NavSVIN while") +
01488 " configuring survey-in");
01489
01490 if(nav_svin.active) {
01491 mode_ = SURVEY_IN;
01492 return true;
01493 }
01494
01495 if(nav_svin.valid) {
01496 setTimeMode();
01497 return true;
01498 }
01499 ublox_msgs::NavPVT nav_pvt;
01500 if(!gps.poll(nav_pvt))
01501 throw std::runtime_error(std::string("Failed to poll NavPVT while") +
01502 " configuring survey-in");
01503
01504 if (nav_pvt.fixType == nav_pvt.FIX_TYPE_TIME_ONLY
01505 && nav_pvt.flags & nav_pvt.FLAGS_GNSS_FIX_OK) {
01506 setTimeMode();
01507 return true;
01508 }
01509 }
01510
01511
01512 uint16_t meas_rate_temp = meas_rate < 1000 ? meas_rate : 1000;
01513
01514 if(1000 % meas_rate_temp != 0)
01515 meas_rate_temp = kDefaultMeasPeriod;
01516
01517 if(!gps.configRate(meas_rate_temp, (int) 1000 / meas_rate_temp))
01518 throw std::runtime_error(std::string("Failed to set nav rate to 1 Hz") +
01519 "before setting TMODE3 to survey-in.");
01520
01521 if(!gps.disableTmode3())
01522 ROS_ERROR("Failed to disable TMODE3 before setting to survey-in.");
01523 else
01524 mode_ = DISABLED;
01525
01526 if(!gps.configTmode3SurveyIn(sv_in_min_dur_, sv_in_acc_lim_))
01527 throw std::runtime_error("Failed to set TMODE3 to survey-in.");
01528 mode_ = SURVEY_IN;
01529 }
01530 return true;
01531 }
01532
01533 void HpgRefProduct::subscribe() {
01534
01535 nh->param("publish/nav/svin", enabled["nav_svin"], enabled["nav"]);
01536
01537 gps.subscribe<ublox_msgs::NavSVIN>(boost::bind(
01538 &HpgRefProduct::callbackNavSvIn, this, _1), kSubscribeRate);
01539 }
01540
01541 void HpgRefProduct::callbackNavSvIn(ublox_msgs::NavSVIN m) {
01542 if(enabled["nav_svin"]) {
01543 static ros::Publisher publisher =
01544 nh->advertise<ublox_msgs::NavSVIN>("navsvin", kROSQueueSize);
01545 publisher.publish(m);
01546 }
01547
01548 last_nav_svin_ = m;
01549
01550 if(!m.active && m.valid && mode_ == SURVEY_IN) {
01551 setTimeMode();
01552 }
01553
01554 updater->update();
01555 }
01556
01557 bool HpgRefProduct::setTimeMode() {
01558 ROS_INFO("Setting mode (internal state) to Time Mode");
01559 mode_ = TIME;
01560
01561
01562
01563 if(!gps.configRate(meas_rate, nav_rate))
01564 ROS_ERROR("Failed to set measurement rate to %d ms %s %d", meas_rate,
01565 "navigation rate to ", nav_rate);
01566
01567 if(!gps.configRtcm(rtcm_ids, rtcm_rates)) {
01568 ROS_ERROR("Failed to configure RTCM IDs");
01569 return false;
01570 }
01571 return true;
01572 }
01573
01574 void HpgRefProduct::initializeRosDiagnostics() {
01575 updater->add("TMODE3", this, &HpgRefProduct::tmode3Diagnostics);
01576 updater->force_update();
01577 }
01578
01579 void HpgRefProduct::tmode3Diagnostics(
01580 diagnostic_updater::DiagnosticStatusWrapper& stat) {
01581 if (mode_ == INIT) {
01582 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
01583 stat.message = "Not configured";
01584 } else if (mode_ == DISABLED){
01585 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
01586 stat.message = "Disabled";
01587 } else if (mode_ == SURVEY_IN) {
01588 if (!last_nav_svin_.active && !last_nav_svin_.valid) {
01589 stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
01590 stat.message = "Survey-In inactive and invalid";
01591 } else if (last_nav_svin_.active && !last_nav_svin_.valid) {
01592 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
01593 stat.message = "Survey-In active but invalid";
01594 } else if (!last_nav_svin_.active && last_nav_svin_.valid) {
01595 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
01596 stat.message = "Survey-In complete";
01597 } else if (last_nav_svin_.active && last_nav_svin_.valid) {
01598 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
01599 stat.message = "Survey-In active and valid";
01600 }
01601
01602 stat.add("iTOW [ms]", last_nav_svin_.iTOW);
01603 stat.add("Duration [s]", last_nav_svin_.dur);
01604 stat.add("# observations", last_nav_svin_.obs);
01605 stat.add("Mean X [m]", last_nav_svin_.meanX * 1e-2);
01606 stat.add("Mean Y [m]", last_nav_svin_.meanY * 1e-2);
01607 stat.add("Mean Z [m]", last_nav_svin_.meanZ * 1e-2);
01608 stat.add("Mean X HP [m]", last_nav_svin_.meanXHP * 1e-4);
01609 stat.add("Mean Y HP [m]", last_nav_svin_.meanYHP * 1e-4);
01610 stat.add("Mean Z HP [m]", last_nav_svin_.meanZHP * 1e-4);
01611 stat.add("Mean Accuracy [m]", last_nav_svin_.meanAcc * 1e-4);
01612 } else if(mode_ == FIXED) {
01613 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
01614 stat.message = "Fixed Position";
01615 } else if(mode_ == TIME) {
01616 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
01617 stat.message = "Time";
01618 }
01619 }
01620
01621
01622
01623
01624 void HpgRovProduct::getRosParams() {
01625
01626 getRosUint("dgnss_mode", dgnss_mode_,
01627 ublox_msgs::CfgDGNSS::DGNSS_MODE_RTK_FIXED);
01628 }
01629
01630 bool HpgRovProduct::configureUblox() {
01631
01632 if(!gps.setDgnss(dgnss_mode_))
01633 throw std::runtime_error(std::string("Failed to Configure DGNSS"));
01634 return true;
01635 }
01636
01637 void HpgRovProduct::subscribe() {
01638
01639 nh->param("publish/nav/relposned", enabled["nav_relposned"], enabled["nav"]);
01640
01641 gps.subscribe<ublox_msgs::NavRELPOSNED>(boost::bind(
01642 &HpgRovProduct::callbackNavRelPosNed, this, _1), kSubscribeRate);
01643 }
01644
01645 void HpgRovProduct::initializeRosDiagnostics() {
01646 freq_rtcm_ = UbloxTopicDiagnostic(std::string("rxmrtcm"),
01647 kRtcmFreqMin, kRtcmFreqMax,
01648 kRtcmFreqTol, kRtcmFreqWindow);
01649 updater->add("Carrier Phase Solution", this,
01650 &HpgRovProduct::carrierPhaseDiagnostics);
01651 updater->force_update();
01652 }
01653
01654 void HpgRovProduct::carrierPhaseDiagnostics(
01655 diagnostic_updater::DiagnosticStatusWrapper& stat) {
01656 uint32_t carr_soln = last_rel_pos_.flags & last_rel_pos_.FLAGS_CARR_SOLN_MASK;
01657 stat.add("iTow", last_rel_pos_.iTow);
01658 if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_NONE ||
01659 !(last_rel_pos_.flags & last_rel_pos_.FLAGS_DIFF_SOLN &&
01660 last_rel_pos_.flags & last_rel_pos_.FLAGS_REL_POS_VALID)) {
01661 stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
01662 stat.message = "None";
01663 } else {
01664 if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FLOAT) {
01665 stat.level = diagnostic_msgs::DiagnosticStatus::WARN;
01666 stat.message = "Float";
01667 } else if (carr_soln & last_rel_pos_.FLAGS_CARR_SOLN_FIXED) {
01668 stat.level = diagnostic_msgs::DiagnosticStatus::OK;
01669 stat.message = "Fixed";
01670 }
01671 stat.add("Ref Station ID", last_rel_pos_.refStationId);
01672
01673 double rel_pos_n = (last_rel_pos_.relPosN
01674 + (last_rel_pos_.relPosHPN * 1e-2)) * 1e-2;
01675 double rel_pos_e = (last_rel_pos_.relPosE
01676 + (last_rel_pos_.relPosHPE * 1e-2)) * 1e-2;
01677 double rel_pos_d = (last_rel_pos_.relPosD
01678 + (last_rel_pos_.relPosHPD * 1e-2)) * 1e-2;
01679 stat.add("Relative Position N [m]", rel_pos_n);
01680 stat.add("Relative Accuracy N [m]", last_rel_pos_.accN * 1e-4);
01681 stat.add("Relative Position E [m]", rel_pos_e);
01682 stat.add("Relative Accuracy E [m]", last_rel_pos_.accE * 1e-4);
01683 stat.add("Relative Position D [m]", rel_pos_d);
01684 stat.add("Relative Accuracy D [m]", last_rel_pos_.accD * 1e-4);
01685 }
01686 }
01687
01688 void HpgRovProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED &m) {
01689 if (enabled["nav_relposned"]) {
01690 static ros::Publisher publisher =
01691 nh->advertise<ublox_msgs::NavRELPOSNED>("navrelposned", kROSQueueSize);
01692 publisher.publish(m);
01693 }
01694
01695 last_rel_pos_ = m;
01696 updater->update();
01697 }
01698
01699
01700
01701
01702 void TimProduct::getRosParams() {
01703 }
01704
01705 bool TimProduct::configureUblox() {
01706 uint8_t r = 1;
01707
01708 if(!gps.setUTCtime())
01709 throw std::runtime_error(std::string("Failed to Configure TIM Product to UTC Time"));
01710
01711 if(!gps.setTimtm2(r))
01712 throw std::runtime_error(std::string("Failed to Configure TIM Product"));
01713
01714 return true;
01715 }
01716
01717 void TimProduct::subscribe() {
01718 ROS_INFO("TIM is Enabled: %u", enabled["tim"]);
01719 ROS_INFO("TIM-TM2 is Enabled: %u", enabled["tim_tm2"]);
01720
01721 nh->param("publish/tim/tm2", enabled["tim_tm2"], enabled["tim"]);
01722
01723 gps.subscribe<ublox_msgs::TimTM2>(boost::bind(
01724 &TimProduct::callbackTimTM2, this, _1), kSubscribeRate);
01725
01726 ROS_INFO("Subscribed to TIM-TM2 messages on topic tim/tm2");
01727
01728
01729 nh->param("publish/rxm/sfrb", enabled["rxm_sfrb"], enabled["rxm"]);
01730 if (enabled["rxm_sfrb"])
01731 gps.subscribe<ublox_msgs::RxmSFRBX>(boost::bind(
01732 publish<ublox_msgs::RxmSFRBX>, _1, "rxmsfrb"), kSubscribeRate);
01733
01734
01735 nh->param("publish/rxm/raw", enabled["rxm_raw"], enabled["rxm"]);
01736 if (enabled["rxm_raw"])
01737 gps.subscribe<ublox_msgs::RxmRAWX>(boost::bind(
01738 publish<ublox_msgs::RxmRAWX>, _1, "rxmraw"), kSubscribeRate);
01739 }
01740
01741 void TimProduct::callbackTimTM2(const ublox_msgs::TimTM2 &m) {
01742
01743 if (enabled["tim_tm2"]) {
01744 static ros::Publisher publisher =
01745 nh->advertise<ublox_msgs::TimTM2>("timtm2", kROSQueueSize);
01746 static ros::Publisher time_ref_pub =
01747 nh->advertise<sensor_msgs::TimeReference>("interrupt_time", kROSQueueSize);
01748
01749
01750 t_ref_.header.seq = m.risingEdgeCount;
01751 t_ref_.header.stamp = ros::Time::now();
01752 t_ref_.header.frame_id = frame_id;
01753
01754 t_ref_.time_ref = ros::Time((m.wnR * 604800 + m.towMsR / 1000), (m.towMsR % 1000) * 1000000 + m.towSubMsR);
01755
01756 std::ostringstream src;
01757 src << "TIM" << int(m.ch);
01758 t_ref_.source = src.str();
01759
01760 t_ref_.header.stamp = ros::Time::now();
01761 t_ref_.header.frame_id = frame_id;
01762
01763 publisher.publish(m);
01764 time_ref_pub.publish(t_ref_);
01765 }
01766
01767 updater->force_update();
01768 }
01769
01770 void TimProduct::initializeRosDiagnostics() {
01771 updater->force_update();
01772 }
01773
01774 int main(int argc, char** argv) {
01775 ros::init(argc, argv, "ublox_gps");
01776 nh.reset(new ros::NodeHandle("~"));
01777 nh->param("debug", ublox_gps::debug, 1);
01778 if(ublox_gps::debug) {
01779 if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
01780 ros::console::levels::Debug))
01781 ros::console::notifyLoggerLevelsChanged();
01782
01783 }
01784 UbloxNode node;
01785 return 0;
01786 }