client.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <gps_common/GPSFix.h>
3 #include <gps_common/GPSStatus.h>
4 #include <sensor_msgs/NavSatFix.h>
5 #include <sensor_msgs/NavSatStatus.h>
6 #include <libgpsmm.h>
7 
8 #include <cmath>
9 
10 using namespace gps_common;
11 using namespace sensor_msgs;
12 
13 class GPSDClient {
14  public:
15  GPSDClient() :
16  privnode("~"),
17  gps(NULL),
18  use_gps_time(true),
19  check_fix_by_variance(true),
20  frame_id("gps") {}
21 
22  bool start() {
23  gps_fix_pub = node.advertise<GPSFix>("extended_fix", 1);
24  navsat_fix_pub = node.advertise<NavSatFix>("fix", 1);
25 
26  privnode.getParam("use_gps_time", use_gps_time);
27  privnode.getParam("check_fix_by_variance", check_fix_by_variance);
28  privnode.param("frame_id", frame_id, frame_id);
29 
30  std::string host = "localhost";
31  int port = atoi(DEFAULT_GPSD_PORT);
32  privnode.getParam("host", host);
33  privnode.getParam("port", port);
34 
35  char port_s[12];
36  snprintf(port_s, sizeof(port_s), "%d", port);
37 
38  gps_data_t *resp = NULL;
39 
40 #if GPSD_API_MAJOR_VERSION >= 5
41  gps = new gpsmm(host.c_str(), port_s);
42  resp = gps->stream(WATCH_ENABLE);
43 #elif GPSD_API_MAJOR_VERSION == 4
44  gps = new gpsmm();
45  gps->open(host.c_str(), port_s);
46  resp = gps->stream(WATCH_ENABLE);
47 #else
48  gps = new gpsmm();
49  resp = gps->open(host.c_str(), port_s);
50  gps->query("w\n");
51 #endif
52 
53  if (resp == NULL) {
54  ROS_ERROR("Failed to open GPSd");
55  return false;
56  }
57 
58  ROS_INFO("GPSd opened");
59  return true;
60  }
61 
62  void step() {
63 #if GPSD_API_MAJOR_VERSION >= 5
64  if (!gps->waiting(1e6))
65  return;
66 
67  // Read out all queued data and only act on the latest
68  gps_data_t* p = NULL;
69  while (gps->waiting(0))
70  {
71  p = gps->read();
72  }
73 #else
74  gps_data_t *p = gps->poll();
75 #endif
76  process_data(p);
77  }
78 
79  void stop() {
80  // gpsmm doesn't have a close method? OK ...
81  if (gps != NULL)
82  {
83  delete gps;
84  }
85  }
86 
87  private:
88  ros::NodeHandle node;
89  ros::NodeHandle privnode;
90  ros::Publisher gps_fix_pub;
91  ros::Publisher navsat_fix_pub;
92  gpsmm *gps;
93 
94  bool use_gps_time;
95  bool check_fix_by_variance;
96  std::string frame_id;
97 
98  void process_data(struct gps_data_t* p) {
99  if (p == NULL)
100  return;
101 
102 #if GPSD_API_MAJOR_VERSION >= 9
103  if (!p->online.tv_sec && !p->online.tv_nsec) {
104 #else
105  if (!p->online) {
106 #endif
107  return;
108  }
109 
110  process_data_gps(p);
111  process_data_navsat(p);
112  }
113 
114 
115 #if GPSD_API_MAJOR_VERSION >= 4
116 #define SATS_VISIBLE p->satellites_visible
117 #elif GPSD_API_MAJOR_VERSION == 3
118 #define SATS_VISIBLE p->satellites
119 #else
120 #error "gpsd_client only supports gpsd API versions 3+"
121 #endif
122 
123  void process_data_gps(struct gps_data_t* p) {
124  ros::Time time = ros::Time::now();
125 
126  GPSFix fix;
127  GPSStatus status;
128 
129  status.header.stamp = time;
130  fix.header.stamp = time;
131  fix.header.frame_id = frame_id;
132 
133  status.satellites_used = p->satellites_used;
134 
135  status.satellite_used_prn.resize(status.satellites_used);
136  for (int i = 0; i < status.satellites_used; ++i) {
137 #if GPSD_API_MAJOR_VERSION > 5
138  status.satellite_used_prn[i] = p->skyview[i].used;
139 #else
140  status.satellite_used_prn[i] = p->used[i];
141 #endif
142  }
143 
144  status.satellites_visible = SATS_VISIBLE;
145 
146  status.satellite_visible_prn.resize(status.satellites_visible);
147  status.satellite_visible_z.resize(status.satellites_visible);
148  status.satellite_visible_azimuth.resize(status.satellites_visible);
149  status.satellite_visible_snr.resize(status.satellites_visible);
150 
151  for (int i = 0; i < SATS_VISIBLE; ++i) {
152 #if GPSD_API_MAJOR_VERSION > 5
153  status.satellite_visible_prn[i] = p->skyview[i].PRN;
154  status.satellite_visible_z[i] = p->skyview[i].elevation;
155  status.satellite_visible_azimuth[i] = p->skyview[i].azimuth;
156  status.satellite_visible_snr[i] = p->skyview[i].ss;
157 #else
158  status.satellite_visible_prn[i] = p->PRN[i];
159  status.satellite_visible_z[i] = p->elevation[i];
160  status.satellite_visible_azimuth[i] = p->azimuth[i];
161  status.satellite_visible_snr[i] = p->ss[i];
162 #endif
163  }
164 
165 #if GPSD_API_MAJOR_VERSION >= 12
166  if ((p->fix.status != STATUS_NO_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) {
167 #elif GPSD_API_MAJOR_VERSION >= 10
168  if ((p->fix.status != STATUS_NO_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) {
169 #else
170  if ((p->status != STATUS_NO_FIX) && !(check_fix_by_variance && std::isnan(p->fix.epx))) {
171 #endif
172 
173  status.status = 0; // FIXME: gpsmm puts its constants in the global
174  // namespace, so `GPSStatus::STATUS_FIX' is illegal.
175 
176 // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12
177 #if GPSD_API_MAJOR_VERSION != 6
178 #if GPSD_API_MAJOR_VERSION >= 12
179  if (p->fix.status & STATUS_DGPS)
180 #elif GPSD_API_MAJOR_VERSION >= 10
181  if (p->fix.status & STATUS_DGPS_FIX)
182 #else
183  if (p->status & STATUS_DGPS_FIX)
184 #endif
185  status.status |= 18; // same here
186 #endif
187 
188 #if GPSD_API_MAJOR_VERSION >= 9
189  fix.time = (double)(p->fix.time.tv_sec) + (double)(p->fix.time.tv_nsec) / 1000000.;
190 #else
191  fix.time = p->fix.time;
192 #endif
193  fix.latitude = p->fix.latitude;
194  fix.longitude = p->fix.longitude;
195  fix.altitude = p->fix.altitude;
196  fix.track = p->fix.track;
197  fix.speed = p->fix.speed;
198  fix.climb = p->fix.climb;
199 
200 #if GPSD_API_MAJOR_VERSION > 3
201  fix.pdop = p->dop.pdop;
202  fix.hdop = p->dop.hdop;
203  fix.vdop = p->dop.vdop;
204  fix.tdop = p->dop.tdop;
205  fix.gdop = p->dop.gdop;
206 #else
207  fix.pdop = p->pdop;
208  fix.hdop = p->hdop;
209  fix.vdop = p->vdop;
210  fix.tdop = p->tdop;
211  fix.gdop = p->gdop;
212 #endif
213 
214 #if GPSD_API_MAJOR_VERSION < 8
215  fix.err = p->epe;
216 #else
217  fix.err = p->fix.eph;
218 #endif
219  fix.err_vert = p->fix.epv;
220  fix.err_track = p->fix.epd;
221  fix.err_speed = p->fix.eps;
222  fix.err_climb = p->fix.epc;
223  fix.err_time = p->fix.ept;
224 
225  /* TODO: attitude */
226  } else {
227  status.status = -1; // STATUS_NO_FIX or STATUS_UNK
228  }
229 
230  fix.status = status;
231 
232  gps_fix_pub.publish(fix);
233  }
234 
235  void process_data_navsat(struct gps_data_t* p) {
236  NavSatFix fix;
237 
238  /* TODO: Support SBAS and other GBAS. */
239 
240 #if GPSD_API_MAJOR_VERSION >= 9
241  if (use_gps_time && (p->online.tv_sec || p->online.tv_nsec)) {
242  fix.header.stamp = ros::Time(p->fix.time.tv_sec, p->fix.time.tv_nsec);
243 #else
244  if (use_gps_time && !std::isnan(p->fix.time)) {
245  fix.header.stamp = ros::Time(p->fix.time);
246 #endif
247  }
248  else {
249  fix.header.stamp = ros::Time::now();
250  }
251 
252  fix.header.frame_id = frame_id;
253 
254  /* gpsmm pollutes the global namespace with STATUS_,
255  * so we need to use the ROS message's integer values
256  * for status.status
257  */
258 #if GPSD_API_MAJOR_VERSION >= 10
259  switch (p->fix.status) {
260 #else
261  switch (p->status) {
262 #endif
263 #if GPSD_API_MAJOR_VERSION >= 12
264  case STATUS_GPS:
265 #else
266  case STATUS_NO_FIX:
267 #endif
268  fix.status.status = 0; // NavSatStatus::STATUS_FIX or NavSatStatus::STATUS_GPS;
269  break;
270 // STATUS_DGPS_FIX was removed in API version 6 but re-added afterward and next renamed since the version 12
271 #if GPSD_API_MAJOR_VERSION != 6
272 #if GPSD_API_MAJOR_VERSION >= 12
273  case STATUS_DGPS:
274 #else
275  case STATUS_DGPS_FIX:
276 #endif
277  fix.status.status = 2; // NavSatStatus::STATUS_GBAS_FIX;
278  break;
279 #endif
280  }
281 
282  fix.status.service = NavSatStatus::SERVICE_GPS;
283 
284  fix.latitude = p->fix.latitude;
285  fix.longitude = p->fix.longitude;
286  fix.altitude = p->fix.altitude;
287 
288  /* gpsd reports status=OK even when there is no current fix,
289  * as long as there has been a fix previously. Throw out these
290  * fake results, which have NaN variance
291  */
292  if (std::isnan(p->fix.epx) && check_fix_by_variance) {
293  ROS_DEBUG_THROTTLE(1,
294  "GPS status was reported as OK, but variance was invalid");
295  return;
296  }
297 
298  fix.position_covariance[0] = p->fix.epx;
299  fix.position_covariance[4] = p->fix.epy;
300  fix.position_covariance[8] = p->fix.epv;
301 
302  fix.position_covariance_type = NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
303 
304  navsat_fix_pub.publish(fix);
305  }
306 };
307 
308 int main(int argc, char ** argv) {
309  ros::init(argc, argv, "gpsd_client");
310 
311  GPSDClient client;
312 
313  if (!client.start())
314  return -1;
315 
316 
317  while(ros::ok()) {
318  ros::spinOnce();
319  client.step();
320  }
321 
322  client.stop();
323 }
GPSDClient::step
void step()
Definition: client.cpp:62
GPSDClient::node
ros::NodeHandle node
Definition: client.cpp:88
ros::Publisher
GPSDClient::navsat_fix_pub
ros::Publisher navsat_fix_pub
Definition: client.cpp:91
GPSDClient::process_data
void process_data(struct gps_data_t *p)
Definition: client.cpp:98
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
main
int main(int argc, char **argv)
Definition: client.cpp:308
ros.h
GPSDClient::start
bool start()
Definition: client.cpp:22
frame_id
std::string frame_id
ros::spinOnce
ROSCPP_DECL void spinOnce()
GPSDClient::gps_fix_pub
ros::Publisher gps_fix_pub
Definition: client.cpp:90
GPSDClient::use_gps_time
bool use_gps_time
Definition: client.cpp:94
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::ok
ROSCPP_DECL bool ok()
GPSDClient::stop
void stop()
Definition: client.cpp:79
GPSDClient::process_data_gps
void process_data_gps(struct gps_data_t *p)
Definition: client.cpp:123
GPSDClient::frame_id
std::string frame_id
Definition: client.cpp:96
GPSDClient::privnode
ros::NodeHandle privnode
Definition: client.cpp:89
GPSDClient::check_fix_by_variance
bool check_fix_by_variance
Definition: client.cpp:95
GPSDClient::process_data_navsat
void process_data_navsat(struct gps_data_t *p)
Definition: client.cpp:235
GPSDClient
Definition: client.cpp:13
GPSDClient::GPSDClient
GPSDClient()
Definition: client.cpp:15
ros::Time
gps_common
ROS_ERROR
#define ROS_ERROR(...)
ROS_INFO
#define ROS_INFO(...)
sensor_msgs
GPSDClient::gps
gpsmm * gps
Definition: client.cpp:92
ROS_DEBUG_THROTTLE
#define ROS_DEBUG_THROTTLE(period,...)
ros::NodeHandle
ros::Time::now
static Time now()

gpsd_client
Author(s): Ken Tossell , Rob Thomson , Timo Roehling , P. J. Reed
autogenerated on Fri Aug 2 2024 08:42:49