scripts/gps_follow_gpsd.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import thread
   5 import gps
   6 import numpy as np
   7 from geodesy import utm
   8 from gps_follow_waypoints import GPSGotoCoords
   9
  10 DEFAULT_GPSD_HOSTNAME="wildthumper"
  11 DEFAULT_GPSD_PORT="2948"
  12
  13 class GPSFollowGPSD:
  14         def __init__(self, hostname=DEFAULT_GPSD_HOSTNAME, port=DEFAULT_GPSD_PORT):
  15                 self.gpsd = gps.gps(host=hostname, port=port)
  16                 self.gpsd.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)
  17                 self.follower = GPSGotoCoords()
  18
  19         def run(self):
  20                 coords_last = (0, 0)
  21                 for report in self.gpsd:
  22                         if report['class'] == "TPV":
  23                                 if report['mode'] == 0:
  24                                         print "No data from gpsd"
  25                                         continue
  26                                 coords_cur = utm.fromLatLong(report.lat, report.lon)
  27                                 coords_cur = [coords_cur.northing, coords_cur.easting]
  28
  29                                 # Check difference to last goal
  30                                 if np.linalg.norm(np.array(coords_cur) - np.array(coords_last)) > 3:
  31                                         print "Setting new goal"
  32                                         # Set a new goal
  33                                         thread.start_new_thread(self.follower.next_pos, (report.lat, report.lon))
  34                                         coords_last = coords_cur
  35
  36 if __name__ == "__main__":
  37         p = GPSFollowGPSD()
  38         p.run()