Add gps follow me example from dronekit with zmq

DroneKit/FollowByGPS/client.py

@@ -0,0 +1,10 @@
+import zmq
+import sys
+
+context = zmq.Context()
+
+sock = context.socket(zmq.REQ)
+sock.connect("tcp://127.0.0.1:5678")
+while True:
+    sock.send(raw_input('Get new pos(y/n): '))
+    print 'Pos:', sock.recv()

DroneKit/FollowByGPS/followMe.py

@@ -0,0 +1,118 @@
+
+from dronekit import connect, VehicleMode, LocationGlobalRelative
+import gps
+import socket
+import time
+import sys
+
+import zmq
+context = zmq.Context()
+sock = context.socket(zmq.REQ)
+sock.connect("tcp://127.0.0.1:5678")
+
+def nextLoc():
+    sock.send('y')
+    message = sock.recv()
+    tup = (float(message.split(', ', 2)[0], float(message.split(', ', 2)[1])
+    return tup
+
+
+#Set up option parsing to get connection string
+import argparse  
+parser = argparse.ArgumentParser(description='Tracks GPS position of your computer (Linux only).')
+parser.add_argument('--connect', 
+                   help="vehicle connection target string. If not specified, SITL automatically started and used.")
+args = parser.parse_args()
+
+connection_string = args.connect
+sitl = None
+
+
+#Start SITL if no connection string specified
+if not connection_string:
+    import dronekit_sitl
+    sitl = dronekit_sitl.start_default()
+    connection_string = sitl.connection_string()
+
+# Connect to the Vehicle
+print 'Connecting to vehicle on: %s' % connection_string
+vehicle = connect(connection_string, wait_ready=True)
+
+
+def arm_and_takeoff(aTargetAltitude):
+    """
+    Arms vehicle and fly to aTargetAltitude.
+    """
+
+    print "Basic pre-arm checks"
+    # Don't let the user try to arm until autopilot is ready
+    while not vehicle.is_armable:
+        print " Waiting for vehicle to initialise..."
+        time.sleep(1)
+
+
+    print "Arming motors"
+    # Copter should arm in GUIDED mode
+    vehicle.mode = VehicleMode("GUIDED")
+    vehicle.armed = True    
+
+    while not vehicle.armed:      
+        print " Waiting for arming..."
+        time.sleep(1)
+
+    print "Taking off!"
+    vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
+
+    # Wait until the vehicle reaches a safe height before processing the goto (otherwise the command 
+    #  after Vehicle.simple_takeoff will execute immediately).
+    while True:
+        print " Altitude: ", vehicle.location.global_relative_frame.alt      
+        if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Trigger just below target alt.
+            print "Reached target altitude"
+            break
+        time.sleep(1)
+
+
+
+try:
+    # Use the python gps package to access the laptop GPS
+    gpsd = gps.gps(mode=gps.WATCH_ENABLE)
+
+    #Arm and take off to altitude of 5 meters
+    arm_and_takeoff(5)
+
+    while True:
+
+        if vehicle.mode.name != "GUIDED":
+            print "User has changed flight modes - aborting follow-me"
+            break    
+
+        # Read the GPS state from the laptop
+        pos = nextLoc()
+
+        # Once we have a valid location (see gpsd documentation) we can start moving our vehicle around
+        if (gpsd.valid & gps.LATLON_SET) != 0:
+            altitude = 30  # in meters
+            dest = LocationGlobalRelative(pos[0], pos[1])
+            print "Going to: %s" % dest
+
+            # A better implementation would only send new waypoints if the position had changed significantly
+            vehicle.simple_goto(dest)
+
+            # Send a new target every two seconds
+            # For a complete implementation of follow me you'd want adjust this delay
+            time.sleep(2)
+
+except socket.error:
+    print "Error: gpsd service does not seem to be running, plug in USB GPS or run run-fake-gps.sh"
+    sys.exit(1)
+
+#Close vehicle object before exiting script
+print "Close vehicle object"
+vehicle.close()
+
+# Shut down simulator if it was started.
+if sitl is not None:
+    sitl.stop()
+
+print("Completed")

DroneKit/FollowByGPS/servers.py

@@ -0,0 +1,46 @@
+import threading
+import time
+import socket
+import zmq
+
+pos = '22,22'
+
+class Server (threading.Thread):
+    def __init__(self):
+        threading.Thread.__init__(self)
+
+    def run(self):
+        s = socket.socket()
+        host = socket.gethostname()
+        port = 12345
+        s.bind((host, port))
+        s.listen(5)
+        c = None
+        while True:
+            if c is None:
+                c, addr = s.accept()    # wait till it connects
+            else:
+                pos = c.recv(1024)      # recv pos 
+
+class Serverzmq (threading.Thread):
+    def __init__(self):
+        threading.Thread.__init__(self)
+    def run(self):
+        context = zmq.Context()
+
+        sock = context.socket(zmq.REP)
+        sock.bind("tcp://127.0.0.1:5678")
+        while True:
+            message = sock.recv()
+            if message == 'y':  
+                print 'Received request, sending new pos...', pos
+                sock.send(pos)
+            else:
+                continue        
+
+
+server = Server()
+zmqserver = Serverzmq()
+server.start()
+zmqserver.start()
+