node.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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   // Example geometry message
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   // Position message
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     // open serial port
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   // configure the GPS
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   // subscribe messages
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   // wait...
00285   ros::spin();
00286 
00287   // cleanup
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 }


ublox_gps
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 08:16:41