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 #include <ublox_gps/gps.h>
00030 #include <boost/asio/serial_port.hpp>
00031 #include <boost/asio/ip/tcp.hpp>
00032
00033 #include <boost/regex.hpp>
00034
00035 #include <ros/ros.h>
00036 #include <ublox_msgs/ublox_msgs.h>
00037
00038 #include <sensor_msgs/NavSatFix.h>
00039
00040 #include <geometry_msgs/Vector3Stamped.h>
00041
00042 using namespace ublox_gps;
00043
00044 ros::NodeHandle *nh;
00045 Gps gps;
00046 ublox_msgs::NavSTATUS status;
00047 std::map<std::string,bool> enabled;
00048 std::string frame_id;
00049
00050 sensor_msgs::NavSatFix fix;
00051 geometry_msgs::Vector3Stamped velocity;
00052
00053 void publishNavStatus(const ublox_msgs::NavSTATUS& m)
00054 {
00055 static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSTATUS>("ublox/navstatus", 10);
00056 publisher.publish(m);
00057
00058 status = m;
00059 }
00060
00061 void publishNavSOL(const ublox_msgs::NavSOL& m)
00062 {
00063 static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSOL>("ublox/navsol", 10);
00064 publisher.publish(m);
00065 }
00066
00067 void publishNavVelNED(const ublox_msgs::NavVELNED& m)
00068 {
00069 static ros::Publisher publisher = nh->advertise<ublox_msgs::NavVELNED>("ublox/navvelned", 10);
00070 publisher.publish(m);
00071
00072
00073 static ros::Publisher velocityPublisher = nh->advertise<geometry_msgs::Vector3Stamped>("fix_velocity",10);
00074 velocity.header.stamp = fix.header.stamp.isZero() ? ros::Time::now() : fix.header.stamp;
00075 velocity.header.frame_id = frame_id;
00076 velocity.vector.x = m.velN/100.0;
00077 velocity.vector.y = -m.velE/100.0;
00078 velocity.vector.z = -m.velD/100.0;
00079 velocityPublisher.publish(velocity);
00080
00081 }
00082
00083 void publishNavPosLLH(const ublox_msgs::NavPOSLLH& m)
00084 {
00085 static ros::Publisher publisher = nh->advertise<ublox_msgs::NavPOSLLH>("ublox/navposllh", 10);
00086 publisher.publish(m);
00087
00088
00089 static ros::Publisher fixPublisher = nh->advertise<sensor_msgs::NavSatFix>("fix", 10);
00090 fix.header.stamp = ros::Time::now();
00091 fix.header.frame_id = frame_id;
00092 fix.latitude = m.lat*1e-7;
00093 fix.longitude = m.lon*1e-7;
00094 fix.altitude = m.height*1e-3;
00095 if (status.gpsFix >= status.GPS_3D_FIX)
00096 fix.status.status = fix.status.STATUS_FIX;
00097 else
00098 fix.status.status = fix.status.STATUS_NO_FIX;
00099
00100 fix.status.service = fix.status.SERVICE_GPS;
00101 fixPublisher.publish(fix);
00102
00103 }
00104
00105 void publishNavSVINFO(const ublox_msgs::NavSVINFO& m)
00106 {
00107 static ros::Publisher publisher = nh->advertise<ublox_msgs::NavSVINFO>("ublox/navsvinfo", 10);
00108 publisher.publish(m);
00109 }
00110
00111 void publishNavCLK(const ublox_msgs::NavCLOCK& m)
00112 {
00113 static ros::Publisher publisher = nh->advertise<ublox_msgs::NavCLOCK>("ublox/navclock", 10);
00114 publisher.publish(m);
00115 }
00116
00117 void publishRxmRAW(const ublox_msgs::RxmRAW& m)
00118 {
00119 static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmRAW>("ublox/rxmraw", 10);
00120 publisher.publish(m);
00121 }
00122
00123 void publishRxmSFRB(const ublox_msgs::RxmSFRB& m)
00124 {
00125 static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmSFRB>("ublox/rxmsfrb", 10);
00126 publisher.publish(m);
00127 }
00128
00129 void publishRxmALM(const ublox_msgs::RxmALM& m)
00130 {
00131 static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmALM>("ublox/rxmalm", 10);
00132 publisher.publish(m);
00133 }
00134
00135 void publishRxmEPH(const ublox_msgs::RxmEPH& m)
00136 {
00137 static ros::Publisher publisher = nh->advertise<ublox_msgs::RxmEPH>("ublox/rxmeph", 10);
00138 publisher.publish(m);
00139 }
00140
00141 void publishAidALM(const ublox_msgs::AidALM& m)
00142 {
00143 static ros::Publisher publisher = nh->advertise<ublox_msgs::AidALM>("ublox/aidalm", 10);
00144 publisher.publish(m);
00145 }
00146
00147 void publishAidEPH(const ublox_msgs::AidEPH& m)
00148 {
00149 static ros::Publisher publisher = nh->advertise<ublox_msgs::AidEPH>("ublox/aideph", 10);
00150 publisher.publish(m);
00151 }
00152
00153 void publishAidHUI(const ublox_msgs::AidHUI& m)
00154 {
00155 static ros::Publisher publisher = nh->advertise<ublox_msgs::AidHUI>("ublox/aidhui", 10);
00156 publisher.publish(m);
00157 }
00158
00159 template <typename MessageT>
00160 void publish(const MessageT& m, const std::string& topic) {
00161 static ros::Publisher publisher = nh->advertise<MessageT>(topic, 10);
00162 publisher.publish(m);
00163 }
00164
00165 void pollMessages(const ros::TimerEvent& event)
00166 {
00167 static std::vector<uint8_t> payload(1,1);
00168 if (enabled["aid_alm"]) gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::ALM, payload);
00169 if (enabled["aid_eph"]) gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH, payload);
00170 if (enabled["aid_hui"]) gps.poll(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI);
00171 payload[0]++;
00172 if (payload[0] > 32) payload[0] = 1;
00173 }
00174
00175 int main(int argc, char **argv) {
00176 boost::asio::io_service io_service;
00177 ros::Timer poller;
00178 boost::shared_ptr<void> handle;
00179
00180 ros::init(argc, argv, "ublox_gps");
00181 nh = new ros::NodeHandle;
00182
00183 std::string device;
00184 int baudrate;
00185
00186 ros::NodeHandle param("~");
00187 param.param("device", device, std::string("/dev/ttyUSB0"));
00188 param.param("frame_id", frame_id, std::string("gps_frame"));
00189 if (param.getParam("baudrate", baudrate)) gps.setBaudrate(baudrate);
00190
00191 boost::smatch match;
00192 if (boost::regex_match(device, match, boost::regex("(tcp|udp)://(.+):(\\d+)"))) {
00193 std::string proto(match[1]);
00194 std::string host(match[2]);
00195 std::string port(match[3]);
00196 ROS_INFO("Connecting to %s://%s:%s ...", proto.c_str(), host.c_str(), port.c_str());
00197
00198 if (proto == "tcp") {
00199 boost::asio::ip::tcp::resolver::iterator endpoint;
00200
00201 try {
00202 boost::asio::ip::tcp::resolver resolver(io_service);
00203 endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port));
00204 if (endpoint == boost::asio::ip::tcp::resolver::iterator()) {
00205 ROS_ERROR("Could not resolve %s:%s", host.c_str(), port.c_str());
00206 return 1;
00207 }
00208 } catch(std::runtime_error& e) {
00209 ROS_ERROR("Could not resolve %s:%s: %s", host.c_str(), port.c_str(), e.what());
00210 return 1;
00211 }
00212
00213 boost::asio::ip::tcp::socket *socket = new boost::asio::ip::tcp::socket(io_service);
00214 handle.reset(socket);
00215
00216 try {
00217 socket->connect(*endpoint);
00218 } catch(std::runtime_error& e) {
00219 ROS_ERROR("Could not connect to %s:%s: %s", endpoint->host_name().c_str(), endpoint->service_name().c_str(), e.what());
00220 return 1;
00221 }
00222
00223 ROS_INFO("Connected to %s:%s.", endpoint->host_name().c_str(), endpoint->service_name().c_str());
00224 gps.initialize(*socket, io_service);
00225 }
00226
00227 } else {
00228 boost::asio::serial_port *serial = new boost::asio::serial_port(io_service);
00229 handle.reset(serial);
00230
00231
00232 try {
00233 serial->open(device);
00234 } catch(std::runtime_error& e) {
00235 ROS_ERROR("Could not open serial port %s: %s", device.c_str(), e.what());
00236 delete serial;
00237 return 1;
00238 }
00239
00240 ROS_INFO("Opened serial port %s", device.c_str());
00241 gps.initialize(*serial, io_service);
00242 }
00243
00244
00245 ROS_INFO("Configuring GPS receiver...");
00246 if (!gps.configure()) {
00247 ROS_ERROR("Could not configure u-blox GPS");
00248 goto cleanup;
00249 }
00250
00251 ROS_INFO("Done!");
00252
00253
00254 param.param("all", enabled["all"], false);
00255 param.param("rxm", enabled["rxm"], false);
00256 param.param("aid", enabled["aid"], false);
00257
00258 param.param("nav_sol", enabled["nav_sol"], enabled["all"]);
00259 if (enabled["nav_sol"]) gps.subscribe<ublox_msgs::NavSOL>(boost::bind(&publish<ublox_msgs::NavSOL>, _1, "/ublox/navsol"), 1);
00260 param.param("nav_status", enabled["nav_status"], true);
00261 if (enabled["nav_status"]) gps.subscribe<ublox_msgs::NavSTATUS>(&publishNavStatus, 1);
00262 param.param("nav_svinfo", enabled["nav_svinfo"], enabled["all"]);
00263 if (enabled["nav_svinfo"]) gps.subscribe<ublox_msgs::NavSVINFO>(&publishNavSVINFO, 20);
00264 param.param("nav_clk", enabled["nav_clk"], enabled["all"]);
00265 if (enabled["nav_clk"]) gps.subscribe<ublox_msgs::NavCLOCK>(&publishNavCLK, 1);
00266 param.param("rxm_raw", enabled["rxm_raw"], enabled["all"] || enabled["rxm"]);
00267 if (enabled["rxm_raw"]) gps.subscribe<ublox_msgs::RxmRAW>(&publishRxmRAW, 1);
00268 param.param("rxm_sfrb", enabled["rxm_sfrb"], enabled["all"] || enabled["rxm"]);
00269 if (enabled["rxm_sfrb"]) gps.subscribe<ublox_msgs::RxmSFRB>(&publishRxmSFRB, 1);
00270 param.param("nav_posllh", enabled["nav_posllh"], true);
00271 if (enabled["nav_posllh"]) gps.subscribe<ublox_msgs::NavPOSLLH>(&publishNavPosLLH, 1);
00272 param.param("nav_velned", enabled["nav_velned"], true);
00273 if (enabled["nav_velned"]) gps.subscribe<ublox_msgs::NavVELNED>(&publishNavVelNED, 1);
00274 param.param("aid_alm", enabled["aid_alm"], enabled["all"] || enabled["aid"]);
00275 if (enabled["aid_alm"]) gps.subscribe<ublox_msgs::AidALM>(&publishAidALM);
00276 param.param("aid_eph", enabled["aid_eph"], enabled["all"] || enabled["aid"]);
00277 if (enabled["aid_eph"]) gps.subscribe<ublox_msgs::AidEPH>(&publishAidEPH);
00278 param.param("aid_hui", enabled["aid_hui"], enabled["all"] || enabled["aid"]);
00279 if (enabled["aid_hui"]) gps.subscribe<ublox_msgs::AidHUI>(&publishAidHUI);
00280
00281 poller = nh->createTimer(ros::Duration(1.0), &pollMessages);
00282 poller.start();
00283
00284
00285 ros::spin();
00286
00287
00288 cleanup:
00289 gps.close();
00290 handle.reset();
00291 ROS_INFO("Closed connection to %s.", device.c_str());
00292
00293 delete nh;
00294 return 0;
00295 }