Como ya se introdujo en la práctica 1, Dronekit es una librería de python que permite controlar e interactuar con nuestro vehículo controlado por ardupilot mediante Python.
Se puede acceder a la documentación oficial de esta API aquí
Los objetivos de esta práctica son:
Para exportar los datos seguiremos el tutorial de clase, ampliándolo y obteniendo un acabado final más completo y profesional.
El código propuesto sería semejante a este:
"""Importar todas las librerías necesarias"""
import dronekit
import time
import datetime
def test(): #crear la función para poder integrarla en un main
conexion = 'tcp:127.0.0.1:5762' #URI de la conexion
vehicle = dronekit.connect(conexion, wait_ready=True, baud=115200) #crear un objeto de la clase vehicle
print('Vehcicle connected!')
print('starting printing data:')
while True: #bucle en el que
print(vehicle.mode.name)
print(vehicle.location.global_frame)
print(vehicle.airspeed)
time.sleep(5)
vehicle.close()
if __name__ == '__main__':
test()
Este código nos imprime por pantalla cada 5 segundos los valores de posición, modo del piloto automático y velocidad del aire.
En los requisitos se nos pide que básicamente guardemos estos valores junto con una fecha y hora en un fichero de texto .txt.
Sin embargo, profundizaré más en el tema para así guardar más datos interesante y además añadir la posibilidad de exportarlos en un formato .csv o insertándolos en una
tabla de datos en dentro de una base de datos SQL. Se puede ver un ejemplo mucho más completo de la Exportación de datos aquí.
Para lograr el registro de datos, se creará una librería de funciones llamada gestor.py que tendrá todas las funciones de explotación:
Esta función tiene como cometido cubrir los requerimientos que se piden de la práctica, sin embargo, No tiene mucho sentido exportar los datos de esta forma teniendo al alcance de la mano extensiones que permiten guardar los datos de forma más ordenada y eficiente como por ejemplo un .csv o una base de datos SQL
La lógica detrás de esta función será parecida en respecto a las demás, ya que solo cambia la forma de guardar los datos.
Primero de todo, si no se le pasa el valor de un fichero entenderá que su nombre será log, la forma de abrir el fichero será en w, por lo que si
ya tenemos un fichero con ese nombre, se va a machacar y se perderá toda la información que hubiese sido guardado en este. También es necesario pasar como parámetro un objeto de la clase vehicle de dronekit.
Es importante tener en cuenta que esta función se ejecutará indefinidamente hasta que se desarme el rover. Una vez dentro del bucle,
se leen los datos escogidos y se convierten en variables de tipo string, la parte más "complicada" es la de sacar la posición, ya que para ello hay que tratar el string.
Remplazamos los caracteres como ":" y "=" por "," para luego dividir la cadena de caracteres en una lista usando la "," como separador.
Finalmente guardamos en el .txt los valores separados por un tabulador \t.
import dronekit
import time
import datetime
import re
def listener_txt (vehicle, file_name = 'log'):
tiempo = datetime.datetime.now()
file = open(file_name+'.txt','w')
file.write('Latitiud\tLongitud\tAltitud\tGS\tIAS\tHDG\tIsArmable\tstatus\tmode\tarmed\tdate-time\n')
while vehicle.armed == 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])
ground_speed = str(vehicle.groundspeed)
air_speed = str(vehicle.airspeed)
heading = str(vehicle.heading)
IsArmable = str(vehicle.is_armable)
status = str(vehicle.system_status.state)
mode = str(vehicle.mode.name) # settable
armed = str(vehicle.armed)
tiempo = datetime.datetime.now()
line =f'''{lat}\t{lon}\t{alt}\t{ground_speed}\t{air_speed}\t{heading}\t{IsArmable}\t{status}\t{mode}\t{armed}\t{tiempo.strftime("%d/%m/%Y %H:%M:%S")}\n'''
#line = lat+'\t'+lon+'\t'+alt+'\t'+ground_speed+'\t'+air_speed+'\t'+heading+'\t'+ IsArmable +'\t'+status+'\t'+mode+'\t'+armed+'\t'+tiempo.strftime("%d/%m/%Y %H:%M:%S")+'\n'
print('line to insert:\n'+line)
file.write(line)
time.sleep(1)
file.close()
De modo similar al anterior, podemos crear un .csv que además facilita la lectura y el procesado de la información:
def listener_csv (vehicle, file_name = 'log'):
import csv
tiempo = datetime.datetime.now()
file = open(file_name+'.csv','w')
file.write('Latitiud,Longitud,Altitud,GS,IAS,HDG,IsArmable,status,mode,armed,date-time\n')
while vehicle.armed == 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])
ground_speed = str(vehicle.groundspeed)
air_speed = str(vehicle.airspeed)
heading = str(vehicle.heading)
IsArmable = str(vehicle.is_armable)
status = str(vehicle.system_status.state)
mode = str(vehicle.mode.name) # settable
armed = str(vehicle.armed)
tiempo = datetime.datetime.now()
line =f'''{lat},{lon},{alt},{ground_speed},{air_speed},{heading},{IsArmable},{status},{mode},{armed},{tiempo.strftime("%d/%m/%Y %H:%M:%S")}\n'''
file.write(line)
time.sleep(1)
file.close()
Con este método, podemos guardar los registros dentro de una tabla de datos de SQL. Para este proyecto trabajaremos con
SQLite que nos permite trabajar con una base de datos local sin necesidad de instalar ningún servidor SQL.
Entre las ventajas de trabajar con una base de datos SQL se encuentra la robusteza, optimización y la posiblididad de poder
hacer consultas estructuradas que permiten acceder, filtar y procesar la información de manera más eficiente.
Para ello nuestra función como siempre debe recibir un objeto vehicle y además puede recibir cómo parámetros
opcionales el nombre de la base de datos y en que tabla hacer el registro.
Si la base de datos no existe, se creará. De mismo modo ocurrirá con la tabla.
Nota: cada vez que se accede a una tabla ya creada esta será eliminada y creada de nuevo para tener un registro consistente de los datos.
Por lo que es importante tenerlo en mente ya que podríamos borrar datos de nuestro interés de sesiones pasadas.
El motivo de esta lógica es evitar generar archivos y basura innecesarios en nuestro sistema para así hacer un uso óptimo de nuestra memoria.
def listener_sql(vehicle,database = "ROVER_TELEMETRY", table= "logs"):
import sqlite3 as sql
"""creates a database by default named: ROVER_TELEMETRY if it doesn't exists
"""
conn = sql.connect(f"{database}")
conn.commit()
conn.close()
'''Creates a table for logs, default name logs'''
#IF THE TABLE ALLREADY EXISTS, DELETE IT AND CREATED FROM SCRATCH
conn = sql.connect(database)
cursor= conn.cursor()
querry=f"""DROP TABLE {table};"""
cursor.execute(querry)
conn.commit()
querry=f"""CREATE TABLE IF NOT EXISTS {table} (
Latitud FLOAT,
Longitud FLOAT,
Altitud FLOAT,
GS FLOAT,
IAS FLOAT,
HDG INT,
IsArmable TEXT,
Status VARCHAR(10),
Mode VARCHAR(10),
Armed TEXT,
Date_time DATETIME2 PRIMARY KEY);
"""
cursor.execute(querry)
conn.commit()
conn.close()
while vehicle.armed == 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])
ground_speed = str(vehicle.groundspeed)
air_speed = str(vehicle.airspeed)
heading = str(vehicle.heading)
IsArmable = str(vehicle.is_armable)
status = str(vehicle.system_status.state)
mode = str(vehicle.mode.name)
armed = str(vehicle.armed)
tiempo = datetime.datetime.now()
querry = f"""INSERT INTO {table}
(Latitud,Longitud,Altitud,GS,IAS,HDG,IsArmable,Status,Mode,Armed,Date_time)
VALUES ({lat},{lon},{alt},{ground_speed},{air_speed},{heading},'{IsArmable.upper()}','{status}','{mode}', '{armed.upper()}', '{tiempo.isoformat()}');
"""
print(querry)
conn=sql.connect(database)
cursor= conn.cursor()
cursor.execute(querry)
conn.commit()
conn.close()
Entre otros beneficios que no se han mencionado anteriormente, remarcar que esta función es fácilmente exportable para trabajar con bases de datos de MySQL (o bien MariaDB si estamos trabajando en raspbian) así como PostgreSQL, Lo que permiten compartir los datos en tiempo real con qualquier dispositivo que tenga acceso a dicho servidor.
Para poder visualizar los datos, podemos usar qualquier visualizador como por ejemplo DB Browser
Para controlar el rover, se ha creado otra librería llamada control.py, que incluirá todas las funciones necesarias a lo largo del proyecto.
De momento solo tememos disponible controller(), la cual recibe como parámetro obligatorio un objeto de la clase vehicle. Así como la función compare_location()
que se usa internamente para que controller funcione correctamente.
Por lo que respecta al código, no hay mucho que comentar más que se ha mejorado el código proporcionado originalmente.
Entre las mejoras se encuentran una mejor respuesta del rover (prácticamente no se para) ya que los time.sleep() són más pequeños, lo que significa que realizaremos muchas más
comprovaciones, permintiendo así saber dónde nos encontramos y si es necesario realizar alguna acción.
También significa una mayor precisión ya que la sensibilidad es mucho mayor. También se ha mejorado la sintaxis y otros aspectos menos importantes.
import dronekit
import time
def controller(vehicle):
cmd = vehicle.commands
cmd.download()
cmd.wait_ready()
vehicle.mode = dronekit.VehicleMode('GUIDED')
while vehicle.mode != dronekit.VehicleMode('GUIDED'):
time.sleep(0.1)
vehicle.armed = True
while not vehicle.armed:
time.sleep(0.1)
for command in cmd:
point = dronekit.LocationGlobal(lat=command.x, lon=command.y, alt=command.z)
if command.command == 20:
point = dronekit.LocationGlobal(vehicle.home_location.lat,vehicle.home_location.lon,vehicle.home_location.alt )
vehicle.simple_goto(point,3)
arrived = False
prevLoc = vehicle.location.global_frame
time.sleep(0.5)
while not arrived:
arrived = compare_location(prevLoc, vehicle.location.global_frame)
prevLoc = vehicle.location.global_frame
time.sleep(0.1)
vehicle.armed = False
vehicle.close()
def compare_location(prevLoc, actualLoc):
if prevLoc.lat == actualLoc.lat and prevLoc.lon == actualLoc.lon:
return True
else:
return False
if __name__ == '__main__':
conexion = 'tcp:127.0.0.1:5762'
vehicle = dronekit.connect(conexion, wait_ready=True, baud=115200)
controller(vehicle)
Con los datos guardados podemos rezlizar algunos gráficos interesantes, de momento solo estamos guardando ciertos parámetros pero recordemos que aquí
se muestra una lista más extensa de parámetros como el estado de la batería y el estado del gimbal.
De momento lo que he hecho es crear un mapa usando Folium que es una librería que integra el proyecto de Leaflet .js
y un plot que compara las velocidades respecto al tiempo.
Todo estas funcionalidades se encuentran dentro de una nueva librería llamada gráficas.py
Para mostrar el mapa, se ha empleado la siguiente función:
import folium
import sqlite3 as sql
def mapa(database = "ROVER_TELEMETRY", table= "logs"):
m = folium.Map(location=[41.275077428505135, 1.9854523449599073],zoom_start=18)
conn = sql.connect(database)
cursor = conn.cursor()
result = cursor.execute(f"""SELECT Latitud, Longitud from {table}""")
result= result.fetchall()
conn.close()
lista_puntos=[]
for i in result:
punto = (i[0],i[1])
lista_puntos.append(punto)
folium.PolyLine(lista_puntos).add_to(m)
m.save("mapa.html")
def velocidad(database = "ROVER_TELEMETRY", table= "logs"):
import datetime
import matplotlib.pyplot as plt
conn = sql.connect(database)
cursor = conn.cursor()
result = cursor.execute(f"""SELECT GS,IAS,Date_time Longitud from {table}""")
result= result.fetchall()
conn.close()
GS=[]
IAS=[]
time=[]
for i in result:
GS.append(i[0])
IAS.append(i[1])
time.append(datetime.datetime.strptime(i[2],"%Y-%m-%dT%H:%M:%S.%f"))
plt.plot(time,GS,label='GS')
plt.plot(time,IAS,label='IAS')
plt.legend()
plt.xlabel('date-time')
plt.ylabel('speed [m/s]')
plt.show()
if __name__ == '__main__':
velocidad()
Inicialmente se pensó en hacer una interfaz gráfica simple usando la librería tinker de python.
El problema fué que dronekit es un proyecto muy antiguo, desarrollado en python 2.7 y luego adaptado para las versiones 3.x.
Por lo que hay ciertos problemas de incompatibilidades con intérpretes más avannzados, pues esta librería se encuentra discontinuada y no recibe soporte.
El main creado es es siguiente:
import threading
import dronekit
import time
import datetime
from control import controller
from gestor import *
from graficas import *
def mandunga():
conexion = 'tcp:127.0.0.1:5762'
vehicle = dronekit.connect(conexion, wait_ready=True, baud=115200)
thread=threading.Thread(target=listener_sql, args=( vehicle,))
thread.start()
controller(vehicle=vehicle)
vehicle.close()
if __name__ == '__main__':
mandunga()
mapa()
velocidad()
Importante notar que se crea un thread para así poder ejecutar dos funciones al mismo tiempo de forma asíncrona ya que como está pensada la lógica,
no sería posible controlar el rover y a la vez registrar los datos de navegación.
Finalmente, se ejecutan las funciones de análisis de datos explicadas previamente.
Por último, solo queda probar la conexión entre nuestra aplicación y el rover usando el bus de telemetría, para ello es necesitamos identificar en que puerto se encuentra conectado nuestro rover. Para sistemas operativos Windows debemos ir al administrador de dispositivos y fijarnos el el apartado de puertos COMimport dronekit mientras que si trabajamos en sistemas Unix, deberíamos usar el comando lsusb. En mi caso, este se encuentra conectado en el puerto COM10.
Finalmente, debemos ajustar los baudios del puerto COM, para ello iremos a ardupilot y en CONFIG Full parameter list, buscaremos la opción de SERIAL2_BAUD, ahí podremos escoger el ratio de baudios que más nos convenga para nuestro sistema, lo más importante es que todas las partes funcionen con el mismo ratio de baudios.
para realizar la conexión, solo debemos cambiar los variables de conexion y baud. Tal y cómo se muestra subrayado en el siguiente código.
import dronekit
import time
import datetime
from gestor import *
def test():
#conexion = 'tcp:127.0.0.1:5762'
conexion = 'com10' #'tcp:127.0.0.1:5762'
baud = 57600 #57600 ==> radio, baud=115200 net/usb
vehicle = dronekit.connect(conexion, wait_ready=True, baud=baud)
print('Vehcicle connected!')
print('starting printing data:')
while True:
print(vehicle.mode.name)
print(vehicle.location.global_frame)
print(vehicle.airspeed)
listener_sql(vehicle=vehicle)
break
time.sleep(5)
vehicle.close()
if __name__ == '__main__':
test()
Si al ejecutar el código anterior recibimos por pantalla su estado, posición, y velocidad, significa que la conexión se ha realizado con éxito.