From: Erik Andresen <erik@vontaene.de>
Date: Sat, 25 Nov 2017 11:13:09 +0000 (+0100)
Subject: Added script to follow coordinates from another gpsd
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=e8c2719dce145ad9f2a451849c722ca06c8ef1a6;p=ros_wild_thumper.git

Added script to follow coordinates from another gpsd
---

diff --git a/scripts/gps_follow_gpsd.py b/scripts/gps_follow_gpsd.py
new file mode 100755
index 0000000..2747504
--- /dev/null
+++ b/scripts/gps_follow_gpsd.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import thread
+import gps
+import numpy as np
+from geodesy import utm
+from gps_follow_waypoints import GPSGotoCoords
+
+DEFAULT_GPSD_HOSTNAME="wildthumper"
+DEFAULT_GPSD_PORT="2948"
+
+class GPSFollowGPSD:
+	def __init__(self, hostname=DEFAULT_GPSD_HOSTNAME, port=DEFAULT_GPSD_PORT):
+		self.gpsd = gps.gps(host=hostname, port=port)
+		self.gpsd.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)
+		self.follower = GPSGotoCoords()
+
+	def run(self):
+		coords_last = (0, 0)
+		for report in self.gpsd:
+			if report['class'] == "TPV":
+				if report['mode'] == 0:
+					print "No data from gpsd"
+					continue
+				coords_cur = utm.fromLatLong(report.lat, report.lon)
+				coords_cur = [coords_cur.northing, coords_cur.easting]
+
+				# Check difference to last goal
+				if np.linalg.norm(np.array(coords_cur) - np.array(coords_last)) > 3:
+					print "Setting new goal"
+					# Set a new goal
+					thread.start_new_thread(self.follower.next_pos, (report.lat, report.lon))
+					coords_last = coords_cur
+
+if __name__ == "__main__":
+	p = GPSFollowGPSD()
+	p.run()