client.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
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  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <corobot_msgs/GPSFix.h>
00032 #include <corobot_msgs/GPSStatus.h>
00033 #include <sensor_msgs/NavSatFix.h>
00034 #include <sensor_msgs/NavSatStatus.h>
00035 #include <libgpsmm.h>
00036 #include <diagnostic_updater/diagnostic_updater.h>
00037 #include <diagnostic_updater/publisher.h>
00038 
00043 using namespace corobot_msgs;
00044 using namespace sensor_msgs;
00045 
00046 // Class interfacing with the gps sensor
00047 class GPSDClient {
00048   public:
00049   
00050     GPSDClient() : privnode("~"), use_gps_time(true) {
00051 #if GPSD_API_MAJOR_VERSION < 5
00052     gps = new gpsmm();
00053     gps_state = 0;
00054 #endif
00055     }
00056 
00057     ~GPSDClient()
00058     {
00059             delete gps;
00060     }
00061 
00062     // Start the gps sensor
00063     bool start() {
00064       // Advertise the topics
00065       gps_fix_pub = node.advertise<GPSFix>("extended_fix", 1);
00066       navsat_fix_pub = node.advertise<NavSatFix>("fix", 1);
00067 
00068       privnode.getParam("use_gps_time", use_gps_time);
00069 
00070       // Set up where the gps data are read from
00071       std::string host = "localhost";
00072       int port = 2947;
00073       privnode.getParam("host", host);
00074       privnode.getParam("port", port);
00075 
00076       char port_s[12];
00077       snprintf(port_s, 12, "%d", port);
00078 
00079       // Start gpsmm 
00080       gps_data_t *resp;
00081 #if GPSD_API_MAJOR_VERSION >= 5
00082       gps = new gpsmm(host.c_str(), port_s);
00083 #elif GPSD_API_MAJOR_VERSION < 5
00084       resp = gps->open(host.c_str(), port_s);
00085       if (resp == NULL) {
00086         ROS_ERROR("Failed to open GPSd");
00087               gps_state = 1;
00088         return false;
00089       }
00090 #endif
00091 
00092       // Start the stream that will give us gps values
00093 #if GPSD_API_MAJOR_VERSION >= 5
00094       resp = gps->stream(WATCH_ENABLE);
00095       if (resp == NULL) {
00096         ROS_ERROR("Failed to intialize the gps");
00097             gps_state = 1;
00098             return false;
00099       }
00100 #elif GPSD_API_MAJOR_VERSION == 4
00101       resp = gps->stream(WATCH_ENABLE);
00102       if (resp == NULL) {
00103         ROS_ERROR("Failed to intialize the gps");
00104             gps_state = 1;
00105             return false;
00106       }
00107 #elif GPSD_API_MAJOR_VERSION == 3
00108       gps->query("w\n");
00109 #else
00110 #error "gpsd_client only supports gpsd API versions 3, 4 and 5"
00111       gps_state = 2;
00112 #endif
00113       
00114 
00115       ROS_INFO("GPSd opened");
00116       return true;
00117     }
00118 
00119     // Read the gps data and process them
00120     void step() {
00121 
00122       gps_data_t *p;
00123 #if GPSD_API_MAJOR_VERSION >= 5
00124       p = gps->read();
00125 #elif GPSD_API_MAJOR_VERSION < 5
00126       p = gps->poll();
00127 #endif
00128       process_data(p);
00129     }
00130 
00131     // Stop the gps
00132     void stop() {
00133       // gpsmm doesn't have a close method? OK ...
00134     }
00135 
00139     void gps_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00140     {
00141             if (gps_state == 0)  
00142                     stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "The gps is working");
00143             else if (gps_state == 1)
00144             {
00145                     stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Can't intialize");
00146                     stat.addf("Recommendation", "The gps could not be initialized. Please make sure the gps is connected to the motherboard and is configured. You can follow points 1.3 and after in the following tutorial for the configuration: http://ros.org/wiki/gpsd_client/Tutorials/Getting started with gpsd_client");
00147             }
00148             else if (gps_state == 2)
00149             {
00150                     stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "wrong lib gpsd version");
00151                     stat.addf("Recommendation", "Please make sure that gpsd is installed and that you have at least the api major version 3 or after installed.");
00152             }
00153     }
00154 
00155   private:
00156     ros::NodeHandle node;
00157     ros::NodeHandle privnode;
00158     // ROS publisher
00159     ros::Publisher gps_fix_pub, navsat_fix_pub;
00160     gpsmm *gps;
00161     int gps_state;
00162 
00163     bool use_gps_time;
00164 
00165     void process_data(struct gps_data_t* p) 
00166     {
00167       if (p == NULL)
00168         return;
00169 
00170       if (!p->online)
00171         return;
00172 
00173       process_data_gps(p);
00174       process_data_navsat(p);
00175     }
00176 
00177 #if GPSD_API_MAJOR_VERSION == 5
00178 #define SATS_VISIBLE p->satellites_visible
00179 #elif GPSD_API_MAJOR_VERSION == 4
00180 #define SATS_VISIBLE p->satellites_visible
00181 #elif GPSD_API_MAJOR_VERSION == 3
00182 #define SATS_VISIBLE p->satellites
00183 #endif
00184 
00185     // process the data received from the gps
00186     void process_data_gps(struct gps_data_t* p) {
00187       ros::Time time = ros::Time::now();
00188 
00189       GPSFix fix;
00190       GPSStatus status;
00191 
00192       status.header.stamp = time;
00193       fix.header.stamp = time;
00194 
00195       status.satellites_used = p->satellites_used;
00196 
00197       status.satellite_used_prn.resize(status.satellites_used);
00198       for (int i = 0; i < status.satellites_used; ++i) {
00199         status.satellite_used_prn[i] = p->used[i];
00200       }
00201 
00202       status.satellites_visible = SATS_VISIBLE;
00203 
00204       status.satellite_visible_prn.resize(status.satellites_visible);
00205       status.satellite_visible_z.resize(status.satellites_visible);
00206       status.satellite_visible_azimuth.resize(status.satellites_visible);
00207       status.satellite_visible_snr.resize(status.satellites_visible);
00208 
00209       for (int i = 0; i < SATS_VISIBLE; ++i) {
00210         status.satellite_visible_prn[i] = p->PRN[i];
00211         status.satellite_visible_z[i] = p->elevation[i];
00212         status.satellite_visible_azimuth[i] = p->azimuth[i];
00213     //    status.satellite_visible_snr[i] = p->ss[i];
00214       }
00215 
00216       if ((p->status & STATUS_FIX) && !isnan(p->fix.epx)) {
00217         // FIXME: gpsmm puts its constants in the global namespace, so `GPSStatus::STATUS_FIX' is illegal.
00218         status.status = 0; 
00219 
00220         if (p->status & STATUS_DGPS_FIX)
00221         {
00222           // same here
00223           status.status |= 18; 
00224         }
00225 
00226         // Set the important values into the gps variables
00227         fix.time = p->fix.time;
00228         fix.latitude = p->fix.latitude;
00229         fix.longitude = p->fix.longitude;
00230         fix.altitude = p->fix.altitude;
00231         fix.track = p->fix.track;
00232         fix.speed = p->fix.speed;
00233         fix.climb = p->fix.climb;
00234 
00235 #if GPSD_API_MAJOR_VERSION > 3
00236         fix.pdop = p->dop.pdop;
00237         fix.hdop = p->dop.hdop;
00238         fix.vdop = p->dop.vdop;
00239         fix.tdop = p->dop.tdop;
00240         fix.gdop = p->dop.gdop;
00241 #else
00242         fix.pdop = p->pdop;
00243         fix.hdop = p->hdop;
00244         fix.vdop = p->vdop;
00245         fix.tdop = p->tdop;
00246         fix.gdop = p->gdop;
00247 #endif
00248 
00249         fix.err = p->epe;
00250         fix.err_vert = p->fix.epv;
00251         fix.err_track = p->fix.epd;
00252         fix.err_speed = p->fix.eps;
00253         fix.err_climb = p->fix.epc;
00254         fix.err_time = p->fix.ept;
00255 
00256         /* TODO: attitude */
00257       } else {
00258         status.status = -1; // STATUS_NO_FIX
00259       }
00260 
00261       fix.status = status;
00262 
00263       gps_fix_pub.publish(fix);
00264     }
00265 
00266     void process_data_navsat(struct gps_data_t* p) {
00267       NavSatFixPtr fix(new NavSatFix);
00268 
00269       /* TODO: Support SBAS and other GBAS. */
00270 
00271       if (use_gps_time)
00272         fix->header.stamp = ros::Time(p->fix.time);
00273       else
00274         fix->header.stamp = ros::Time::now();
00275 
00276       /* gpsmm pollutes the global namespace with STATUS_,
00277        * so we need to use the ROS message's integer values
00278        * for status.status
00279        */
00280       switch (p->status) {
00281         case STATUS_NO_FIX:
00282           // NavSatStatus::STATUS_NO_FIX;
00283           fix->status.status = -1; 
00284           break;
00285         case STATUS_FIX:
00286           // NavSatStatus::STATUS_FIX;
00287           fix->status.status = 0; 
00288           break;
00289         case STATUS_DGPS_FIX:
00290           // NavSatStatus::STATUS_GBAS_FIX;
00291           fix->status.status = 2; 
00292           break;
00293       }
00294 
00295       fix->status.service = NavSatStatus::SERVICE_GPS;
00296 
00297       fix->latitude = p->fix.latitude;
00298       fix->longitude = p->fix.longitude;
00299       fix->altitude = p->fix.altitude;
00300 
00301       /* gpsd reports status=OK even when there is no current fix,
00302        * as long as there has been a fix previously. Throw out these
00303        * fake results, which have NaN variance
00304        */
00305       if (isnan(p->fix.epx)) {
00306         //return;    //MODIFIED
00307       }
00308 
00309       fix->position_covariance[0] = p->fix.epx;
00310       fix->position_covariance[4] = p->fix.epy;
00311       fix->position_covariance[8] = p->fix.epv;
00312 
00313       fix->position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
00314 
00315       navsat_fix_pub.publish(fix);
00316     }
00317 };
00318 
00319 int main(int argc, char ** argv) {
00320   ros::init(argc, argv, "corobot_gps");
00321 
00322   GPSDClient client;
00323 
00324 
00325   //create an updater that will send information on the diagnostics topics
00326   diagnostic_updater::Updater updater;
00327   updater.setHardwareIDf("GPS");
00328   updater.add("gps", &client, &GPSDClient::gps_diagnostic); //function that will be executed with updater.update()
00329 
00330   if (!client.start())
00331   {
00332     updater.force_update();
00333     return -1;
00334   }
00335 
00336 
00337   while(ros::ok()) {
00338     ros::spinOnce();
00339     updater.update();
00340     client.step();
00341   }
00342 
00343   client.stop();
00344 }


corobot_gps
Author(s):
autogenerated on Sun Oct 5 2014 23:18:04