asr_vosk: Allow to handle keyword and command in one sentence
[ros_wild_thumper.git] / 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()