From a48c1a6c919ddd5aaf5f7a8de435d8fbdef3a84a Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Fri, 22 Apr 2022 18:50:06 -0400 Subject: [PATCH 1/7] Prefer built-in CMake library search logic --- gpsd_client/CMakeLists.txt | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) 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 ## From ec71e18bc3ed81133c46fa2e3bf91a4a0225ed68 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Fri, 22 Apr 2022 18:50:55 -0400 Subject: [PATCH 2/7] Declare runtime message type dependencies --- gpsd_client/package.xml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 + From 8897599e96485115e0f4fe3ff98d58deabbbc224 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Fri, 22 Apr 2022 18:51:24 -0400 Subject: [PATCH 3/7] Remove unnecessary heap allocation --- gpsd_client/src/client.cpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index 736bfef..f4fd80a 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -219,23 +219,23 @@ 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 @@ -251,7 +251,7 @@ class GPSDClient { #else case STATUS_NO_FIX: #endif - fix->status.status = 0; // NavSatStatus::STATUS_FIX or NavSatStatus::STATUS_GPS; + fix.status.status = 0; // NavSatStatus::STATUS_FIX or NavSatStatus::STATUS_GPS; break; // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12 #if GPSD_API_MAJOR_VERSION != 6 @@ -260,16 +260,16 @@ class GPSDClient { #else case STATUS_DGPS_FIX: #endif - fix->status.status = 2; // NavSatStatus::STATUS_GBAS_FIX; + fix.status.status = 2; // 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 +281,11 @@ 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 = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; navsat_fix_pub.publish(fix); } From b36be026da5544e6ff13386f43eb5b57c10a2e6b Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Fri, 22 Apr 2022 19:25:47 -0400 Subject: [PATCH 4/7] Resolve naming conflict between gps.h and message enums --- gpsd_client/src/client.cpp | 74 +++++++++++++++++++++----------------- 1 file changed, 41 insertions(+), 33 deletions(-) diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index f4fd80a..98e7165 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -1,9 +1,31 @@ +#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 @@ -148,27 +170,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 +227,7 @@ class GPSDClient { /* TODO: attitude */ } else { - status.status = -1; // STATUS_NO_FIX or STATUS_UNK + status.status = GPSStatus::STATUS_NO_FIX; } fix.status = status; @@ -237,30 +254,21 @@ class GPSDClient { 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 } From e592a56eb6dd2fa1324b7482050a7aa23f1fba55 Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Fri, 22 Apr 2022 19:27:27 -0400 Subject: [PATCH 5/7] Do not hardcode port number --- gpsd_client/src/client.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index 98e7165..d8db610 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -27,6 +27,7 @@ #include #include +#include #include using namespace gps_common; @@ -45,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; From fa1336741e3caefd23450f9b1d91b11e5fc6941d Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Fri, 22 Apr 2022 19:49:02 -0400 Subject: [PATCH 6/7] Free gpsmm instance --- gpsd_client/src/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index d8db610..abf8cc5 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -90,7 +90,7 @@ class GPSDClient { } void stop() { - // gpsmm doesn't have a close method? OK ... + delete gps; } private: From b680df6142d7dfbc8a63224116b9a4a56e964c5b Mon Sep 17 00:00:00 2001 From: Ryan Govostes Date: Thu, 29 Jun 2023 12:11:39 -0700 Subject: [PATCH 7/7] Set COVARIANCE_TYPE_UNKNOWN when covariance is NaN --- gpsd_client/src/client.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gpsd_client/src/client.cpp b/gpsd_client/src/client.cpp index abf8cc5..e4e7aa3 100644 --- a/gpsd_client/src/client.cpp +++ b/gpsd_client/src/client.cpp @@ -294,7 +294,9 @@ class GPSDClient { 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); }