1+ #include < libgpsmm.h>
2+
3+ // gps.h defines some macros which conflict with enum values in NavSatStatus.h,
4+ // so we map them to new names before including other headers.
5+ #ifdef STATUS_FIX
6+ enum {
7+ GPSD_STATUS_NO_FIX = STATUS_NO_FIX,
8+ GPSD_STATUS_FIX = STATUS_FIX,
9+ GPSD_STATUS_DGPS_FIX = STATUS_DGPS_FIX,
10+ };
11+ #undef STATUS_NO_FIX
12+ #undef STATUS_FIX
13+ #undef STATUS_DGPS_FIX
14+ #else
15+ // Renamed in gpsd >= 3.23.1 (commits d4a4d8d3, af3b7dc0, 7d7b889f) without
16+ // revising the API version number.
17+ enum {
18+ GPSD_STATUS_NO_FIX = STATUS_UNK,
19+ GPSD_STATUS_FIX = STATUS_GPS,
20+ GPSD_STATUS_DGPS_FIX = STATUS_DGPS,
21+ };
22+ #endif
23+
124#include < ros/ros.h>
225#include < gps_common/GPSFix.h>
326#include < gps_common/GPSStatus.h>
427#include < sensor_msgs/NavSatFix.h>
528#include < sensor_msgs/NavSatStatus.h>
6- #include < libgpsmm.h>
729
830#include < cmath>
931
@@ -148,27 +170,22 @@ class GPSDClient {
148170#endif
149171 }
150172
151- #if GPSD_API_MAJOR_VERSION >= 12
152- if ((p->fix .status & STATUS_GPS) && !(check_fix_by_variance && std::isnan (p->fix .epx ))) {
153- #elif GPSD_API_MAJOR_VERSION >= 10
154- if ((p->fix .status & STATUS_FIX) && !(check_fix_by_variance && std::isnan (p->fix .epx ))) {
173+ #if GPSD_API_MAJOR_VERSION >= 10
174+ if ((p->fix .status & GPSD_STATUS_FIX) && !(check_fix_by_variance && std::isnan (p->fix .epx ))) {
155175#else
156- if ((p->status & STATUS_FIX ) && !(check_fix_by_variance && std::isnan (p->fix .epx ))) {
176+ if ((p->status & GPSD_STATUS_FIX ) && !(check_fix_by_variance && std::isnan (p->fix .epx ))) {
157177#endif
158178
159- status.status = 0 ; // FIXME: gpsmm puts its constants in the global
160- // namespace, so `GPSStatus::STATUS_FIX' is illegal.
179+ status.status = GPSStatus::STATUS_FIX;
161180
162- // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12
181+ // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward
163182#if GPSD_API_MAJOR_VERSION != 6
164- #if GPSD_API_MAJOR_VERSION >= 12
165- if (p->fix .status & STATUS_DGPS)
166- #elif GPSD_API_MAJOR_VERSION >= 10
167- if (p->fix .status & STATUS_DGPS_FIX)
183+ #if GPSD_API_MAJOR_VERSION >= 10
184+ if (p->fix .status & GPSD_STATUS_DGPS_FIX)
168185#else
169- if (p->status & STATUS_DGPS_FIX )
186+ if (p->status & GPSD_STATUS_DGPS_FIX )
170187#endif
171- status.status |= 18 ; // same here
188+ status.status |= GPSStatus::STATUS_DGPS_FIX;
172189#endif
173190
174191#if GPSD_API_MAJOR_VERSION >= 9
@@ -210,7 +227,7 @@ class GPSDClient {
210227
211228 /* TODO: attitude */
212229 } else {
213- status.status = - 1 ; // STATUS_NO_FIX or STATUS_UNK
230+ status.status = GPSStatus:: STATUS_NO_FIX;
214231 }
215232
216233 fix.status = status;
@@ -237,30 +254,21 @@ class GPSDClient {
237254
238255 fix.header .frame_id = frame_id;
239256
240- /* gpsmm pollutes the global namespace with STATUS_,
241- * so we need to use the ROS message's integer values
242- * for status.status
243- */
244257#if GPSD_API_MAJOR_VERSION >= 10
245258 switch (p->fix .status ) {
246259#else
247260 switch (p->status ) {
248261#endif
249- #if GPSD_API_MAJOR_VERSION >= 12
250- case STATUS_GPS:
251- #else
252- case STATUS_NO_FIX:
253- #endif
254- fix.status .status = 0 ; // NavSatStatus::STATUS_FIX or NavSatStatus::STATUS_GPS;
262+ case GPSD_STATUS_NO_FIX:
263+ fix.status .status = NavSatStatus::STATUS_NO_FIX;
255264 break ;
256- // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12
265+ case GPSD_STATUS_FIX:
266+ fix.status .status = NavSatStatus::STATUS_FIX;
267+ break ;
268+ // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward
257269#if GPSD_API_MAJOR_VERSION != 6
258- #if GPSD_API_MAJOR_VERSION >= 12
259- case STATUS_DGPS:
260- #else
261- case STATUS_DGPS_FIX:
262- #endif
263- fix.status .status = 2 ; // NavSatStatus::STATUS_GBAS_FIX;
270+ case GPSD_STATUS_DGPS_FIX:
271+ fix.status .status = NavSatStatus::STATUS_GBAS_FIX;
264272 break ;
265273#endif
266274 }
0 commit comments