diff --git a/gpsd_client/CMakeLists.txt b/gpsd_client/CMakeLists.txt index a6d3778..44d7715 100644 --- a/gpsd_client/CMakeLists.txt +++ b/gpsd_client/CMakeLists.txt @@ -11,8 +11,26 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs ) -find_package(PkgConfig) -pkg_check_modules (libgps REQUIRED libgps) +# Try to find libgps, first with CMake's usual library search method, then by +# querying pkg-config. +find_library(libgps_LIBRARIES NAMES gps) +find_path(libgps_INCLUDE_DIRS NAMES libgpsmm.h gps.h) + +if(NOT libgps_LIBRARIES) + message(STATUS "Checking pkg-config for libgps") + find_package(PkgConfig) + if(PkgConfig_FOUND) + pkg_check_modules(libgps libgps) + endif() +endif() + +if(NOT libgps_LIBRARIES) + message(FATAL_ERROR "Could not find libgps " + "(hint for Debian/Ubuntu: apt install libgps-dev)") +else() + message(STATUS "Found libgps: ${libgps_LIBRARIES}") +endif() + ################################################### ## Declare things to be passed to other projects ## diff --git a/gpsd_client/package.xml b/gpsd_client/package.xml index b536431..050a345 100644 --- a/gpsd_client/package.xml +++ b/gpsd_client/package.xml @@ -1,5 +1,5 @@ - + gpsd_client 0.3.4 connects to a GPSd server and broadcasts GPS fixes @@ -16,9 +16,10 @@ catkin roscpp - gps_common - sensor_msgs pkg-config libgps + gps_common + sensor_msgs + diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index 736bfef..e4e7aa3 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -1,10 +1,33 @@ +#include + +// gps.h defines some macros which conflict with enum values in NavSatStatus.h, +// so we map them to new names before including other headers. +#ifdef STATUS_FIX + enum { + GPSD_STATUS_NO_FIX = STATUS_NO_FIX, + GPSD_STATUS_FIX = STATUS_FIX, + GPSD_STATUS_DGPS_FIX = STATUS_DGPS_FIX, + }; + #undef STATUS_NO_FIX + #undef STATUS_FIX + #undef STATUS_DGPS_FIX +#else + // Renamed in gpsd >= 3.23.1 (commits d4a4d8d3, af3b7dc0, 7d7b889f) without + // revising the API version number. + enum { + GPSD_STATUS_NO_FIX = STATUS_UNK, + GPSD_STATUS_FIX = STATUS_GPS, + GPSD_STATUS_DGPS_FIX = STATUS_DGPS, + }; +#endif + #include #include #include #include #include -#include +#include #include using namespace gps_common; @@ -23,12 +46,12 @@ class GPSDClient { privnode.param("frame_id", frame_id, frame_id); std::string host = "localhost"; - int port = 2947; + int port = atoi(DEFAULT_GPSD_PORT); privnode.getParam("host", host); privnode.getParam("port", port); char port_s[12]; - snprintf(port_s, 12, "%d", port); + snprintf(port_s, sizeof(port_s), "%d", port); gps_data_t *resp = NULL; @@ -67,7 +90,7 @@ class GPSDClient { } void stop() { - // gpsmm doesn't have a close method? OK ... + delete gps; } private: @@ -148,27 +171,22 @@ class GPSDClient { #endif } -#if GPSD_API_MAJOR_VERSION >= 12 - if ((p->fix.status & STATUS_GPS) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { -#elif GPSD_API_MAJOR_VERSION >= 10 - if ((p->fix.status & STATUS_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { +#if GPSD_API_MAJOR_VERSION >= 10 + if ((p->fix.status & GPSD_STATUS_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { #else - if ((p->status & STATUS_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { + if ((p->status & GPSD_STATUS_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) { #endif - status.status = 0; // FIXME: gpsmm puts its constants in the global - // namespace, so `GPSStatus::STATUS_FIX' is illegal. + status.status = GPSStatus::STATUS_FIX; -// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12 +// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward #if GPSD_API_MAJOR_VERSION != 6 -#if GPSD_API_MAJOR_VERSION >= 12 - if (p->fix.status & STATUS_DGPS) -#elif GPSD_API_MAJOR_VERSION >= 10 - if (p->fix.status & STATUS_DGPS_FIX) +#if GPSD_API_MAJOR_VERSION >= 10 + if (p->fix.status & GPSD_STATUS_DGPS_FIX) #else - if (p->status & STATUS_DGPS_FIX) + if (p->status & GPSD_STATUS_DGPS_FIX) #endif - status.status |= 18; // same here + status.status |= GPSStatus::STATUS_DGPS_FIX; #endif #if GPSD_API_MAJOR_VERSION >= 9 @@ -210,7 +228,7 @@ class GPSDClient { /* TODO: attitude */ } else { - status.status = -1; // STATUS_NO_FIX or STATUS_UNK + status.status = GPSStatus::STATUS_NO_FIX; } fix.status = status; @@ -219,57 +237,48 @@ class GPSDClient { } void process_data_navsat(struct gps_data_t* p) { - NavSatFixPtr fix(new NavSatFix); + NavSatFix fix; /* TODO: Support SBAS and other GBAS. */ #if GPSD_API_MAJOR_VERSION >= 9 if (use_gps_time && (p->online.tv_sec || p->online.tv_nsec)) { - fix->header.stamp = ros::Time(p->fix.time.tv_sec, p->fix.time.tv_nsec); + fix.header.stamp = ros::Time(p->fix.time.tv_sec, p->fix.time.tv_nsec); #else if (use_gps_time && !std::isnan(p->fix.time)) { - fix->header.stamp = ros::Time(p->fix.time); + fix.header.stamp = ros::Time(p->fix.time); #endif } else { - fix->header.stamp = ros::Time::now(); + fix.header.stamp = ros::Time::now(); } - fix->header.frame_id = frame_id; + fix.header.frame_id = frame_id; - /* gpsmm pollutes the global namespace with STATUS_, - * so we need to use the ROS message's integer values - * for status.status - */ #if GPSD_API_MAJOR_VERSION >= 10 switch (p->fix.status) { #else switch (p->status) { #endif -#if GPSD_API_MAJOR_VERSION >= 12 - case STATUS_GPS: -#else - case STATUS_NO_FIX: -#endif - fix->status.status = 0; // NavSatStatus::STATUS_FIX or NavSatStatus::STATUS_GPS; + case GPSD_STATUS_NO_FIX: + fix.status.status = NavSatStatus::STATUS_NO_FIX; break; -// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12 + case GPSD_STATUS_FIX: + fix.status.status = NavSatStatus::STATUS_FIX; + break; +// STATUS_DGPS_FIX was removed in API version 6 but re-added afterward #if GPSD_API_MAJOR_VERSION != 6 -#if GPSD_API_MAJOR_VERSION >= 12 - case STATUS_DGPS: -#else - case STATUS_DGPS_FIX: -#endif - fix->status.status = 2; // NavSatStatus::STATUS_GBAS_FIX; + case GPSD_STATUS_DGPS_FIX: + fix.status.status = NavSatStatus::STATUS_GBAS_FIX; break; #endif } - fix->status.service = NavSatStatus::SERVICE_GPS; + fix.status.service = NavSatStatus::SERVICE_GPS; - fix->latitude = p->fix.latitude; - fix->longitude = p->fix.longitude; - fix->altitude = p->fix.altitude; + fix.latitude = p->fix.latitude; + fix.longitude = p->fix.longitude; + fix.altitude = p->fix.altitude; /* gpsd reports status=OK even when there is no current fix, * as long as there has been a fix previously. Throw out these @@ -281,11 +290,13 @@ class GPSDClient { return; } - fix->position_covariance[0] = p->fix.epx; - fix->position_covariance[4] = p->fix.epy; - fix->position_covariance[8] = p->fix.epv; + fix.position_covariance[0] = p->fix.epx; + fix.position_covariance[4] = p->fix.epy; + fix.position_covariance[8] = p->fix.epv; - fix->position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + fix.position_covariance_type = std::isnan(p->fix.epx) ? + NavSatFix::COVARIANCE_TYPE_UNKNOWN : + NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; navsat_fix_pub.publish(fix); }