Uavcourse fall2020

gettingstarted/GCS/GCS_Runner.py

@@ -0,0 +1,73 @@
+import dronekit_sitl
+import dronekit
+import json
+import argparse
+import os
+import threading
+import time
+import signal
+import util
+import logging
+import simplegcs
+from dronekit import connect, VehicleMode, LocationGlobalRelative
+
+# make sure you change this so that it's correct for your system
+ARDUPATH = os.path.join('/', 'home', 'bayley', 'git', 'ardupilot')
+
+
+def main(ardupath=None):
+    if ardupath is not None:
+        global ARDUPATH
+        ARDUPATH = ardupath
+
+
+    connect()
+    print("Registering Drones...")
+    vehicle1=addDrone([41.715446209367,-86.242847096132,0],"Frank")
+
+    print("Taking off...")
+    vehicle1.simple_takeoff(10)
+    time.sleep(10)
+    print("Going somewhere...")
+
+    gotoWaypoint(vehicle1,41.515446209367,-86.342847096132, 40,3)
+
+    # point1 = LocationGlobalRelative(41.515446209367,-86.342847096132, 40)
+    # vehicle1.simple_goto(point1)
+
+    # point2 = LocationGlobalRelative(41.515446209367, -86.342847096132, 40)
+    # vehicle2.simple_goto(point2)
+
+def connect():
+    print("Connecting to Dronology...")
+    print("SITL path: "+ARDUPATH)
+    global GCS
+    GCS = simplegcs.SimpleGCS(ARDUPATH,"simplegcs")
+    GCS.connect()
+    time.sleep(10)
+
+def gotoWaypoint(vehicle,xcoord,ycoord,zcoord, airspeed=10):
+    vehicle.airspeed = airspeed
+    point = LocationGlobalRelative(xcoord,ycoord,zcoord)
+    vehicle.simple_goto(point)
+
+def addDrone(home, name=None):
+    vehicle = GCS.registerDrone(home,name)
+    time.sleep(5)
+    while not vehicle.is_armable:
+        print(" Waiting for vehicle to initialise...")
+        time.sleep(3)
+    print("Arming motors")
+    vehicle.mode = VehicleMode("GUIDED")
+    vehicle.armed = True
+    while not vehicle.armed:
+        print(" Waiting for arming...")
+        time.sleep(1)
+    return vehicle
+
+if __name__ == '__main__':
+    ap = argparse.ArgumentParser()
+    #ap.add_argument('path_to_config', type=str, help='the path to the drone configuration file.')
+    ap.add_argument('--ardupath', type=str, default=ARDUPATH)
+    args = ap.parse_args()
+    main(ardupath=args.ardupath)

gettingstarted/GCS/simplegcs.py

