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;
}
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;
}