Práctica 6: Detección de intruso:

En esta práctica se implementarán todos los conocimientos adquiridos a lo largo del curso junto a un algoritmo de predicción de conflictos empleados en la aviación.

Para esta parte, se creará un objeto con los métodos necesarios para calcular las taus tal y como se pedía en clase después de leer el paper.
Importante tener en cuenta que para trabajar con el algoritmo, tenemos que usar un sistema cartesiano, mientras que la posición de los objetos se encuentra en formato WSG-84. Por lo que hay que realizar un cambio de coordenadas.

import utm
import numpy as np

class avion:
    
    def __init__(self, lat,lon,vx,vy) -> None:
        self.lat = lat
        self.lon = lon
        self.x,self.y,basura,basura =utm.from_latlon(lat,lon)
        print(self.x,self.y,basura,basura)
        self.vx=vx
        self.vy=vy   
    
def tau(avion1,avion2):
    s=np.array([avion2.x-avion1.x, avion2.y-avion1.y])
    v=np.array([avion2.vx-avion1.vx,avion2.vy-avion1.vy])
    tau =-(np.sum(s**2))/(np.dot(s,v))
    return tau

def tau_mod(avion1,avion2,DMOD):
    s=np.array([avion2.x-avion1.x, avion2.y-avion1.y])
    v=np.array([avion2.vx-avion1.vx,avion2.vy-avion1.vy])
    tau =(DMOD**2-np.sum(s**2))/(np.dot(s,v))
    return tau

if __name__ == '__main__':
    DMOD=0.2
    avion1 = avion(lat=41.27479228277224,lon= 1.985268532366749, vx=1,vy=0)
    avion2 = avion(lat=41.27547342864018, lon=1.9855681672713248, vx=-1,vy=0)
    print('tau =', tau(avion1,avion2))
    print('tau_mod =', tau_mod(avion1,avion2,DMOD=DMOD))

        

Para realizar la desmostración, se ha planteado una ruta que va directamente hacia un obstáculo, el cual se ubica en la punta de la sombra producida por la farola. Por lo que es fácil tener una referencia visual.

Una vez comprobado el correcto funcionamiento del algoritmo, es hora de integrar este junto dronekit, para ello se diseña una función para comprobar el correcto funcionamiento entre módulos emulando los datos que ser recibirian de un canal MQTT.

import json
import numpy as np
import dronekit
from control import *
import paho.mqtt.client as mqtt
import signal
from threading import Thread
from TACAS import avion, tau_mod, tau 


def avoidance(vehicle):
    TAU =1
    DMOD = 1


    LocationGlobal = str(vehicle.location.global_frame)
    LocationGlobal = LocationGlobal.replace(':',',')
    LocationGlobal= LocationGlobal.replace('=', ',')
    LocationGlobal=LocationGlobal.split(',')
    lat = str(LocationGlobal[2])
    lon = str(LocationGlobal[4])
    alt = str(LocationGlobal[6])

    v = np.array(vehicle.velocity).astype(np.float)
    print(v)
    avion1 = avion(lat,lon,1,1)
    avion2 = avion(lat =41.274957,lon =1.984774 ,vx=0,vy=0)
    tau = tau_mod(avion1,avion2,DMOD)
    print(tau)
    if tau<= TAU:
        print('take action')
        vehicle.mode = dronekit.VehicleMode("RTL")


conexion = 'tcp:127.0.0.1:5762'
vehicle = dronekit.connect(conexion, wait_ready=True, baud=115200)
while True:
    avoidance(vehicle)
    time.sleep(1)
        

Esta es la parte crucial de la práctica, pues es indispensable recibir los datos de otros vehículos/obstáculos para poder hacer los cálculos necesarios para evitar los conflictos. Por esto esta parte se encontrará desglosada.

Fichero JSON:

La Telemetría se mandará siguiendo un formato de codificación standard JSON, el cual permite trabajar los datos de forma estructurada y fácil mediante el uso de diccionarios. Se puede encontrar más información aquí, Entre otros datos, se manda un identificador único para saber quien ha sido el emisor del mensaje.

Aquí, se puede ver la función que genera la telemetría

def send_telemetry(vehicle):

    broker = "test.mosquitto.org"
    client =mqtt.Client("Sutton")
    client.connect(broker)
    topic = "excedida/telemetry"
    while True:
        LocationGlobal = str(vehicle.location.global_frame)
        LocationGlobal = LocationGlobal.replace(':',',')
        LocationGlobal= LocationGlobal.replace('=', ',')
        LocationGlobal=LocationGlobal.split(',')
        lat = str(LocationGlobal[2])
        lon = str(LocationGlobal[4])
        alt = str(LocationGlobal[6])
        v = vehicle.velocity
        data ={'ID':'Sutton','lat':lat, 'lon':lon, 'alt': alt, 'vx':v[0], 'vy':v[1], 'vz':v[2] }
        client.publish(topic, json.dumps(data))
        time.sleep(1)
        

Tratamiento de los mensajes:

Una vez recibido los mensajes, estos son procesado y se calcula el conflicto, tomando la medida pertinente.
Si la tau alcanza un valor límite el rover se pondrá en un return to launch para volver al origen y evitar la colisión

def listerner_telemetry(TAU,vehicle):
    global avion1
    global avion2

    def command(client,userdata,cmd):
        avion1 = None
        avion2 = None
        data = str(cmd.payload.decode('utf-8'))
        data = json.loads(data)
        if data['ID'] == 'Sutton':
            avion1 = avion( data['lat'],data['lon'],data['vx'], data['vy'])
        else:
            avion2 = avion( data['lat'],data['lon'],data['vx'], data['vy'])
        tau = 1e9
        if avion1 != None and avion2 != None:
            tau = tau_mod(avion1,avion2,DMOD)
        print(tau)
        if tau<= TAU:
            print('take action')
            vehicle.mode = dronekit.VehicleMode("RTL")

    topic = "excedida/telemetry"
    broker = "test.mosquitto.org"
    client =mqtt.Client("Sutton")
    client.connect(broker)
    client.subscribe(topic)
    client.on_message = command
    client.loop_forever()
        

Emulación de tráfico de datos mediante MQTT:

Para emular el reporte de otras aeronaves se ha diseñado una función que publica la posición de un vehículo ficticio:

def emulate_conflict(lat =41.274957,lon =1.984774 ,alt=0):


    broker = "test.mosquitto.org"
    client =mqtt.Client("LuzDeGas")
    client.connect(broker)
    topic = "excedida/telemetry"

    while True:
        vx,vy,vz = 0,0,0
        data ={'ID':'LuzDeGas','lat':lat, 'lon':lon, 'alt': alt, 'vx':vx, 'vy':vy, 'vz':vz }
        client.publish(topic, json.dumps(data))
        #print(json.dumps(data))
        time.sleep(1)