@@ -0,0 +1,94 @@
+import dronekit_sitl
+import dronekit
+import json
+import argparse
+import os
+import threading
+import time
+import signal
+import util
+import logging
+
+_LOG = logging.getLogger(__name__)
+_LOG.setLevel(logging.INFO)
+
+fh = logging.FileHandler('main.log', mode='w')
+fh.setLevel(logging.INFO)
+formatter = logging.Formatter('| %(levelname)6s | %(funcName)8s:%(lineno)2d | %(message)s |')
+fh.setFormatter(formatter)
+_LOG.addHandler(fh)
+
+DO_CONT = False
+MESSAGE_FREQUENCY=1.0
+
+class SimpleGCS:
+    sitls = []
+    vehicles = {}
+
+    def __init__(self,ardupath,g_id="default_groundstation"):
+        self.g_id=g_id
+        self.ardupath=ardupath
+
+    def registerDrone(self, home,name,virtual=True):
+        if name is  None:
+            name = get_vehicle_id(len(self.vehicles))
+
+        if virtual:
+            vehicle, sitl = self.connect_virtual_vehicle(len(self.vehicles), home)
+            self.sitls.append(sitl)
+        else:
+            vehicle = self.connect_physical_vehicle(home)
+        handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, self.dronology._g_id,name)
+
+        self.vehicles[name]=vehicle
+        self.dronology.send(str(handshake))
+        print("New drone registered.."+handshake.__str__())
+        return vehicle
+
+    def connect(self):
+        self.dronology = util.Connection(None, "localhost", 1234,self.g_id)
+        self.dronology.start()
+        global DO_CONT
+        DO_CONT = True
+        w0 = threading.Thread(target=state_out_work, args=(self.dronology, self.vehicles))
+        w0.start()
+
+    def connect_virtual_vehicle(self,instance, home):
+        home_ = tuple(home) + (0,)
+        home_ = ','.join(map(str, home_))
+        sitl_defaults = os.path.join(self.ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm')
+        sitl_args = ['-I{}'.format(instance), '--home', home_, '--model', '+', '--defaults', sitl_defaults]
+        sitl = dronekit_sitl.SITL(path=os.path.join(self.ardupath, 'build', 'sitl', 'bin', 'arducopter'))
+        sitl.launch(sitl_args, await_ready=True)
+
+        tcp, ip, port = sitl.connection_string().split(':')
+        port = str(int(port) + instance * 10)
+        conn_string = ':'.join([tcp, ip, port])
+
+        vehicle = dronekit.connect(conn_string)
+        vehicle.wait_ready(timeout=120)
+
+        return vehicle, sitl
+
+    def connect_physical_vehicle(self, home):
+
+        vehicle = dronekit.connect(home, wait_ready=True)
+        vehicle.wait_ready(timeout=120)
+
+        return vehicle
+
+
+def get_vehicle_id(i):
+    return 'drone{}'.format(i)
+
+
+def state_out_work(dronology, vehicles):
+    while DO_CONT:
+       # for i, v in enumerate(vehicles):
+        for name, v in vehicles.iteritems():
+            state = util.StateMessage.from_vehicle(v,dronology._g_id,name)
+            state_str = str(state)
+            _LOG.info(state_str)
+            dronology.send(state_str)
+
+        time.sleep(MESSAGE_FREQUENCY)

gettingstarted/GCS/util.py

@@ -0,0 +1,173 @@
+import json
+import os
+import threading
+import socket
+import time
+from boltons import socketutils
+
+
+class DronologyMessage(object):
+    def __init__(self, m_type,g_id, uav_id, data):
+        self.m_type = m_type
+        self.g_id=g_id
+        self.uav_id = uav_id
+        self.data = data
+
+    def __str__(self):
+        return json.dumps({'type': self.m_type,
+                           'sendtimestamp': long(round(time.time() * 1000)),
+                           'uavid': str(self.uav_id),
+                           'groundstationid': str(self.g_id),
+                           'data': self.data})
+
+    def __repr__(self):
+        return str(self)
+
+
+class DroneHandshakeMessage(DronologyMessage):
+    def __init__(self, g_id,uav_id, data, p2sac='../cfg/sac.json'):
+        super(DroneHandshakeMessage, self).__init__('handshake',g_id, uav_id, data)
+        self.p2sac = p2sac
+
+    @classmethod
+    def from_vehicle(cls, vehicle,g_id, v_id, p2sac='../cfg/sac.json'):
+        battery = {
+            'voltage': vehicle.battery.voltage,
+            'current': vehicle.battery.current,
+            'level': vehicle.battery.level,
+        }
+
+        lla = vehicle.location.global_relative_frame
+        data = {
+            'home': {'x': lla.lat,
+                     'y': lla.lon,
+                     'z': lla.alt},
+            'safetycase': json.dumps({})}
+        return cls(g_id,v_id, data)
+
+
+class StateMessage(DronologyMessage):
+    def __init__(self, g_id, uav_id, data):
+        super(StateMessage, self).__init__('state',g_id, uav_id, data)
+
+    @classmethod
+    def from_vehicle(cls, vehicle,g_id, v_id, **kwargs):
+        lla = vehicle.location.global_relative_frame
+        att = vehicle.attitude
+        vel = vehicle.velocity
+        battery = {
+            'voltage': vehicle.battery.voltage,
+            'current': vehicle.battery.current,
+            'level': vehicle.battery.level,
+        }
+        data = {
+            'location': {'x': lla.lat, 'y': lla.lon, 'z': lla.alt},
+            'attitude': {'x': att.roll, 'y': att.pitch, 'z': att.yaw},
+            'velocity': {'x': vel[0], 'y': vel[1], 'z': vel[2]},
+            'status': vehicle.system_status.state,
+            'heading': vehicle.heading,
+            'armable': vehicle.is_armable,
+            'airspeed': vehicle.airspeed,
+            'groundspeed': vehicle.airspeed,
+            'armed': vehicle.armed,
+            'mode': vehicle.mode.name,
+            'batterystatus': battery
+        }
+
+        return cls(g_id,v_id, data)
+
+class Connection:
+    _WAITING = 1
+    _CONNECTED = 2
+    _DEAD = -1
+
+    def __init__(self, msg_queue=None, addr='localhost', port=1234, g_id='default_groundstation'):
+        self._g_id = g_id
+        self._msgs = msg_queue
+        self._addr = addr
+        self._port = port
+        self._sock = None
+        self._conn_lock = threading.Lock()
+        self._status = Connection._WAITING
+        self._status_lock = threading.Lock()
+        self._msg_buffer = ''
+
+    def get_status(self):
+        with self._status_lock:
+            return self._status
+
+    def set_status(self, status):
+        with self._status_lock:
+            self._status = status
+
+    def is_connected(self):
+        return self.get_status() == Connection._CONNECTED
+
+    def start(self):
+        threading.Thread(target=self._work).start()
+
+    def stop(self):
+        self.set_status(Connection._DEAD)
+
+    def send(self, msg):
+        success = False
+        with self._conn_lock:
+            if self._status == Connection._CONNECTED:
+                try:
+                    self._sock.send(msg)
+                    self._sock.send(os.linesep)
+                    success = True
+                except Exception as e:
+                    print('failed to send message! ({})'.format(e))
+
+        return success
+
+    def _work(self):
+        """
+        Main loop.
+            1. Wait for a connection
+            2. Once connected, wait for commands from dronology
+            3. If connection interrupted, wait for another connection again.
+            4. Shut down when status is set to DEAD
+        :return:
+        """
+        cont = True
+        while cont:
+            status = self.get_status()
+            if status == Connection._DEAD:
+                # Shut down
+                cont = False
+            elif status == Connection._WAITING:
+                # Try to connect, timeout after 10 seconds.
+                try:
+                    sock = socket.create_connection((self._addr, self._port), timeout=5.0)
+                    self._sock = socketutils.BufferedSocket(sock)
+                    handshake = json.dumps({'type': 'connect', 'groundstationid': self._g_id})
+                    self._sock.send(handshake)
+                    self._sock.send(os.linesep)
+                    self.set_status(Connection._CONNECTED)
+                except socket.error as e:
+                    print('Socket error ({})'.format(e))
+                    time.sleep(10.0)
+            else:
+                # Receive messages
+                try:
+                    msg = self._sock.recv_until(os.linesep, timeout=0.1)
+                    if self._msgs is not None:
+                        self._msgs.append(msg)
+                except socket.timeout:
+                    pass
+                except socket.error as e:
+                    print('connection interrupted! ({})'.format(e))
+                    self._sock.shutdown(socket.SHUT_RDWR)
+                    self._sock.close()
+                    self._sock = None
+                    self.set_status(Connection._WAITING)
+                    time.sleep(20.0)
+
+        if self._sock is not None:
+            print('Shutting down socket.')
+            self._sock.shutdown(socket.SHUT_WR)
+            print('Closing socket.')
+            self._sock.close()
+            return