client.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <gps_common/GPSFix.h>
00003 #include <gps_common/GPSStatus.h>
00004 #include <sensor_msgs/NavSatFix.h>
00005 #include <sensor_msgs/NavSatStatus.h>
00006 #include <libgpsmm.h>
00007 
00008 using namespace gps_common;
00009 using namespace sensor_msgs;
00010 
00011 class GPSDClient {
00012   public:
00013     GPSDClient() : privnode("~"), gps(NULL), use_gps_time(true), check_fix_by_variance(true) {}
00014 
00015     bool start() {
00016       gps_fix_pub = node.advertise<GPSFix>("extended_fix", 1);
00017       navsat_fix_pub = node.advertise<NavSatFix>("fix", 1);
00018 
00019       privnode.getParam("use_gps_time", use_gps_time);
00020       privnode.getParam("check_fix_by_variance", check_fix_by_variance);
00021 
00022       std::string host = "localhost";
00023       int port = 2947;
00024       privnode.getParam("host", host);
00025       privnode.getParam("port", port);
00026 
00027       char port_s[12];
00028       snprintf(port_s, 12, "%d", port);
00029 
00030       gps_data_t *resp = NULL;
00031 
00032 #if GPSD_API_MAJOR_VERSION >= 5
00033       gps = new gpsmm(host.c_str(), port_s);
00034       resp = gps->stream(WATCH_ENABLE);
00035 #elif GPSD_API_MAJOR_VERSION == 4
00036       gps = new gpsmm();
00037       gps->open(host.c_str(), port_s);
00038       resp = gps->stream(WATCH_ENABLE);
00039 #else
00040       gps = new gpsmm();
00041       resp = gps->open(host.c_str(), port_s);
00042       gps->query("w\n");
00043 #endif
00044 
00045       if (resp == NULL) {
00046         ROS_ERROR("Failed to open GPSd");
00047         return false;
00048       }
00049 
00050       ROS_INFO("GPSd opened");
00051       return true;
00052     }
00053 
00054     void step() {
00055 #if GPSD_API_MAJOR_VERSION >= 5
00056       if (!gps->waiting(1e6))
00057         return;
00058 
00059       gps_data_t *p = gps->read();
00060 #else
00061       gps_data_t *p = gps->poll();
00062 #endif
00063       process_data(p);
00064     }
00065 
00066     void stop() {
00067       // gpsmm doesn't have a close method? OK ...
00068     }
00069 
00070   private:
00071     ros::NodeHandle node;
00072     ros::NodeHandle privnode;
00073     ros::Publisher gps_fix_pub;
00074     ros::Publisher navsat_fix_pub;
00075     gpsmm *gps;
00076 
00077     bool use_gps_time;
00078     bool check_fix_by_variance;
00079 
00080     void process_data(struct gps_data_t* p) {
00081       if (p == NULL)
00082         return;
00083 
00084       if (!p->online)
00085         return;
00086 
00087       process_data_gps(p);
00088       process_data_navsat(p);
00089     }
00090 
00091 
00092 #if GPSD_API_MAJOR_VERSION >= 4
00093 #define SATS_VISIBLE p->satellites_visible
00094 #elif GPSD_API_MAJOR_VERSION == 3
00095 #define SATS_VISIBLE p->satellites
00096 #else
00097 #error "gpsd_client only supports gpsd API versions 3+"
00098 #endif
00099 
00100     void process_data_gps(struct gps_data_t* p) {
00101       ros::Time time = ros::Time::now();
00102 
00103       GPSFix fix;
00104       GPSStatus status;
00105 
00106       status.header.stamp = time;
00107       fix.header.stamp = time;
00108 
00109       status.satellites_used = p->satellites_used;
00110 
00111       status.satellite_used_prn.resize(status.satellites_used);
00112       for (int i = 0; i < status.satellites_used; ++i) {
00113         status.satellite_used_prn[i] = p->used[i];
00114       }
00115 
00116       status.satellites_visible = SATS_VISIBLE;
00117 
00118       status.satellite_visible_prn.resize(status.satellites_visible);
00119       status.satellite_visible_z.resize(status.satellites_visible);
00120       status.satellite_visible_azimuth.resize(status.satellites_visible);
00121       status.satellite_visible_snr.resize(status.satellites_visible);
00122 
00123       for (int i = 0; i < SATS_VISIBLE; ++i) {
00124         status.satellite_visible_prn[i] = p->PRN[i];
00125         status.satellite_visible_z[i] = p->elevation[i];
00126         status.satellite_visible_azimuth[i] = p->azimuth[i];
00127         status.satellite_visible_snr[i] = p->ss[i];
00128       }
00129 
00130       if ((p->status & STATUS_FIX) && !(check_fix_by_variance && isnan(p->fix.epx))) {
00131         status.status = 0; // FIXME: gpsmm puts its constants in the global
00132                            // namespace, so `GPSStatus::STATUS_FIX' is illegal.
00133 
00134         if (p->status & STATUS_DGPS_FIX)
00135           status.status |= 18; // same here
00136 
00137         fix.time = p->fix.time;
00138         fix.latitude = p->fix.latitude;
00139         fix.longitude = p->fix.longitude;
00140         fix.altitude = p->fix.altitude;
00141         fix.track = p->fix.track;
00142         fix.speed = p->fix.speed;
00143         fix.climb = p->fix.climb;
00144 
00145 #if GPSD_API_MAJOR_VERSION > 3
00146         fix.pdop = p->dop.pdop;
00147         fix.hdop = p->dop.hdop;
00148         fix.vdop = p->dop.vdop;
00149         fix.tdop = p->dop.tdop;
00150         fix.gdop = p->dop.gdop;
00151 #else
00152         fix.pdop = p->pdop;
00153         fix.hdop = p->hdop;
00154         fix.vdop = p->vdop;
00155         fix.tdop = p->tdop;
00156         fix.gdop = p->gdop;
00157 #endif
00158 
00159         fix.err = p->epe;
00160         fix.err_vert = p->fix.epv;
00161         fix.err_track = p->fix.epd;
00162         fix.err_speed = p->fix.eps;
00163         fix.err_climb = p->fix.epc;
00164         fix.err_time = p->fix.ept;
00165 
00166         /* TODO: attitude */
00167       } else {
00168         status.status = -1; // STATUS_NO_FIX
00169       }
00170 
00171       fix.status = status;
00172 
00173       gps_fix_pub.publish(fix);
00174     }
00175 
00176     void process_data_navsat(struct gps_data_t* p) {
00177       NavSatFixPtr fix(new NavSatFix);
00178 
00179       /* TODO: Support SBAS and other GBAS. */
00180 
00181       if (use_gps_time && !isnan(p->fix.time))
00182         fix->header.stamp = ros::Time(p->fix.time);
00183       else
00184         fix->header.stamp = ros::Time::now();
00185 
00186       /* gpsmm pollutes the global namespace with STATUS_,
00187        * so we need to use the ROS message's integer values
00188        * for status.status
00189        */
00190       switch (p->status) {
00191         case STATUS_NO_FIX:
00192           fix->status.status = -1; // NavSatStatus::STATUS_NO_FIX;
00193           break;
00194         case STATUS_FIX:
00195           fix->status.status = 0; // NavSatStatus::STATUS_FIX;
00196           break;
00197         case STATUS_DGPS_FIX:
00198           fix->status.status = 2; // NavSatStatus::STATUS_GBAS_FIX;
00199           break;
00200       }
00201 
00202       fix->status.service = NavSatStatus::SERVICE_GPS;
00203 
00204       fix->latitude = p->fix.latitude;
00205       fix->longitude = p->fix.longitude;
00206       fix->altitude = p->fix.altitude;
00207 
00208       /* gpsd reports status=OK even when there is no current fix,
00209        * as long as there has been a fix previously. Throw out these
00210        * fake results, which have NaN variance
00211        */
00212       if (isnan(p->fix.epx) && check_fix_by_variance) {
00213         return;
00214       }
00215 
00216       fix->position_covariance[0] = p->fix.epx;
00217       fix->position_covariance[4] = p->fix.epy;
00218       fix->position_covariance[8] = p->fix.epv;
00219 
00220       fix->position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
00221 
00222       navsat_fix_pub.publish(fix);
00223     }
00224 };
00225 
00226 int main(int argc, char ** argv) {
00227   ros::init(argc, argv, "gpsd_client");
00228 
00229   GPSDClient client;
00230 
00231   if (!client.start())
00232     return -1;
00233 
00234 
00235   while(ros::ok()) {
00236     ros::spinOnce();
00237     client.step();
00238   }
00239 
00240   client.stop();
00241 }


gpsd_client
Author(s): Ken Tossell , Rob Thomson
autogenerated on Mon Oct 6 2014 00:16:10