Simulación en Gazebo de un Control de Seguimiento de un Sistema UAV-UGV

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Trabajo Fin de Grado Grado en Ingeniería Electrónica Robótica y Mecatrónica Simulación en Gazebo de un Control de Seguimiento de un Sistema UAV-UGV Autor: María Verdejo Villanueva Tutor: Begoña Chiquinquira Arrue Ulles Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2024 Trabajo Fin de Grado Grado en Ingeniería Electrónica Robótica y Mecatrónica Simulación en Gazebo de un Control de Seguimiento de un Sistema UAV-UGV Autor: María Verdejo Villanueva Tutor: Begoña Chiquinquira Arrue Ulles Profesor Titular Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2024 Trabajo Fin de Grado: Simulación en Gazebo de un Control de Seguimiento de un Sistema UAV-UGV Autor: María Verdejo Villanueva Tutor: Begoña Chiquinquira Arrue Ulles El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Agradecimientos A gradecer a mi familia, especialmente a mis padres, Ana y Pedro, por su amor incondicional, por creer en mi y apoyarmme en cada etapa de mi vida académica. Sin su respaldo, este logro no habría sido posible. A mi abuela Sagrario, por nunca dejar de repetirme que soy capaz de lograr todo lo que me proponga y a mi abuelo Jaime por darme este bonito gusto por la ingeniería. Agradecerle también a mi tutora Begoña Chiquinquira Arrue Ulles por su inestimable guía, paciencia y apoyo durante todo el proceso. A mis amigas del colegio, Ana, Julia y Belén, gracias por entenderme y apoyarme. Su amistad ha sido una fuente constante de alegría, su presencia en mi vida ha hecho que los momentos difíciles sean más llevaderos y los momentos felices sean aún más especiales. A mis compañeros de clase y amigos, especialmente a Fer, Ali, Dani, Guille y Rodri, gracias por hacerme reír en los momentos más difíciles. Sus risas y compañía han hecho estos cuatro años una etapa memorable. Gracias a CATEC por darme la oportunidad de trabajar con ellos y enseñarme tanto en tan poco tiempo. Agradecer especialmente a mi tutor Juan Manuel Marredo por guiarme en todo momento y compartir conmigo sus conocimientos. Y a Pari, por ayudarme y entenderme en el duro periodo final de este proyecto. Y por último gracias a la ETSI por darme unos de los mejores años de mi vida. María Verdejo Villanueva Estudiante de Ingeniería Electrónica Robótica y Mecatrónica Sevilla, 2024 I Resumen E ste trabajo presenta el diseño y desarrollo de un sistema de coordinación entre vehículos aéreos no tripulados (UAV) y vehículos terrestres no tripulados (UGV) enfocado en aplicaciones de logística y gestión de inventarios en entornos industriales. El objetivo principal es implementar un sistema en el que el UAV pueda seguir al UGV utilizando marcadores ArUco para estimar la posición relativa. Este enfoque permite que el UAV realice tareas de inspección y monitoreo mientras el UGV proporciona soporte en términos de energía y posicionamiento preciso. Para llevar a cabo este proyecto, se desarrolló un entorno de simulación utilizando ROS y Gazebo, integrando herramientas como Ardupilot y MAVROS para el control del UAV. Se utilizó Blender para modificar los modelos 3D del UAV, añadiendo los marcadores necesarios y ajustando sus características físicas. Los algoritmos de detección y seguimiento se implementaron empleando técnicas avanzadas de visión por computadora y control autónomo, garantizando así la coordinación eficiente entre ambos vehículos. Las simulaciones demuestran la viabilidad del sistema propuesto para aplicaciones donde es necesario un seguimiento continuo y preciso, como en tareas de inspección y monitoreo. III XÍndice Nodo reference_node_UGV 35 yaw_node 36 4.3.2 bt_UAV_node.cpp 36 4.3.3 key_detection.py 36 4.3.4 tf.launch 37 4.3.5 camera_world_tf.cpp 37 4.4 Paquete rs_aruco_detector 37 4.5 Paquete sumministra_pose_estimation 38 5 Simulaciones, Resultados y Discusión 39 5.1 Simulación 39 5.2 Resultados 43 5.3 Discusión 46 5.3.1 Comparación con otros métodos 46 Métodos de control 47 Software de simulación 48 Algoritmo de detección de ArUcos 48 5.3.2 Posibles mejoras y optimizaciones 49 6 Conclusiones y Desarrollos futuros 51 6.1 Conclusión 51 6.2 Desarollos futuros 51 Apéndice A Códigos 53 A.1 ground_target_follower 53 A.2 rs_aruco_detector 81 A.3 suministra_pose_estimation 101 Índice de Figuras 109 Bibliografía 111 1 Introducción S e presentará a continuación el contexto que ha motivado la realización de este trabajo, así como los objetivos bajo los cuales se ha desarrollado. 1.1 Contexto y Motivación La maduración de la globalización, el aumento exponencial del comercio electrónico, la amenaza constante de la disrupción tecnológica y, más recientemente, la pandemia de coronavirus, han cambiado totalmente la forma en la que operan los negocios en el siglo XXI. En este contexto, haciendo un análisis rápido, prevalece una realidad muy clara: la logística está entrando en una década transformadora[1]. En la última década, la internacionalización de la industria nacional y regional ha sido un elemento clave para sobrevivir a la crisis económica y mantener los niveles de producción. Con la crisis económica del “primer mundo” remontando, el siguiente reto consiste en consolidar el mercado, cada vez es más exigente y posicionarse frente a la competencia. La irrupción de las economías emergentes (BRICS: Brasil, Rusia, India, China y Sudáfrica) en el sector industrial mundial, obliga a la Industria 4.0 en Europa a invertir en el nivel de servicio del producto, la calidad y el precio. En definitiva, mejorar su competitividad global [2]. El concepto de “Industria 4.0” se centra en la búsqueda de la mejora continua de procesos productivos, logísticos, comerciales y de gestión mediante el desarrollo de nuevo software y sistemas de análisis que conviertan la enorme cantidad de datos de los que se disponen en información que pueda ser utilizada en la toma de decisiones. Uno de los aspectos críticos para asegurar el éxito de estas “Fábricas del Futuro” es el correcto diseño y gestión de la cadena de suministro en la que interactúan diferentes agentes del sistema: proveedores, fabricantes, distribuidores y comerciantes. Entre los elementos que tienen un mayor impacto en la cadena de suministro se encuentra la logística[ 3 ]. Esta abarca la planificación, organización, gestión, control y ejecución de las operaciones de transporte de carga. Figura 1.1 Evolución de la industria a lo largo de los años [4]. 1 2Capítulo 1. Introducción Por otro lado, los vehículos no tripulados, tanto aéreos como terrestres (UAVs y UGVs) se consideran una tecnología clave para la gestión de las fábricas inteligentes, ya que permiten realizar tareas repetitivas y peligrosas sin casi ninguna intervención o supervisión humana[ 5 ]. En los últimos años, los UAV demostraron ser realmente útiles en campos como la detección remota (por ejemplo, en minería), monitorización en tiempo real, gestión de desastres, vigilancia de fronteras y multitudes, aplicaciones militares, entrega de bienes, agricultura de precisión, inspección de infraestructura o medios y entretenimiento, entre otros. Figura 1.2 Primer robot aéreo o dron inteligente del mundo especializado en realizar tareas de inspección de puentes y estructuras de ingeniería civil [6]. En muchos de estos campos, los UAV realizan tareas que constituyen uno de los pilares de la Industria 4.0: recopilar la mayor cantidad de datos posible de múltiples ubicaciones de forma dinámica. Además, los UAV no solo recopilan datos, sino que también pueden almacenar, procesar e intercambiar información con proveedores o con otros dispositivos [ 7 ]. La amplia adopción de los vehículos no tripulados dependerá de cuatro retos tecnológicos principales: 1) colaboración, capacidad del robot para colaborar con elementos de su entorno (otros robots, equipamiento o personas del entorno) y para realizar tareas complejas dentro de un enjambre colaborativo, 2) autonomía y versatilidad, tareas con una mínima (o nula) intervención humana. El dotar a los robots de la capacidad para realizar más de una función, 3) destreza, capacidad del robot para medir la ejecución correcta de tareas, y su capacidad para corregirlas y 4) efectividad, medida por su precisión, repetitividad y confianza. En este contexto nace el proyecto «SUMINISTRA: Transformación tecnológica y digitalización de la cadena de suministro del futuro», dando respuesta a los retos anteriormente comentados e investigando en cada una de las diferentes tecnologías TICs hacia el despliegue de la Industria 4.0 en el almacén, entendido éste como una fábrica extendida. Para que una empresa pueda determinar dinámicamente su necesidad de suministros, es necesario realizar un seguimiento de sus existencias. Es por ello que, como caso de uso de las diferentes tecnologías, el proyecto se centra en su aplicación en las tareas de inventario. En particular, en estas tareas, los datos masivos disponibles pueden ayudar a las organizaciones a diseñar sistemas modernos de optimización de inventario, predecir y responder a las demandas cambiantes de los clientes, reducir los costos de inventario, obtener una visión holística de los niveles de stock y optimizar el flujo y el almacenamiento. Figura 1.3 Logo del proyecto SUMINISTRA [8]. 1.2 Objetivos 3 1.2 Objetivos El objetivo general o misión del proyecto SUMINISTRA es la optimización y digitalización de la cadena de suministro, centrándose en su corazón, el almacén. En fundamental aumentar la productividad, la seguridad y ahorrar tiempo en la realización de inventarios, inspecciones y expediciones, así como disponer de un sistema de seguimiento y trazabilidad de los procesos operativos. Por lo tanto, el objetivo principal es desarrollar un sistema integral para la gestión y toma de inventario en entornos industriales utilizando drones y vehículos guiados autónomos. Este sistema busca mejorar la visibilidad de los activos en tiempo real, así como verificar la disposición y estado de las mercancías en las ubicaciones pertinentes, sincronizando esta información con el sistema ERP del almacén. Figura 1.4 Visión general de sistema de coordinación UAV-UGV [8]. El sistema mencionado estará compuesto por un UAV y un UGV. El primero de ellos se encargará de detectar el inventario utilizando varias tecnologías avanzadas integradas en su sistema. A continuación se describen los principales métodos y herramientas utilizadas para la detección del inventario por parte del dron. • Cámara y Software de visión artificial. Los drones están equipados con cámaras y software de visión artificial que permiten la lectura de códigos de barra y etiquetas RFID. Esta tecnología facilita la identificación de cada unidad de mercancía y su ubicación exacta dentro del almacén. • Mapeo de almacén. El dron asignará el código leído a la posición exacta de la mercancía crenado un mapeo del almacén. Por otro lado, el UGV tiene varios roles importantes. • Provisión de Energía al UAV El UGV será el encargado de proporcionarle energía al UAV a través de un cable, aumentando así su autonomía y permitiendo operaciones más prolongadas. • Navegación y posicionamiento preciso. Utilizando sistemas de navegación, guiado y control, el UGV se posiciona de manera precisa dentro del almacén, que al estar sincronizado con el UAV, para compartir datos en tiempo real, le proporciona al dron la posición global, lo cual permite al UAV navegar con precisión. 4Capítulo 1. Introducción • Detección y comunicación de posición. El UGV tiene integrada una cámara que, junto a unos marcadores ArUcos integrados en el UAV, permite enviarle la posición exacta del dron, facilitando una coordinación precisa entre ambos vehículos. Figura 1.5 Sistema que se plantea como objetivo. El objetivo de este Trabajo Fin de Grado es desarrollar una simulación en la que se pueda probar el funcionamiento tanto del control que se ha desarrollado para el UAV como el algoritmo de detección por medio de marcadores ArUcos que se ha integrado. Esta simulación tiene como propósito proporcionar una visión del resultado final del proyecto, así como verificar el correcto funcionamiento del control. 1.3 Estructura del trabajo La estructura de este documento viene dada por diferentes secciones que pretenden exponer el tema de forma clara, aportar un contexto amplio sobre el mismo así como mostrar la investigación e implementación que se ha llevado a cabo junto con pruebas que la verifican. Concretamente, el apartado “Estado del arte” describe las tecnologías y metodologías relevantes para sistemas robóticos UAV y UGV, explorando sus capacidades, aplicaciones y avances tecnológicos. También se discuten las simulaciones en robótica, los marcadores ArUco con OpenCV, los Behavior Trees y el formato SDF, elementos clave para el desarrollo y control de robots autónomos. En “Metodología y Diseño del Sistema” se expone el enfoque práctico adoptado para desarrollar la simulación del sistema UAV-UGV. Se describen los componentes hardware ysoftware utilizados, así como una descripción detallada del diseño del sistema, incluyendo el control desarrollado y el algoritmo de detección implementado. Por otra parte, en “Desarrollo” se detallan los pasos seguidos para la modificación del modelo UAV con marcadores usando Blender, así como la implementación de algoritmos y sistemas necesarios para el control y la simulación del UAV, proporcionando una visión integral del proceso desde la concepción hasta la aplicación práctica. “Simulaciones, Resultados y Discusión" muestra, como su propio nombre indica, los resultados finales obtenidos a través de una serie de gráficas derivadas de simulaciones detalladas. Cada 1.3 Estructura del trabajo 5 resultado se acompaña de su correspondiente análisis para proporcionar una visión clara de la efectividad y precisión del sistema desarrollado. Para concluir, en el apartado “Conclusiones y Desarrollos futuros” aporto mis pensamientos finales sobre el desarrollo de este proyecto de principio a fin. Además, se introducen brevemente varios puntos que podrían mejorarse en futuros esfuerzos de desarrollo. 2 Estado del Arte E n este capítulo se presenta una revisión del estado del arte en tecnologías y metodologías relevantes para sistemas robóticos UAV y UGV. Se exploran las capacidades, aplicaciones y avances tecnológicos de los UAVs y UGVs, así como la integración de ambos sistemas para misiones complejas. Además, se discuten las simulaciones en robótica, los marcadores ArUco con OpenCV, los Behavior Trees y el formato SDF, elementos que han sido claves para el desarollo de este trabajo. 2.1 UAVs Un vehículo aéreo no tripulado (UAV), comúnmente conocido como dron, hace referencia a una aeronave que vuela sin tripulación, reutilizables, capaz de mantener de manera autónoma un nivel de vuelo controlado y sostenido, y propulsado por motores de explosión, eléctrico o de reacción [ 9 ]. Figura 2.1 UAV convencional con geometría compatible con multirotor y gimbal [10]. El diseño de los UAVs presenta una amplia gama de formas, tamaños, configuraciones y características [ 11 ]. Existen dos variantes: los que son controlados desde una ubicación remota, y aquellos de vuelo autónomo a partir de planes de vuelo preprogramados gracias a sistemas de automatización dinámica. Los UAVs se pueden clasificar según varios criterios, según su uso, estructura y número de hélices, así como información de las categorías de drones establecidas en los reglamentos europeos que rigen toda la actividad aérea relacionadas con los vehículos aéreos no tripulados. En primer lugar, hay que destacar la clasificación de drones según estructura o forma, pudiéndose hacer dos distinciones básicas: de ala fija y de ala rotatoria. Los drones de ala fija son aquellos que aprovechan la aerodinámica para su elevación. Tienen una composición similar a los aviones, cuerpo alargado y alas que salen del cuerpo principal central, que generan la fuerza de sustentación en el aire. Las ventajas de los drones de ala fija es que normalmente tienen una mayor eficiencia energética 7 8Capítulo 2. Estado del Arte respecto a los de ala rotatoria, por lo que son perfectos para los vuelos de largo alcance y duración. Entre sus principales usos, esta categoría de drones se utiliza para fotogrametría, agricultura de precisión y vigilancia, entre otros. Por otro lado, también existen los drones de ala rotatoria, o multirrotores. Son los más comunes en el mercado actual por su versatilidad, los cuales logran su sustentación por medio de las hélices impulsadas por motores que se ubican en cada uno de sus brazos. Este tipo de drones son bastante estables, por lo que son muy usados para realizar todo tipo de actividades, sistemas de vigilancia, seguridad e intervenciones de emergencia, o incluso grabaciones aéreas para cine y televisión. Gracias a las características de los drones de ala rotatoria, es posible que estos UAVs puedan mantenerse en el aire sobrevolando un mismo punto, y tienen la habilidad de elevarse y aterrizar de manera vertical, por lo que se pueden usar en espacios reducidos si se tiene un correcto control de manejo. Figura 2.2 Comparación UAVs de ala fija y de ala rotatoria. Estos a su vez se subdividen en distintas clases de drones, según el número de hélices o brazos que posean. Esta es la clasificación de los drones más usada, ya que utiliza el aspecto del dron para poder diferenciarlos entre ellos, por lo tanto es sencillo identificarlos. Éstos pueden ser distintos tamaños y con funciones diversas, pero su característica principal es la cantidad de brazos que poseen y por consiguiente la cantidad de hélices. En esta clasificación se pueden encontrar 5 tipos de drones: tricópteros, cuadricópteros, hexacópteros, octocópteros y coaxiales. El UAV utilizado en este trabajo se trata de un cuadricóptero, probablemente el tipo de dron más común en el mercado. Se trata de un vehículo aéreo que cuenta con 4 brazos y en cada uno de ellos se posiciona un motor. Gracias a su configuración equidistante ofrece gran estabilidad. Figura 2.3 Tipos de UAVs segun el número de brazos [12]. 2.1 UAVs 9 En segundo lugar, también se pueden clasificar los drones según su método de control. Los drones autónomos son aquellos que no necesitan un piloto operando en tiempo real para controlarlos. Es decir, este tipo de drones se mueve de forma autónoma de acuerdo a una programación previa que le guía por los puntos de interéss que debe recorrer. Los drones que operan por control remoto son aquellos que vuelan controlados remotamente por un piloto. Es el tipo de dron más habitual de todos y puede ser controlado mediante un mando con cámara. Por último, cabe destacar la clasificación según la normativa de drones de AESA (Agencia Estatal de Seguridad Aérea) [ 13 ]. De acuerdo a dicha normativa, existen 6 clases de drones que se clasifican de acuerdo a su masa máxima al despegue o MTOW(Maximum Take Off Weight); pero también se incluyen una serie de características que debe cumplir el dron para ser considerado dentro de cada categoría. Entre las categorías de drones establecidas en los Reglamentos (UE) 2019/947 y (UE) 2019/945, se enumeran las clases que van desde C0 hasta C6. Las dos últimas clases han sido incorporadas en el Reglamento Delegado (UE) 2020/1058 p, donde se han incluido estas 2 que serán las clases utilizadas en los escenarios estándar europeos. Los requisitos de masa máxima al despegue para las clases C0, C1 y C2 son superior a 250g, menor de 900g e inferior a 4 kg respectivamente. Las cases C3, C4, C5 y C6 no deben superar los 25 kg. Las diferencias entre las clases C3, C4, C5 y C6 de drones se basan en el sistemas de seguridad y control, protección de datos, identificación, y capacidades de vuelo automatizado y nocturno. Figura 2.4 Clases enumeradas en las categorias de drones de los Reglamentos (UE) 2019/947 y (UE) 2019/945 [14]. Los UAVs tienen multitud de aplicaciones [ 15 ], y aunque su uso no está muy extendido, su demanda está en constante aumento. Existen drones tanto para uso civil como comercial, pero su desarrollo inicial se llevó a cabo en la industria militar, donde se les conocía como Vehículos de Combate No Tripulados. Los drones civiles son utilizados principalmente para fotografía y videografía aérea; los drones comerciales, empleados en industrias como la agricultura para monitoreo de cultivos [ 16 ], en logística para la entrega de paquetes, y en inspección de infraestructuras como líneas eléctricas y torres de comunicación. También están los drones utilizados en emergencias y rescate, que ayudan en la búsqueda de personas desaparecidas y en la evaluación de daños tras desastres naturales. Por otro lado, los drones militares abarcan una amplia gama de aplicaciones, desde vigilancia y reconocimiento hasta combate directo. Entre estos se encuentran los UAVs de vigilancia, diseñados para recopilar información y realizar misiones de reconocimiento, y los Vehículos de Combate No Tripulados (UCAVs), que están armados y preparados para llevar a cabo ataques. 3 Metodología y Diseño del Sistema E n este capítulo se expondrá el enfoque práctico adoptado para desarrollar la simulación del sistema UAV-UGV. Se abordarán todos los componentes que se han utilizado para el desarrollo de este trabajo, incluyendose una explicación detallada de su aplicación específica. 3.1 Componentes hardware A continuación, se procederá a describir los componentes hardware que se han empleado en la realización de este proyecto. Únicamente se ha utilizado un ordenador portátil donde se ejecuta la simulación. Las características del ordenador son las siguientes. El ordenador es un Intel Core i7-10875H que cuenta con una CPU cuya frecuencia base es de 2.30 GHz y puede alcanzar una frecuencia máxima de 5.10 GHz mediante la tecnología Turbo Boost. Dispone de 8 núcleos físicos y, gracias a la tecnología Hyper-Threading de Intel, puede manejar hasta 16 subprocesos simultáneamente. Además, está equipado con una caché L1 de 32 KB por núcleo para datos e instrucciones (totalizando 256 KB cada una), una caché L2 de 256 KB por núcleo (2 MB en total) y una caché L3 compartida de 16 MB. También cuenta con una memoria RAM de 16GB y una capacidad de almacenamiento de disco duro de 230 GB. Figura 3.1 Modelo de ordenador empleado [36]. 3.2 Componentes software En la realización de este proyecto, se han integrado diversas herramientas y plataformas de software que facilitan el desarrollo, simulación y control del sistema. Cada uno de estos componente juega un papel crucial en diferentes etapas del proyecto, desde la programación y configuración necesaria para la simulación hasta la implementación del control en tiempo real. A continuación, se presenta una descripción de los principales componentes de software utilizados. 17 18 Capítulo 3. Metodología y Diseño del Sistema Figura 3.2 Logos de componentes software utilizados. ROS, conocido por Robot Operating System, es una plataforma de software diseñada para aplicaciones robóticas. Contiene una colección de herramientas y librerías cuyo objetivo es crear un software de propósito general suficientemente robusto para distintas plataformas. Debido a que ROS es de código abierto, tienes la flexibilidad para decidir dónde y cómo utilizar ROS, así como la libertad para personalizarlo según tus necesidades. Además, ROS no es exclusivo, no necesitas elegir entre ROS u otro conjunto de software; ROS se integra fácilmente con tu software existente para aplicar sus herramientas a tu problema [37]. ROS es una plataforma utilizada a nivel mundial, que ofrece gran cantidad de recursos y soporte en su página oficial. Se encontrará una amplia gama de tutoriales que abarcan desde conceptos básicos hasta niveles avanzados, lo que te permite adentrarte en ROS y comprender su funcionamiento . Además la comunidad de ROS es activa y accesible a través de foros donde se discuten estrategias para abordar diversas tareas. Por lo que es muy útil a la hora de solucionar problemas que puedan surgir ya que existen referencias para casi cualquier tipo de error. Para este proyecto, ha sido indispensable la integración de ROS, ya que sin ella no habría sido posible establecer la comunicación entre Simulink y Gazebo. En concreto se hace uso de la versión ROS Noetic en el sistema operativo Ubuntu 20.04 [38]. Una vez se descarga e instala correctamente se procede a crear el espacio de trabajo: mkdir -p ~/catkin_ws_ardupilot/src cd ~/catkin_ws_ardupilot/ catkin_make Si se observa la estructura del espacio de trabajo generado, después de que se ejecuten los comandos anteriores, se pueden encontrar tres carpetas, como se puede apreciar en la Figura 3.3. Figura 3.3 Estructura del espacio de trabajo de ROS. 3.2 Componentes software 19 En primer lugar se encuentra la carpeta build, recoge los archivos que se usan en la compilación cuando se ejecuta el comando catkin_make y su configuración se determina en el archivo CMake- List.txt, el cual le indica a catkin que paquetes del código fuente debe compilar. En segundo lugar se observa la carpeta devel, la cual contiene tanto los ejecutables del proyecto que se han creado en la compilación como las posibles librerías que también existan. Y por último se encuentra la carpeta src, la cual contiene el código fuente del proyecto, es decir, en ella será donde se creen y manejen los paquetes. Dentro de esta carpeta será donde se instalen o creen los paquetes necesarios. Para este proyecto se puede observar la siguiente estructura en la Figura 3.4, dentro de la carpeta src. Figura 3.4 Estructura de la carpeta src del espacio de trabajo. Como se puede observar en la Figura 3.4, para el desarrollo de esta simulación ha sido imprescindible la creación de un paquete denominado ground_target_follower, así como la instalación de varios paquetes adicionales, cuya explicación se proporcionará más adelante. A continuación se detallarán los componentes de ROS que se utilizará en este proyecto: • Nodos. Son la unidad básica de procesamiento en ROS. Un nodo es un ejecutable que realiza cálculos y comunica información. Los nodos pueden interactuar entre sí y con el nodo maestro de ROS, y tienen la capacidad de subscribirse a diversos mensajes, publicarlos o utilizar servicios. Cada nodo está diseñado para realizar una tarea específica. • Tópicos. Los tópicos se utilizan para la comunicación asíncrona entre nodos. A través de los tópicos, los nodos pueden publicar información o suscribirse para recibir información. Este mecanismo permite el intercambio eficiente de datos. • Mensajes. Los mensajes son las estructuras de datos básicas utilizadas en la comunicación entre nodos. Un mensaje puede contener datos simples, como números enteros, o complejos, como estructuras de datos creadas por el usuario. Los mensajes se envían y reciben de manera asíncrona, facilitando la comunicación continua entre nodos sin bloquear el flujo de ejecución. • Servicios. Los servicios en ROS permiten una comunicación síncrona entre nodos. Un servicio consta de un nodo servidor que ofrece el servicio y uno o más nodos clientes que lo solicitan. La comunicación a través de servicios es útil para operaciones que requieren una respuesta inmediata y confirmación, como consultas o comandos específicos. • Nodo Maestro. El nodo maestro es el núcleo central de ROS. Se encarga de gestionar la comunicación básica entre nodos, asegurando que todos los mensajes y servicios lleguen a sus destinos correctos. Además, el nodo maestro mantiene un registro de todos los nodos, tópicos y servicios activos, gestionando la interacción y la asignación de memoria entre ellos. Su función es crucial para coordinar y supervisar el funcionamiento del sistema distribuido en ROS. 20 Capítulo 3. Metodología y Diseño del Sistema Figura 3.5 Estructura de funcionamiento de ROS [39]. Gazebo es un simulador de dinámica en 3D con la capacidad de simular de forma precisa y eficiente poblaciones de robots en entornos de exterior e interior. De forma parecida a algunos videojuegos, Gazebo proporciona simulaciones de las físicas con mayor grado de fidelidad, una gran cantidad de sensores e interfaces para los usuarios y programas [ 40 ]. Entre los usos típicos de Gazebo se encuentran: prueba de algoritmos para robótica, diseño de robots y validación de funcionamientos en escenarios realistas. Además, cuenta con multitud de modelos de elementos para poder hacer un entorno lo más realista posible. Al igual que en ROS, también puedes usar modelos creados por otros desarrolladores fácilmente. Un autopiloto es un sistema de control automático utilizado en vehículos aéreos no tripulados para gestionar el vuelo sin intervención humana constante. Este sistema incluye una serie de sensores, controladores y software que permiten al UAV mantener una trayectoria predefinida, gestionar la estabilidad del vuelo, y realizar maniobras complejas de manera autónoma [ 41 ]. Los autopilotos modernos pueden ejecutar una variedad de misiones, desde simples vuelos estacionarios hasta rutas de vuelo complejas con múltiples puntos de referencia. ArduPilot es una plataforma de software de código abierto que proporciona capacidades avanzadas de autopiloto para una variedad de vehículos, incluyendo drones (UAVs), rovers, barcos y submarinos. Desarrollado y mantenido por una comunidad activa de desarrolladores, Ardupilot es altamente configurable y puede adaptarse a diferentes tipos de hardware y misiones. Utiliza el protocolo MAVLink para la comunicación entre el controlador de vuelo y las estaciones de control en tierra, permitiendo el envío de comandos y la recepción de datos en tiempo real [42]. ArduPilot incluye características como estabilización de vuelo, navegación autónoma, planificación de misiones, control de velocidad y mucho más. Su versatilidad y robustez lo han convertido en una de las plataformas más populares en el ámbito de los vehículos autónomos y las aplicaciones de 3.2 Componentes software 21 investigación. Además permite que se ejecute como SITL, que se refiere a ‘Software In The Loop’, un entorno de simulación que permite ejecutar ArduPilot en una computadora sin necesidad de hardware físico. Cuando se lanza la simulación se obtiene lo que aparece tanto en la Figura 3.6 como en la 3.7. sim\_vehicle.py -v ArduCopter -f gazebo-iris --console map gazebo --verbose ~/ardupilot/Tools/autotest/gazebo_worlds/iris_ arducopter_runway.world (a) Mapa de Ardupilot con UAV. (b) Lanzamiento de SITL con UAV. Figura 3.6 Lanzamiento de Ardupilot en SITL. (a) Ardupilot en Gazebo. (b) Primera prueba de simulación de Ardupilot. Figura 3.7 Imágenes relacionadas con Ardupilot. Cuando se lanza ArduPilot en modo de simulación (SITL), también se está iniciando la comunicación utilizando el protocolo MAVLink. MAVLink es un protocolo de comunicación ligero diseñado para vehículos aéreos no tripulados que permite la comunicación entre el autopiloto y las estaciones de control en tierra, así como otros dispositivos. MAVROS es un paquete de ROS que actúa como un puente entre ROS y MAVLink, lo cual es imprescindible a la hora de comunicarse con el autopiloto y por lo tanto controlar el UAV por medio 22 Capítulo 3. Metodología y Diseño del Sistema de Simulink a través de ROS. Dado que este Trabajo de Fin de Grado se basa en un proyecto que se implementará físicamente, por motivos de seguridad, se ha decidido incluir una capa de abstracción (UAL). Esta capa garantiza que, en caso de que alguno de los valores que se proporcionan al control por MAVROS deje de recibirse o tenga un valor irreal, no se produzca una caída del UAV durante una prueba de vuelo. Figura 3.8 Arquitectura de comunicaciones. La capa de abstracción utilizada ha sido desarrollada por el grupo de investigación GRVC de la Escuela Técnica Superior de Ingeniería de la Universidad de Sevilla. A continuación, se procederá a explicar en qué consiste [43]. Figura 3.9 Logo de GRVC. UAL es una capa de software que abstrae a los usuarios de UAVs del hardware específico de la plataforma y de las interfaces del piloto automático. El objetivo principal de esta capa de abstracción, denominada Unmanned Aerial Vehicle Abstraction Layer (UAL), es simplificar el desarrollo y las pruebas de algoritmos de nivel superior en robótica aérea al intentar estandarizar y simplificar las interfaces con los UAV. UAL admite la operación con pilotos automáticos y puede trabajar sin problema con plataformas simuladas o reales. UAL se basa en ROS, y sus funcionalidades están distribuidas en varios nodos. Cada nodo tiene una responsabilidad específica, como el control de vuelo, la comunicación con el piloto automático y la gestión de la misión. 3.2 Componentes software 23 Figura 3.10 Arquitectura de UAL [43]. La arquitectura de UAL y sus funcionalidades principales están diseñadas para facilitar la interfaz con diferentes pilotos automáticos. Para cada piloto automático soportado, se crea un backend que maneja sus características específicas. El backend que se ha utilizado en este proyecto ha sido el de MAVROS, para que sea posible la comunicación con ArduPilot. La interfaz de UAL está diseñada para ser intuitiva y fácil de usar. Ofrece comandos estandarizados como: despegue y aterrizaje, control de posición (mover el UAV a una posición específica), control de velocidad (ajustar la velocidad del UAV), obtener el estado del UAV (posición, velocidad, estado de la batería, etc). UAL ofrece una doble interfaz al usuario, ya sea como una clase o como un servidor ROS que responde a solicitudes, en el caso de este proyecto, se han utilizado los servicios de ROS. Figura 3.11 Interfaz de usuario de UAL [43]. Simulink es un entorno de programación visual que funciona sobre el entorno de programación Matlab. Es un entorno de programación de más alto nivel de abstracción que el lenguaje interpretado Matlab (archivos con extensión .m). Simulink genera archivos con extensión .mdl (de "model"). Es desarrollado por MathWorks y se utiliza principalmente en ingeniería y ciencias aplicadas para el diseño, simulación y análisis de sistemas multidisciplinarios. Simulink permite construir modelos mediante diagramas de bloques, donde cada bloque representa una operación matemática, una función o un sistema físico. Esto facilita la visualización y simulación de sistemas complejos antes de implementarlos físicamente. Blender es un software de código abierto y gratuito utilizado principalmente para la creación de gráficos en 3D, animaciones, modelado, simulaciones y renderizado de imágenes. Es desarrollado por la Fundación Blender y es compatible con varias plataformas como Windows, macOS y Linux. 24 Capítulo 3. Metodología y Diseño del Sistema 3.3 Descripción detallada del diseño A continuación, se procederá primero con un análisis general y luego más específico del diseño del sistema. Se comenzará con una visión general de lo que se pretendía diseñar, seguido por un análisis exhaustivo de la arquitectura del sistema. Finalmente, se explicará tanto el control desarrollado como el algoritmo de detección implementado. 3.3.1 Descripción General del Sistema El sistema consta de un vehículo aéreo no tripulado, en concreto el modelo DJI f450 que ha sido modificado para añadir los marcadores ArUcos. Figura 3.12 Modelo del UAV en Gazebo. Los marcadores utilizados han sido obtenidos de la librería oficial de ArUcos. En total, se han empleado ocho marcadores distribuidos como se puede apreciar en la Figura 3.13. Figura 3.13 Disposición de marcadores ArUcos en el UAV. Además, el sistema incluye un vehículo terrestre no tripulado proporcionado por la empresa Robotnik, el cual está equipado con una cámara Intel RealSense D435i orientada en dirección positiva del eje Z, encargada de localizar los marcadores ArUcos. 3.3 Descripción detallada del diseño 25 Figura 3.14 Modelo del UGV en Gazebo. 3.3.2 Arquitectura del sistema Para el diseño de esta simulación se han utilizado varios paquetes de ROS. En primer lugar, se encuentra el paquete que se ha desarrollado para este Trabajo Fin de Grado, llamado ground_- target_follower(ver A.1). Este paquete contiene los principales archivos de lanzamiento, el modelo en formato SDF del UAV, los scripts necesarios para el movimiento del UGV por medio del teclado y una serie de nodos, necesarios para la comunicación, cuyo desarrollo se detallarán en el próximo capítulo. En segundo lugar, se hace uso del paquete ual_backend_mavros, que se trata de una versión de UAL modificada por CATEC. Este paquete se utiliza para iniciar la capa de abstracción que se conecta con MAVROS mediante el archivo de lanzamiento server.launch. En tercer lugar, se utiliza el paquete rs_ar_detector(ver A.2), también desarrollado por CATEC, el cual se subscribe a los topics correspondientes de la cámara, procesa la imagen recibida, detecta los marcadores ArUcos y publica su posición en un topic. En cuarto lugar, se encuentra el paquete suministra_pose_estimation, también desarrollado por CATEC, el cual se subscribe al topic donde se ha publicado la posición de los ArUcos, calcula las coordenadas del centro y las publica en un topic(ver A.3). En quinto lugar se hace uso del paquete realsense_gazebo_plugin el cual permite simular cámaras Intel RealSense dentro de Gazebo, permitiendo capturar datos de imágenes RGB, profundidad y de infrarrojos como lo haría una cámara física en el mundo real. Se integra como un plugin en Gazebo, lo que significa que puede ser añadido y configurado dentro del modelo del UGV en Gazebo. Para usar este paquete basta con ejecutar los siguiente comandos: cd ~/catkin_ws_ardupilot/src git clone https://github.com/pal-robotics/realsense_gazebo_plugin.git Finalmente, dado que el proyecto SUMINISTRA es en colaboración con otras empresas y que el UGV lo proporcionaba la empresa Robotnik, todo lo referente al modelo del UGV se ha obtenido de su repositorio público. De este repositorio se han empleado los siguientes paquetes: summit_xl_- gazebo,summit_xl_description yrobotnik_sensors. Del primero de ellos se utiliza únicamente el archivo de lanzamiento, el cual lanza el modelo, cuya descripción se encuentra en el segundo paquete mencionado. Y el tercero de ellos es necesario para incluir el modelo de la cámara en el UGV. Una vez se ha aclarado cuales son los paquetes necesarios, se procede a explicar la arquitectura del sistema. Para un mejor entendimiento de la arquitectura se realizarán las explicaciones con el apoyo del esquema de la Figura 3.15. Esta arquitectura comienza con el archivo que lanza el mundo vacío 32 Capítulo 4. Desarrollo Figura 4.2 Modelo UAV con placas en Blender. Una vez se incluyeron las placas, se procedió a añadir los ArUcos en forma de material. Para ello se hizo uso de las opciones que aparecen en la parte inferior derecha de la interfaz. Se seleccionó el modelo de una de las placas y se añadió un material. Para configurar dicho material se seleccionó el punto en color amarillo de Base Color, del que se obtendrán las posibilidades que aparecen en la Figura 4.3. Figura 4.3 Pestaña donde añadir material a un modelo en Blender. 4.1 Modelo UAV modificado 33 Dado que lo que se pretende añadir es una imagen, se eligió la opción Image Texture. Figura 4.4 Elección de Image Texture. Aparecerá una ventana que permite navegar a través de nuestros archivos en busca de la imagen del marcador ArUco correspondiente. Figura 4.5 Explorador de archivos de Blender. Repitiendo dicho procedimiento para cada uno de los modelos de las placas y asignandole a cada una de ellas el ArUco correspondiente como material, se obtendrá el resultado que se aprecia en la Figura 4.6. Es muy importante colocar los ArUcos con la orientación adecuada ya que tienen asignados a ellos un sistema de ejes el cual será crucial a la hora de transformar las posiciones dadas respecto al marco de referencia de la cámara al marco de referencia del mundo. 34 Capítulo 4. Desarrollo Figura 4.6 Resultado final de UAV con ArUcos. 4.2 Modelo UGV modificado A pesar de que el modelo del UGV se obtiene directamente del repositorio de Robotnik es necesario realizar una serie de modificaciones para que se cumplan los objetivos propuestos en este trabajo. En primer lugar, se procede a cambiar la ubicación de la cámara, la cual se encuentra inicialmente en la parte frontal del UGV. Dado que su ubicación final ha de ser la parte de arriba del UGV, apuntando en la dirección positiva del eje Z, se modificarán las coordenadas de la cámara para que este en la posición (0,0,0.4) respecto al sistema de referencia del UGV y se rotará respecto a su eje Y-90ºpara alcanzar la orientación deseada. Además de la ubicación de la cámara es imprescindible ajustar su resolución para el correcto funcionamiento del algoritmo de detección. Por lo que se modificará en el paquete de robotnik_- sensors, en el archivo intel_d435.urdf.xacro el valor de la resolución a 1280x720. 4.3 Paquete ground_target_follower En esta sección se explicará todo lo referente al desarrollo del paquete ground_target_follower. 4.3.1 gazebo_sim.launch Se comenzará explicando el archivo de lanzamiento de la simulación que incluye la mayoría de los elementos necesarios para su funcionamiento, pero no todos. Este archivo se encarga en primer lugar de lanzar otros dos archivos de lanzamiento, gazebo_- word.launch ygazebo_uav.launch. Además de lanzar el modelo del UGV y de los nodos necesarios para comunicarles las referencias tanto al control del UAV como al propio UGV. Y por último incluye un nodo necesario para poder manejar de forma más cómoda los valores de yaw. gazebo_world.launch En este archivo, en primer lugar se definen una serie de argumentos que serán utilizados por el archivo de lanzamiento que se define a continuación. 4.3 Paquete ground_target_follower 35 El archivo de lanzamiento mencionado anteriormente se llama empty_world.launch y se encuentra en el paquete de gazebo_ros. Al incluir este archivo es necesario pasarle los valores de los argumentos definidos previamente. Y es el encargado de definir y configurar la simulación en Gazebo. Dado que uno de los argumentos que se le pasa es una ruta al modelo del mundo, incluido en la carpeta worlds, que se lanzará, se ha añadido en él el modelo del UAV. Como se puede observar en el repositorio, para la definición del modelo del UAV en Gazebo, ha sido necesario la utilización de dos carpetas: f450_with_ardupilot yf450_with_standoffs. A continuación se procederá a su análisis. A la hora de desarollar el modelo del UGV en Gazebo, en primer lugar se creó la carpeta f450_- with_standoffs, en la que se encuentra un archivo llamado model.sdf en el que se define el modelo del UGV. Para ello, se crearon 5 links, uno para la base (base_link) y otros cuatro para los rotores (rotor_0,rotor_1,rotor_2 yrotor_3). Estos últimos cuatro enlaces se unen, cada uno de ellos de manera individual, al base_link por medio de joints de tipo revolute, los cuales permiten el movimiento rotacional alrededor de un eje específico. Por otro lado, se creó la carpeta f450_with_ardupilot, esencial para configurar y simular el comportamiento dinámico y el control del UAV en Gazebo. En ella se encuentra el archivo model.sdf, donde se incluye el modelo f450_with_standoffs, mencionado anteriormente, y se definen varios plugins,LiftDragPlugin yArduPilotPlugin. Los plugins LiftDragPlugin se utilizan para modelar y calcular las fuerzas de sustentación (lift) y resistencia (drag) de los rotores del UAV, mientras que ArduPilotPlugin permite la integración con un controlador de vuelo simulado que emula el software ArduPilot. Este plugin configura los parámetros necesarios para controlar la velocidad de cada uno de los cuatro rotores (rotor_0 arotor_3) en el dron simulado. gazebo_uav.launch Este archivo de lanzamiento tiene dos tareas. La primera de ellas es definir los argumentos necesarios para la configuración de MAVROS y la segunda lanzar el autopiloto haciendo uso del archivo run_- ardupilot.py que se encuentra en la carpeta de scripts. Lanza modelo del UGV Para incluir el modelo del UGV a la simulación basta con incluir el archivo de lanzamiento del UGV del paquete summit_xl_gazebo.launch a nuestro archivo de lanzamiento principal gazebo_- sim.launch. Nodo reference_node_UAV Este nodo ha sido creado para leer los valores de posición del UGV del topic /gazebo/model_states y publicarlos de un topic que crea el propio nodo, llamado /referencia_UAV. Este topic juega un papel fundamental en el control desarrollado en Simulink ya que se trata de la referencia del control. Nodo reference_node_UGV Este nodo es el encargado de tomar los valores que el archivo key_detection.py, cuyo funcionamiento se explicará a continuación, publica en el topic /teleop_position y mandarlos al UGV, publicando dichos valores en el topic /gazebo/set_model_state. 36 Capítulo 4. Desarrollo yaw_node Este nodo se encarga de subscribirse al topic /ual/pose, extraer el ángulo de yaw, y publicar este ángulo modificado en otro topic llamado /yaw_actual. La modificación que se hace al valor yaw es para convertirla de cuaternión a ángulos de Euler (roll,pitch,yaw) usando tf2::Matrix3x3. 4.3.2 bt_UAV_node.cpp En este archivo se desarrolla el código del Behavior Tree. Para su mejor explicación se adjunta a continuación en la Figura 4.7 un esquema de su funcionamiento. Su objetivo principal es controlar el UAV en diferentes estados y situaciones específicas, asegurando que el UAV mantenga en todo momento su funcionamiento correcto. Figura 4.7 Esquema BehaviorTree. Dado que los nodos están dentro de una estructura Sequence, el Behavior Tree comenzará a ejecutar de izquierda a derecha, siempre y cuando los nodos devuelvan un SUCCESS. Si no es así, la ejecución del árbol se detendrá en ese nodo y en la siguiente iteración comenzará a ejecutarse de nuevo por completo. A continuación, se procede a describir los nodos del Behavior Tree. • IsFlyingAuto.Este nodo verifica si el UAV está en modo "Flying Auto". Se suscribe al topic /ual/state para obtener el estado del UAV y verifica si el estado es igual a 4 (representando "Flying Auto"). Si es así, devuelve SUCCESS; de lo contrario, devuelve FAILURE. • Ready.Este nodo espera a que se active el control para el seguimiento del UGV. Utiliza un servicio ROS, /EnableControlService, para recibir una solicitud que activa el control. Una vez activado, publica un mensaje en el topic /EnableControlResult y devuelve SUCCESS. • ControlActivated.Este nodo verifica continuamente si se estan reciviendo valores en el topic /catec_ekf/pose, es decir, si la posición del UAV dada por los ArUcos no se ha perdido. Si la posición está disponible, imprime un mensaje de confirmación (true) en el topic /IsRefOK y devuelve FAILURE. Si no hay recepción de posición, devuelve SUCCESS, para pasar al nodo de perdida de referencia. • RefLost.Este nodo indica que los valores de posición del UAV recibidos de los ArUcos han dejado de recibirse. 4.3.3 key_detection.py Este código en Python implementa un nodo de ROS llamado keyboard_teleop que permite controlar la posición del UGV a través del teclado. A continuación, se explicará su funcionamiento. 4.4 Paquete rs_aruco_detector 37 El nodo espera constantemente la entrada del teclado y actualiza las coordenadas self.x yself.y según las teclas presionadas (w, s, d, a). A medida que se actualizan las coordenadas, se publica un mensaje en el topic /teleop_position a una frecuencia de 100 Hz, indicando la posición a la que debe moverse el UGV de acuerdo a la tecla presionada. 4.3.4 tf.launch este archivo de lanzamiento configura una transformación estática entre el marco de referencia de la cámara y el marco de referencia del mundo, definiendo un nodo de transformación estática utilizando tf2_ros. 4.3.5 camera_world_tf.cpp Este archivo implementa un nodo de ROS llamado TransformPoseNode que se encarga de transformar y publicar mensajes de poses desde marco de referencia local de la cámara (/catec_ekf/pose) al marco de referencia global denominado world. El nodo utiliza las bibliotecas tf2_ros para manejar las transformaciones y message_filters para asegurar que los mensajes de poses se procesen de manera sincronizada con las transformaciones disponibles. 4.4 Paquete rs_aruco_detector El paquete rs_aruco_detector es una implementación en ROS que utiliza OpenCV para detectar marcadores ArUco en imágenes capturadas por una cámara RealSense. A continuación, se detalla el flujo de procesamiento desde la recepción de la imagen hasta la detección y localización de los marcadores ArUco. El flujo del proceso es el siguiente: • Recepción de la Imagen. La imagen es recibida a través del topic /camera/color/image_raw al que se subscribe el nodo rs_aruco_detector_node. • Conversión de la Imagen. La imagen recibida es convertida desde el formato sensor_- msgs/Image a una matriz de OpenCV utilizando CvBridge. Este paso convierte la imagen a un formato adecuado para su procesamiento con OpenCV. • Detección de los Marcadores ArUco. La función principal que realiza la detección de los marcadores ArUco es detect_markers. En ella se utilizan funciones de OpenCV específicas para este propósito. • Conversión a Escala de Grises. La imagen en color es convertida a escala de grises. Esto es necesario porque la detección de los marcadores ArUco se realiza más eficazmente en imágenes en escala de grises. •Detección de los Marcadores. La función cv2.aruco.detectMarkers se utiliza para detectar los marcadores en la imagen en escala de grises. Esta función devuelve las esquinas (corners) de los marcadores detectados y sus identificadores (IDs). • Dibujo y Publicación de los Resultados. Si se detectan marcadores, se dibujan sus bordes en la imagen original y se publican los resultados en el topic que posteriormente se usará para comunicarse con el paquete sumministra_pose_estimation. • Publicación de la Posición de los Marcadores. La función publish_detections se encarga de publicar la información de las detecciones, incluyendo las esquinas y los identificadores de los marcadores, en un tópico. 38 Capítulo 4. Desarrollo 4.5 Paquete sumministra_pose_estimation El objetivo principal de suministra_pose_estimation es permitir que el UAV determine su posición en el entorno utilizando datos provenientes de múltiples marcadores ArUco. Esto se logra mediante la implementación de un filtro de Kalman Extendido (EKF), que es capaz de integrar información de la cámara para estimar con precisión la posición y orientación del dron. El paquete incorpora un EKF configurado mediante archivos yaml que define la dinámica del sistema y las covarianzas de los estados y observaciones. Esta implementación permite la fusión de datos de marcadores de Aruco para calcular una posición estimada que integra la información de forma coherente y precisa. El paquete se suscribe a múltiples tópicos de datos provenientes de los marcadores de Aruco. Esto asegura que el sistema pueda capturar continuamente las actualizaciones de posición y orientación necesarias para la estimación correcta. suministra_pose_estimation gestiona las transformaciones de referencia entre diferentes marcos de coordenadas (como el marco de la cámara, el marco de odometría y el marco global). Además, publica activamente la posición estimada del UAV en un tópico definido (/catec_ekf/pose), permitiendo que otros componentes del sistema puedan utilizar esta información para control. El paquete incorpora rutinas para calcular y manejar las covarianzas de las observaciones de los marcadores de Aruco. Esto asegura que las mediciones sean evaluadas correctamente y que las estimaciones del EKF sean robustas frente a datos inconsistentes o ruidosos. 5 Simulaciones, Resultados y Discusión E n este capítulo se expondrá de manera detallada los resultados finales que se han obtenido. Para ello se presentarán una serie de gráficas obtenidas realizando una simulación cuyas características se detallarán más adelante. Cada resultado será acompañado de su correspondiente análisis. 5.1 Simulación La simulación que se ha llevado a cabo para la obtención de los resultados que se adjuntan en la próxima sección, se lanza ejecutandose los siguientes comandos: Terminal 1: roslaunch ground_target_follower gazebo_sim.launch Terminal 2: roslaunch ual_backend_mavros server.launch Terminal 3: rosrun ground_target_follower bt_UAV_node Terminal 4: roslaunch suministra_pose_estimation pose_estimator.launch Terminal 5: roslaunch ground_target_follower tf.launch Terminal 6: rosrun ground_target_follower camera_world_tf Una vez se han ejecutado estos comandos, se espera a recibir, en el terminal donde se ha ejecutado el Behavior Tree, el estado LANDED_ARMED para proceder con dos acciones. La primera de estas acciones es presionar el botón run del modelo de Simulink, y la segunda es ejecutar el siguiente comando para proceder al despegue del UAV a una altura de 2 metros haciendo uso de los servicios que proporciona la capa de abstracción. 39 40 Capítulo 5. Simulaciones, Resultados y Discusión Terminal 7: rosservice call /ual/take_off "height: 2.0 blocking: false" Posteriormente, y una vez aparezca en el terminal del Behavior Tree el mensaje ’Ready to be controlled’, se ejecuta el siguiente comando, que permite activar el control. Terminal 7: rosservice call /EnableControlService "value: true" Cuando el UAV esté posicionado en la vertical del UGV, se podrá iniciar el movimiento del UGV utilizando el teclado, ejecutando el siguiente comando. Terminal 8: rosrun ground_target_follower key_detection.py A continuación, se procederá a describir los pasos seguidos en la simulación que se ha empleado para la obtención de los datos que se presentan en la sección de resultados de este capítulo. Es importante destacar que los datos son recogidos mediante Simulink, por lo que, al comenzar la recepción de datos, es decir, tras la activación del control, el UAV se encuentra despegado en la posición (0,0,2). Figura 5.1 Estado inicial de la simulación cuando se comienzan a recibir datos. Tras la activación del control, el UAV inicia su movimiento en dirección al UGV, el cual parte de la posición inicial (3,0,0), hasta situarse a una altura de 1.8 metros sobre el UGV. Figura 5.2 Poceso de búsqueda del UGV 1. 5.1 Simulación 41 (a) Proceso de búsqueda del UGV 2. (b) Proceso de búsqueda del UGV 3. Figura 5.3 Procesos de búsqueda de UGV 2 y UGV 3. Figura 5.4 Poceso de búsqueda del UGV 4. Cuando el UAV entra en el campo de visión de la cámara y se empiezan a recibir los valores de posición a través del algoritmo de detección de ArUcos, el control comienza a tomar los valores de posición del UAV utilizando los ArUcos. Figura 5.5 Visión de la cámara cuando el UAV está sobre el UGV. A continuación, utilizando el nodo creado para mover el UGV mediante las teclas, se procede a desplazar el UGV para verificar que es seguido correctamente por el UAV. 48 Capítulo 5. Simulaciones, Resultados y Discusión Software de simulación Entre las plataformas de simulación más reconocidas y utilizadas en el ámbito de la robótica, se destacan Gazebo y V-REP [45]. Algunas de las ventajas de Gazebo son su estrecha vinculación con ROS, lo que permite un control más abstracto de los robots en la simulación y facilita la transición al control del robot real gracias a paquetes como gazebo_ros_pkg yros_control. Por otro lado, el motor dinámico y de renderizado de Gazebo ofrece resultados superiores en simulación, consumiendo menos recursos de la CPU comparado con otras plataformas. Y por último, Gazebo trabaja de manera nativa con estos formatos de descripción de robots, ampliamente utilizados, permitiendo la fácil integración y modificación de modelos de robots. V-REP, también conocido como CoppeliaSim, es otra popular plataforma de simulación robótica que se caracteriza por su facilidad de uso y su amplia gama de funcionalidades. Presenta las siguientes ventajas. Utiliza scripts basados en Lua, que permiten desarrollar fácilmente códigos para el control de robots con más de 500 funciones distintas. Ofrece un control absoluto de todos los elementos en la escena de simulación, facilitando la modificación y control de robots y otros objetos. V-REP proporciona una interfaz de usuario más amigable y sencilla, ideal para usuarios novatos. Funciona como una plataforma de simulación independiente, no requiriendo otros programas para su operación. A pesar de las ventajas menioncada, V-REP presenta importantes desventajas. Aunque existen plugins que permiten la interacción con ROS, la integración no es tan desarrollada como en Gazebo, lo que puede ser una desventaja para proyectos que dependen fuertemente de ROS [ 46 ]. Además, V-REP tiende a consumir más recursos de la CPU que Gazebo, lo que puede ser un inconveniente en simulaciones más complejas. Dado que la herramienta de ROS es fundamental en este proyecto, se opto por hacer uso de Gazebo. Algoritmo de detección de ArUcos Se han desarrollado sistemas UAV-UGV que se coordinan entre sí por medio de un único marcador ArUco [47]. Opción por la que no se optó debido a varias razones que se explican a continuación. Utilizar múltiples marcadores ArUco para detectar la posición de un UAV mediante cámaras y OpenCV ofrece varias ventajas significativas sobre el uso de un único marcador centralizado. Este enfoque multicámara proporciona una mayor precisión y robustez en la estimación de la posición y orientación del UAV, lo cual es crucial en aplicaciones que requieren seguimiento preciso y continuo en entornos dinámicos y variables como el aire libre. Al emplear varios marcadores distribuidos estratégicamente en el UAV, se puede mejorar la precisión del sistema de seguimiento en comparación con un solo marcador central. Los marcadores ArUco son fácilmente detectables y identificables por las cámaras debido a sus patrones únicos y codificados, lo que permite una estimación más precisa de la posición relativa del UAV respecto al UGV o a cualquier otro objeto de referencia. El uso de múltiples marcadores permite calcular con mayor exactitud la posición y orientación del UAV mediante técnicas como la triangulación visual. Esto minimiza los errores que podrían surgir debido a la perspectiva, la distancia o la iluminación variable, problemas que podrían afectar más gravemente a un solo marcador. 5.3 Discusión 49 En entornos exteriores, donde las condiciones de luz y el clima pueden cambiar rápidamente, los sistemas basados en múltiples marcadores ArUco son más robustos. La presencia de varios marcadores asegura que siempre haya suficientes puntos de referencia visibles para mantener un seguimiento continuo y preciso, incluso en situaciones donde uno o más marcadores pueden estar temporalmente oscurecidos o bloqueados. La distribución de marcadores en diferentes partes del UAV también contribuye a una mayor estabilidad del sistema de seguimiento. Esto se traduce en una reducción de las fluctuaciones y errores de posición. 5.3.2 Posibles mejoras y optimizaciones A pesar de que se hayan alcanzado los objetivos planteados al inicio del proyecto, siempre existe la posibilidad de mejorar y optimizar el sistema. La primera forma de optimización que se plantea es desarrollar un control más robusto tanto para situaciones en las que la referencia cambia continuamente en pequeños incrementos o decrementos, como para cambios bruscos de referencia. El control desarrollado ha demostrado una buena respuesta ante grandes cambios de referencia; sin embargo, en situaciones donde los cambios son pequeños y continuos, el dron únicamente mantiene la verticalidad con el UGV si este se mueve a velocidades muy bajas debido a que el control no es excesivamente rápido. Por lo tanto, una primera optimización sería el desarrollo de un control más rápido para cambios pequeños de referencia sin que oscile demasiado cuando los cambios son grandes. Otra optimización sería permitir el despegue del UAV desde la superficie del UGV, evitando así la necesidad de un despegue manual y la activación manual del control. Para implementar esta mejora, sería suficiente añadir una plataforma en la parte superior del UGV donde el UAV se posicionaría antes del despegue. De este modo, el UAV ya estaría dentro del campo de visión de la cámara al despegar, facilitando la detección de ArUcos y preparando al sistema para su operación. 6 Conclusiones y Desarrollos futuros E n este capítulo se presentan las conclusiones derivadas del proyecto, así como las posibles líneas de desarrollo futuro. 6.1 Conclusión El desarrollo de la simulación en Gazebo de un sistema de control de seguimiento UAV-UGV utilizando un controlador PID y marcadores ArUco ha demostrado ser una solución efectiva para la navegación autónoma y la cooperación entre vehículos no tripulados. Los experimentos realizados muestran que el uso de marcadores ArUco permite una detección precisa y confiable de la posición relativa del UAV respecto al UGV, facilitando el control y seguimiento del movimiento. El controlador PID implementado ha mostrado un rendimiento satisfactorio en términos de estabilidad y precisión, ajustándose adecuadamente a las dinámicas del sistema. Los resultados indican que este enfoque es viable para aplicaciones en las que se requiere un alto grado de coordinación y precisión, como en misiones de búsqueda y rescate, exploración autónoma y tareas de inspección. A pesar de los resultados positivos, se han identificado ciertas limitaciones, como la dependencia de las condiciones ambientales para la detección de los marcadores y la necesidad de ajustar finamente los parámetros del controlador PID para diferentes escenarios operativos. Estas limitaciones abren oportunidades para mejoras y optimizaciones futuras. 6.2 Desarollos futuros El desarrollo futuro de este proyecto se centrará en realizar una simulación completa y exhaustiva del sistema UAV-UGV, abordando las siguientes áreas clave: • Simulación del Entorno Completo. Realizar una simulación detallada del entorno de almacén que incluya estanterías, pasillos y ubicaciones específicas para los paquetes. El dron deberá estar equipado con una cámara que permita la detección de paquetes mediante tecnologías de visión artificial, como la lectura de códigos de barras y etiquetas RFID, y el mapeo del almacén. • Integración de Sensores Adicionales. Incorporar sensores adicionales, como LIDAR y cámaras estéreo, para complementar la información obtenida de los marcadores ArUco, mejorando la percepción del entorno y la capacidad de navegación autónoma. 51 52 Capítulo 6. Conclusiones y Desarrollos futuros • Validación Experimental. Realizar pruebas experimentales en entornos reales para validar los resultados obtenidos en la simulación y ajustar los modelos y algoritmos según sea necesario. Apéndice A Códigos A.1 ground_target_follower Código A.1 gazebo_sim.launch. <launch> <!-- WORLD AND SIMULATION PARAMETERS --> <arg name="world_name" default="empty"/> <arg name="world_path" default="$(find ground_target_follower)/ worlds"/> <arg name="paused" default="false"/> <arg name="use_sim_time" default="true"/> <arg name="gui" default="true"/> <arg name="recording" default="false"/> <arg name="debug" default="false"/> <!-- SITL PARAMETERS--> <arg name="robot_model" default="gazebo-iris"/> <arg name="param_file" default="$(find ground_target_follower)/ config/apm/copter.parm"/> <arg name="mavproxy" default="true"/> <arg name="map" default="true"/> <arg name="console" default="true"/> <!-- MAVROS PARAMETERS--> <arg name="fcu_url" default="udp://:14550@"/> <!-- /dev/ ttyACM0:921600 || udp://localhost:14551@14555 --> <arg name="gcs_url" default="" /> <arg name="tgt_system" default="1" /> <arg name="tgt_component" default="1" /> <arg name="log_output" default="screen" /> <arg name="fcu_protocol" default="v2.0" /> <arg name="respawn_mavros" default="false" /> 53 54 Capítulo A. Códigos <arg name="pluginlists_yaml" default="$(find ground_target_follower) /config/mavros/apm_pluginlists.yaml" /> <arg name="config_yaml" default="$(find ground_target_follower)/ config/mavros/apm_config.yaml" /> <!-- UAL PARAMETERS--> <arg name="ual_server" default="false"/> <!--=================================================================--> <!-- Launch Gazebo with the selected world--> <include file="$(find ground_target_follower)/launch/gazebo_world. launch"> <arg name="world_name" value="$(arg world_name)"/> <arg name="world_path" value="$(arg world_path)"/> <arg name="paused" value="$(arg paused)"/> <arg name="use_sim_time" value="$(arg use_sim_time)"/> <arg name="gui" value="$(arg gui)"/> <arg name="recording" value="$(arg recording)"/> <arg name="debug" value="$(arg debug)"/> </include> <!-- Launch the UAV --> <include file="$(find ground_target_follower)/launch/gazebo_uav. launch"> <arg name="robot_model" value="$(arg robot_model)"/> <arg name="param_file" value="$(arg param_file)"/> <arg name="mavproxy" value="$(arg mavproxy)"/> <arg name="map" value="$(arg map)"/> <arg name="console" value="$(arg console)"/> </include> <!--=================================================================--> <!-- NODOS --> <!-- Launch the UGV with RealSense D435i --> <include file="$(find summit_xl_gazebo)/launch/summit_xl_one_robot. launch"> </include> <include file="$(find rs_aruco_detector)/launch/ suministra_rs_ar_detector_node.launch"> </include> <!-- Lanzar el nodo de referencia del UGV --> A.1 ground_target_follower 55 <node name="reference_node_UGV" pkg="ground_target_follower" type=" referencia_UGV_teclado" output="screen"/> <!-- Lanzar el nodo de referencia del UAV--> <node name="reference_node_UAV" pkg="ground_target_follower" type=" referencia_UAV" output="screen"/> <!-- Lanzar el nodo de yaw--> <node name="yaw_node" pkg="ground_target_follower" type="yaw" output ="screen"/> <!-- Lanzar nodo de referencia VICON --> <node name="reference_node_VICON" pkg="ground_target_follower" type ="referencia_VICON" output="screen"/> </launch> Código A.2 gazebo_world.launch. <launch> <arg name="world_name" default="empty"/> <arg name="world_path" default="$(find ground_target_follower)/ worlds"/> <arg name="paused" default="false"/> <arg name="use_sim_time" default="true"/> <arg name="gui" default="true"/> <arg name="recording" default="false"/> <arg name="debug" default="false"/> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(arg world_path)/$(arg world_name).world"/> <arg name="paused" value="$(arg paused)"/> <arg name="use_sim_time" value="$(arg use_sim_time)"/> <arg name="gui" value="$(arg gui)"/> <arg name="recording" value="$(arg recording)"/> <arg name="debug" value="$(arg debug)"/> </include> </launch> Código A.3 gazebo_uav.launch. <launch> <!-- SITL ardupilot--> <arg name="robot_model" default="quad-jm"/> 56 Capítulo A. Códigos <arg name="param_file" default="$(find ground_target_follower)/ config/apm/copter.parm"/> <arg name="mavproxy" default="True"/> <arg name="map" default="True"/> <arg name="console" default="True"/> <arg name="ual_server" default="false"/> <!-- MAVROS --> <arg name="fcu_url" default="udp://localhost:14551@14555"/> <!-- /dev/ttyACM0:921600 || udp://localhost:14551@14555 --> <arg name="gcs_url" default="" /> <arg name="tgt_system" default="1" /> <arg name="tgt_component" default="1" /> <arg name="log_output" default="screen" /> <arg name="fcu_protocol" default="v2.0" /> <arg name="respawn_mavros" default="false" /> <arg name="pluginlists_yaml" default="$(find ground_target_follower) /config/mavros/apm_pluginlists.yaml" /> <arg name="config_yaml" default="$(find ground_target_follower)/ config/mavros/apm_config.yaml" /> <!--==============================================================--> <!-- Launch autopilot--> <node name="sitl_ardupilot" pkg="ground_target_follower" type=" run_ardupilot.py" args="-model $(arg robot_model) -param_file $(arg param_file) -mavproxy $(arg mavproxy) -map $(arg map) -console $(arg console)" /> </launch> Código A.4 model.sdf de f450_with_standoffs. <?xml version=’1.0’?> <sdf version=’1.5’> <model name=’f450’> <pose>0 0 0.3 0 0 0</pose> <link name=’base_link’> <pose>0 0 0 0 0 0</pose> <velocity_decay> A.1 ground_target_follower 57 <linear>0.0</linear> <angular>0.0</angular> </velocity_decay> <inertial> <pose>0 0 0 0 0 0</pose> <mass>2</mass> <inertia> <ixx>0.008</ixx> <ixy>0</ixy> <ixz>0</ixz> <iyy>0.015</iyy> <iyz>0</iyz> <izz>0.017</izz> </inertia> </inertial> <collision name=’base_link_collision’> <pose>0 0 0 0 0 0</pose> <geometry> <box> <size>0.47 0.47 0.10</size> </box> </geometry> <surface> <contact> <ode> <max_vel>100.0</max_vel> <min_depth>0.001</min_depth> </ode> </contact> <friction> <ode> <mu>100000.0</mu> <mu2>100000.0</mu2> </ode> </friction> </surface> </collision> <visual name=’base_link_visual’> <geometry> <mesh> <scale>0.001 0.001 0.001</scale> <uri>model://f450_with_standoffs/meshes/ suministra_dron_final_todos_arucos_PRUEBA.dae</uri> </mesh> </geometry> </visual> </link> <link name=’f450/ground_truth/odometry_sensorgt_link’> <pose>0 0 0 0 0 0</pose> 64 Capítulo A. Códigos <ixx>9.75e-06</ixx> <ixy>0</ixy> <ixz>0</ixz> <iyy>0.000166704</iyy> <iyz>0</iyz> <izz>0.000167604</izz> </inertia> </inertial> <collision name=’rotor_3_collision’> <pose>0 0 0 0 0 0</pose> <geometry> <cylinder> <length>0.005</length> <radius>0.12</radius> </cylinder> </geometry> <surface> <contact> <ode/> </contact> <friction> <ode/> </friction> </surface> </collision> <visual name=’rotor_3_visual’> <pose>0 0 0 0 0 0</pose> <geometry> <mesh> <scale>1 1 1</scale> <uri>model://f450_with_standoffs/meshes/prop_2.stl</uri> </mesh> </geometry> <material> <script> <name>Gazebo/White</name> <uri>file://media/materials/scripts/gazebo.material</uri> </script> </material> </visual> <gravity>1</gravity> <velocity_decay/> <self_collide>0</self_collide> </link> <joint name=’rotor_3_joint’ type=’revolute’> <child>rotor_3</child> <parent>base_link</parent> <axis> <xyz>0 0 1</xyz> <limit> A.1 ground_target_follower 65 <lower>-1e+16</lower> <upper>1e+16</upper> </limit> <dynamics> <damping>0.004</damping> </dynamics> <use_parent_model_frame>1</use_parent_model_frame> </axis> <physics> <ode> <implicit_spring_damper>1</implicit_spring_damper> </ode> </physics> </joint> <static>0</static> </model> </sdf> Código A.5 model.sdf de f450_with_ardupilot. <?xml version=’1.0’?> <sdf version="1.6" xmlns:xacro=’http://ros.org/wiki/xacro’> <model name="f450_demo"> <include> <uri>model://f450_with_standoffs</uri> </include> <!-- Plugins --> <plugin name="rotor_0_blade_1" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>0.084 0 0</cp> <forward>0 1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_0</link_name> </plugin> 66 Capítulo A. Códigos <plugin name="rotor_0_blade_2" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>-0.084 0 0</cp> <forward>0 -1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_0</link_name> </plugin> <plugin name="rotor_1_blade_1" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>0.084 0 0</cp> <forward>0 1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_1</link_name> </plugin> <plugin name="rotor_1_blade_2" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>-0.084 0 0</cp> <forward>0 -1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_1</link_name> </plugin> A.1 ground_target_follower 67 <plugin name="rotor_2_blade_1" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>0.084 0 0</cp> <forward>0 -1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_2</link_name> </plugin> <plugin name="rotor_2_blade_2" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>-0.084 0 0</cp> <forward>0 1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_2</link_name> </plugin> <plugin name="rotor_3_blade_1" filename="libLiftDragPlugin.so"> <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>0.084 0 0</cp> <forward>0 -1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_3</link_name> </plugin> <plugin name="rotor_3_blade_2" filename="libLiftDragPlugin.so"> 68 Capítulo A. Códigos <a0>0.3</a0> <alpha_stall>1.4</alpha_stall> <cla>4.2500</cla> <cda>0.10</cda> <cma>0.00</cma> <cla_stall>-0.025</cla_stall> <cda_stall>0.0</cda_stall> <cma_stall>0.0</cma_stall> <area>0.002</area> <air_density>1.2041</air_density> <cp>-0.084 0 0</cp> <forward>0 1 0</forward> <upward>0 0 1</upward> <link_name>f450::rotor_3</link_name> </plugin> <plugin name="arducopter_plugin" filename="libArduPilotPlugin.so"> <fdm_addr>127.0.0.1</fdm_addr> <fdm_port_in>9002</fdm_port_in> <fdm_port_out>9003</fdm_port_out> <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</ modelXYZToAirplaneXForwardZDown> <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED> <imuName>f450_demo::f450::f450/imu_link::imu_sensor</imuName> <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount> <control channel="0"> <type>VELOCITY</type> <offset>0</offset> <p_gain>0.20</p_gain> <i_gain>0</i_gain> <d_gain>0</d_gain> <i_max>0</i_max> <i_min>0</i_min> <cmd_max>2.5</cmd_max> <cmd_min>-2.5</cmd_min> <jointName>f450::rotor_0_joint</jointName> <multiplier>838</multiplier> <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim> </control> <control channel="1"> <type>VELOCITY</type> <offset>0</offset> <p_gain>0.20</p_gain> <i_gain>0</i_gain> <d_gain>0</d_gain> <i_max>0</i_max> <i_min>0</i_min> <cmd_max>2.5</cmd_max> <cmd_min>-2.5</cmd_min> <jointName>f450::rotor_1_joint</jointName> <multiplier>838</multiplier> A.1 ground_target_follower 69 <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim> </control> <control channel="2"> <type>VELOCITY</type> <offset>0</offset> <p_gain>0.20</p_gain> <i_gain>0</i_gain> <d_gain>0</d_gain> <i_max>0</i_max> <i_min>0</i_min> <cmd_max>2.5</cmd_max> <cmd_min>-2.5</cmd_min> <jointName>f450::rotor_2_joint</jointName> <multiplier>-838</multiplier> <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim> </control> <control channel="3"> <type>VELOCITY</type> <offset>0</offset> <p_gain>0.20</p_gain> <i_gain>0</i_gain> <d_gain>0</d_gain> <i_max>0</i_max> <i_min>0</i_min> <cmd_max>2.5</cmd_max> <cmd_min>-2.5</cmd_min> <jointName>f450::rotor_3_joint</jointName> <multiplier>-838</multiplier> <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim> </control> </plugin> </model> </sdf> Código A.6 referencia_UAV.cpp. /* Nodo que lee los valores de posición X e Y del modelo del UGV del topic /gazebo/model_states y va publicando dichos valores en un nuevo topic llamado /referncia_UAV (referencia que le entra al control) */ #include "ros/ros.h" #include "gazebo_msgs/ModelStates.h" #include "geometry_msgs/Pose.h" 70 Capítulo A. Códigos class ReferenciaUAV { public: ReferenciaUAV() { ros::NodeHandle nh; model_states_sub = nh.subscribe("/gazebo/model_states", 1000, & ReferenciaUAV::modelStatesCallback, this); referencia_uav_pub = nh.advertise<geometry_msgs::Pose>("/ referencia_UAV", 1000); } void modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr& msg) { // Buscar el índice del modelo UGV en los mensajes recibidos int ugv_index = -1; for (size_t i = 0; i < msg->name.size(); ++i) { if (msg->name[i] == "robot") { ugv_index = i; break; } } if (ugv_index != -1) { // Publicar la posición X e Y del modelo UGV en el nuevo topic geometry_msgs::Pose ugv_pose; ugv_pose.position.x = msg->pose[ugv_index].position.x; ugv_pose.position.y = msg->pose[ugv_index].position.y; referencia_uav_pub.publish(ugv_pose); } } private: ros::Subscriber model_states_sub; ros::Publisher referencia_uav_pub; }; int main(int argc, char **argv) { ros::init(argc, argv, "referencia_UAV"); ReferenciaUAV referencia_uav_node; ros::spin(); return 0; } A.1 ground_target_follower 71 Código A.7 referencia_UGV_teclado.cpp. /* Nodo para asignarle los valores publicados en el topic / teleop_position al modelo en gazebo del UGV y que así pueda moverse con el teclado */ #include "ros/ros.h" #include "geometry_msgs/Point.h" #include "gazebo_msgs/ModelState.h" class ReferenceUGVSubscriber { public: ReferenceUGVSubscriber() { sub = n.subscribe("/teleop_position", 10, &ReferenceUGVSubscriber ::teleopCallback, this); pub = n.advertise<gazebo_msgs::ModelState>("/gazebo/ set_model_state", 10); model_state.model_name = "robot"; // Nombre del modelo UGV en Gazebo } void teleopCallback(const geometry_msgs::Point::ConstPtr& msg) { model_state.pose.position.x = msg->x; model_state.pose.position.y = msg->y; pub.publish(model_state); } private: ros::NodeHandle n; ros::Subscriber sub; ros::Publisher pub; gazebo_msgs::ModelState model_state; }; int main(int argc, char **argv) { ros::init(argc, argv, "referencia_UGV_teclado"); ReferenceUGVSubscriber referenceUGVSubscriber; ros::spin(); return 0; } Código A.8 yaw.cpp. #include "ros/ros.h" #include "geometry_msgs/PoseStamped.h" #include "std_msgs/Float64.h" 72 Capítulo A. Códigos #include "tf2/LinearMath/Quaternion.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" // Declarar el publisher como variable global ros::Publisher yaw_pub; void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) { // Extraer la orientación del mensaje de pose tf2::Quaternion orientation; tf2::fromMsg(msg->pose.orientation, orientation); // Convertir cuaternión a ángulos de Euler (roll, pitch, yaw) double roll, pitch, yaw; tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); // Publicar el valor de yaw en el topic /yaw_actual std_msgs::Float64 yaw_msg; yaw_msg.data = yaw; yaw_pub.publish(yaw_msg); } int main(int argc, char **argv) { ros::init(argc, argv, "yaw_node"); ros::NodeHandle nh; // Crear el publisher fuera de la función de callback yaw_pub = nh.advertise<std_msgs::Float64>("/yaw_actual", 10); // Suscribirse al topic /ual/mavros ros::Subscriber pose_sub = nh.subscribe("/ual/pose", 10, poseCallback); ros::spin(); return 0; } Código A.9 bt_UAV_node.cpp. /* BehaviorTree */ #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/action_node.h" #include "behaviortree_cpp/bt_factory.h" A.1 ground_target_follower 73 #include <iostream> #include <chrono> #include <thread> #include "ros/ros.h" #include "mavros_msgs/SetMode.h" #include "mavros_msgs/CommandBool.h" #include "mavros_msgs/CommandTOL.h" #include <std_msgs/Bool.h> #include "gazebo_msgs/ModelStates.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/PoseStamped.h" #include "catec_ual_msgs/State.h" #include <cmath> using namespace BT; // ===================== Nodo IsFlyingAuto ===================== // /* Comprobar que esta en modo Flying Auto */ class IsFlyingAuto : public SyncActionNode { public: IsFlyingAuto(const std::string& name): SyncActionNode(name, {}){ ros::NodeHandle nh; state_subscriber = nh.subscribe<catec_ual_msgs::State>("/ual/ state", 1, &IsFlyingAuto::stateCallback, this); } void stateCallback(const catec_ual_msgs::State::ConstPtr& msg){ state_msg.state = msg-> state; state_msg.name = msg -> name; // Imprimir el valor de state_msg.state // ROS_INFO("Received UAL state: %d", state_msg.state); // ROS_INFO("Received UAL name: %s", state_msg.name.c_str()); } NodeStatus tick() override { // Enviar la solicitud de cambio de modo if (state_msg.state == 4) { // ROS_INFO("UAL state is flying_auto"); return NodeStatus::SUCCESS; 80 Capítulo A. Códigos Código A.12 camera_world_tf.cpp. #include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <tf2_ros/transform_listener.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <message_filters/subscriber.h> #include <tf2_ros/message_filter.h> class TransformPoseNode { public: TransformPoseNode() : tfListener(tfBuffer) { poseSub.subscribe(nh, "/catec_ekf/pose", 1); tfFilter = new tf2_ros::MessageFilter<geometry_msgs::PoseStamped >(poseSub, tfBuffer, "world", 1, nh); tfFilter->registerCallback(boost::bind(&TransformPoseNode:: poseCallback, this, _1)); posePub = nh.advertise<geometry_msgs::PoseStamped>(" transformed_pose", 1); } void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& poseMsg) { geometry_msgs::PoseStamped transformedPose; try { tfBuffer.transform(*poseMsg, transformedPose, "world"); posePub.publish(transformedPose); } catch (tf2::TransformException &ex) { ROS_WARN("Transform failed: %s", ex.what()); } } private: ros::NodeHandle nh; message_filters::Subscriber<geometry_msgs::PoseStamped> poseSub; tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener; tf2_ros::MessageFilter<geometry_msgs::PoseStamped>* tfFilter; ros::Publisher posePub; }; int main(int argc, char** argv) A.2 rs_aruco_detector 81 { ros::init(argc, argv, "camera_world_tf"); TransformPoseNode node; ros::spin(); return 0; } A.2 rs_aruco_detector Código A.13 suministra_rs_ar_detector_node.launch. <launch> <!-- ROS related configuration --> <arg name="rs_image_topic" default="/robot/front_rgbd_camera/ rgb/image_raw" /> <arg name="aruco_custom_pose_topic" default="/rs_aruco_detection/ aruco_custom_pose" /> <arg name="aruco_tf_topic" default="/rs_aruco_detection/ aruco_tf" /> <!-- Calibration file of realsense camera--> <arg name="calibration_file" default="$(find rs_aruco_detector)/ config/rs_calibration_parameters.yaml" /> <!-- Markers configuration file --> <arg name="markers_config_file" default="$(find rs_aruco_detector)/ config/markers_final_configuration.yaml" /> <!-- Setting up log level --> <env name="ROSCONSOLE_CONFIG_FILE" value="$(find rs_aruco_detector)/ config/rosconsole.conf"/> <!-- Launching node with the configuration --> <node name="rs_aruco_detection" pkg="rs_aruco_detector" type=" rs_aruco_detector_node" output="screen"> <param name="rs_image_topic" value="$(arg rs_image_topic) " /> <param name="aruco_custom_pose_topic" value="$(arg aruco_custom_pose_topic)" /> <param name="aruco_tf_topic" value="$(arg aruco_tf_topic) " /> <param name="calibration_file" value="$(arg calibration_file)" /> <param name="markers_config_file" value="$(arg markers_config_file)" /> </node> 82 Capítulo A. Códigos </launch> Código A.14 rs_aruco_position.cpp. #include "rs_aruco_detector/aruco_marker_position.h" int main(int argc, char** argv) { ros::init(argc, argv, "rs_aruco_position"); ros::NodeHandle nh("~"); catec_suministra::ArucoMarkerPosition rs_aruco_position(nh); ros::MultiThreadedSpinner spinner(4); spinner.spin(); return 0; } Código A.15 aruco_marker_detections.cpp. #include "rs_aruco_detector/aruco_marker_detections.h" #include <cv_bridge/cv_bridge.h> #include <Eigen/Core> #include <Eigen/Dense> #include <cmath> #include <opencv2/core.hpp> #include <opencv2/core/eigen.hpp> namespace catec_suministra { ArucoMarkerDetections::ArucoMarkerDetections( std::string& calibration_file, std::shared_ptr<ArucoMarkerConfig> aruco_marker_config) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); std::map<std::string, ros::console::levels::Level> logger; ros::console::get_loggers(logger); _log_level = logger[ROSCONSOLE_DEFAULT_NAME]; // Class instance _rs_calibration = std::make_shared<RsCalibration>(calibration_file); // Aruco markers parameters _aruco_marker_config = aruco_marker_config; _store_marker_ids = aruco_marker_config->getStoreMarkerIds(); _aruco_detector_parameters = cv::aruco::DetectorParameters::create() ; A.2 rs_aruco_detector 83 _aruco_dictionary = cv::aruco::getPredefinedDictionary(cv:: aruco::DICT_6X6_250); _obj_points = cv::Mat::zeros(4, 1, CV_32FC3); } ArucoMarkerDetections::~ArucoMarkerDetections() { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); } ArucoDetectionsInfo ArucoMarkerDetections::detectArucoPoses(const cv:: Mat& image) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); int freq, vector_position = 0; double length; cv::Mat output_image, rotation_matrix; std::map<double, int> marker_lengths_frequency; std::vector<cv::Vec4d> quaternion_vecs_out; std::vector<cv::Vec3d> rotation_vecs, total_rotation_vecs , translation_vecs, translation_vecs_out; std::vector<std::vector<cv::Point2f>> marker_corners, marker_corners_input, corners_out, rejected_candidates; std::vector<DetectedMarkerInfo> marker_info_process; std::vector<int> ids_out; std::vector<std::string> frames_out; ArucoDetectionsInfo aruco_detections; output_image = image.clone(); cv::aruco::detectMarkers( image, _aruco_dictionary, marker_corners, _marker_ids, _aruco_detector_parameters, rejected_candidates); // if (_marker_ids.empty()) { // // ROS_WARN("No markers detected."); // return aruco_detections; // } // Check if the markers detected are stored in the config file for (int i = 0; i < _marker_ids.size(); i++) { auto it = std::find(_store_marker_ids.begin(), _store_marker_ids. end(), _marker_ids[i]); if (it != _store_marker_ids.end()) { int position = std::distance(_store_marker_ids.begin(), it); DetectedMarkerInfo info; info.corners = marker_corners[i]; 84 Capítulo A. Códigos info.id = _marker_ids[i]; info.length = _aruco_marker_config->getStoreMarkerLengths()[ position]; info.frame = _aruco_marker_config->getStoreMarkerFrames()[ position]; marker_info_process.push_back(info); } else { ROS_WARN("The aruco marker with id %d is not going to be process.", _marker_ids[i]); } } // Sorting detected markers by length std::sort( marker_info_process.begin(), marker_info_process.end(), [](const DetectedMarkerInfo& a, const DetectedMarkerInfo& b) { return a.length < b.length; }); for (const DetectedMarkerInfo& info : marker_info_process) { marker_lengths_frequency[info.length]++; } // Estimate pose for each marker length auto freq_aux = marker_lengths_frequency.begin(); for (int i = 0; i < marker_lengths_frequency.size(); i++) { length = freq_aux->first; freq = freq_aux->second; marker_corners_input.clear(); for (int i = vector_position; i < (vector_position + freq); i++) { marker_corners_input.push_back(marker_info_process[i].corners ); corners_out.push_back(marker_info_process[i].corners); ids_out.push_back(marker_info_process[i].id); frames_out.push_back(marker_info_process[i].frame); } cv::aruco::estimatePoseSingleMarkers( marker_corners_input, length, _rs_calibration->getIntrinsicsMatrix(), _rs_calibration->getDistCoeffsVector(), rotation_vecs, translation_vecs, _obj_points); for (int i = 0; i < rotation_vecs.size(); i++) { A.2 rs_aruco_detector 85 // CHOSE YOUR BELOVED QUATERNION METHOD HERE // Method 1: Using cv::Rodrigues and Eigen::Quaterniond -> parece la mejor solución cv::Vec<double, 4> quaternion_cv = rotVec2Quat2(rotation_vecs [i]); // Method 2: Calculations without considering sing -> the quaternion has big steps shifts (reflexions?) // cv::Vec<double, 4> quaternion_cv = rotVec2Quat( rotation_vecs[i]); // Method 3: Considering sign -- outputs an error in estimation of an invalid quaternion // cv::Vec<double, 4> quaternion_cv = mRot2Quat(rotation_vecs [i]); // Method 4: Considering sign -> same response as method 1 // cv::Vec<double, 4> quaternion_cv = rotMat2Quat( rotation_vecs[i]); quaternion_vecs_out.push_back(quaternion_cv); total_rotation_vecs.push_back(rotation_vecs[i]); translation_vecs_out.push_back(translation_vecs[i]); } vector_position = vector_position + freq; freq_aux++; ROS_DEBUG_STREAM("length: " << length); ROS_DEBUG_STREAM("freq: " << freq); ROS_DEBUG_STREAM("vector_position: " << vector_position); } // if (_log_level == ros::console::levels::Debug) { cv::aruco::drawDetectedMarkers(output_image, corners_out, ids_out); for (int i = 0; i < marker_info_process.size(); i++) { cv::drawFrameAxes( output_image, _rs_calibration->getIntrinsicsMatrix(), _rs_calibration->getDistCoeffsVector(), total_rotation_vecs[i], translation_vecs_out[i], marker_info_process[i].length); } cv::imshow("RealSense D435i", output_image); cv::waitKey(1); // } for (int i = 0; i < marker_info_process.size(); i++) { 86 Capítulo A. Códigos ROS_DEBUG_STREAM("Marker lengths process ordered value: " << marker_info_process[i].length); ROS_DEBUG_STREAM("Marker ids process ordered value: " << marker_info_process[i].id); ROS_DEBUG_STREAM("Marker frames process ordered value: " << marker_info_process[i].frame); ROS_DEBUG_STREAM("Marker corners process ordered value: " << marker_info_process[i].corners); ROS_DEBUG_STREAM("Translation vector: " << translation_vecs_out[ i]); ROS_DEBUG_STREAM("Quaternion: " << quaternion_vecs_out[i]); } aruco_detections.rvecs = quaternion_vecs_out; aruco_detections.tvecs = translation_vecs_out; aruco_detections.ids = ids_out; aruco_detections.frames = frames_out; return aruco_detections; } cv::Vec4d ArucoMarkerDetections::rotVec2Quat2(const cv::Vec3d& rot_vec) // m1 { cv::Vec4d quaternion_vec; cv::Mat rotation_matrix; Eigen::Matrix3d eigen_rot_mat; Eigen::Quaterniond quaternion; cv::Rodrigues(rot_vec, rotation_matrix); cv2eigen(rotation_matrix, eigen_rot_mat); Eigen::Quaterniond eigen_quat(eigen_rot_mat); quaternion = eigen_quat.normalized(); cv::Vec<double, 4> quaternion_cv(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); return quaternion_cv; } cv::Vec4d ArucoMarkerDetections::rotVec2Quat(const cv::Vec3d& rot_vec) // m2 { cv::Vec3d axis; cv::Vec4d quaternion; double theta = cv::norm(rot_vec); double s = sin(theta / 2); axis = rot_vec / theta; A.2 rs_aruco_detector 87 quaternion[1] = axis[0] * s; quaternion[2] = axis[1] * s; quaternion[3] = axis[2] * s; quaternion[0] = cos(theta / 2); return quaternion; } inline float ArucoMarkerDetections::SIGN(float x) { return (x >= 0.0f) ? +1.0f : -1.0f; } inline float ArucoMarkerDetections::NORM(float a, float b, float c, float d) { return sqrt(a * a + b * b + c * c + d * d); } // quaternion = [w, x, y, z]’ cv::Vec4d ArucoMarkerDetections::mRot2Quat(const cv::Vec3d& rot_vec) // m3 { cv::Mat m; cv::Rodrigues(rot_vec, m); float r11 = m.at<float>(0, 0); float r12 = m.at<float>(0, 1); float r13 = m.at<float>(0, 2); float r21 = m.at<float>(1, 0); float r22 = m.at<float>(1, 1); float r23 = m.at<float>(1, 2); float r31 = m.at<float>(2, 0); float r32 = m.at<float>(2, 1); float r33 = m.at<float>(2, 2); float q0 = (r11 + r22 + r33 + 1.0f) / 4.0f; float q1 = (r11 - r22 - r33 + 1.0f) / 4.0f; float q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f; float q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f; if (q0 < 0.0f) { q0 = 0.0f; } if (q1 < 0.0f) { q1 = 0.0f; } if (q2 < 0.0f) { q2 = 0.0f; } if (q3 < 0.0f) { q3 = 0.0f; 88 Capítulo A. Códigos } q0 = sqrt(q0); q1 = sqrt(q1); q2 = sqrt(q2); q3 = sqrt(q3); if (q0 >= q1 && q0 >= q2 && q0 >= q3) { q0 *= +1.0f; q1 *= SIGN(r32 - r23); q2 *= SIGN(r13 - r31); q3 *= SIGN(r21 - r12); } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) { q0 *= SIGN(r32 - r23); q1 *= +1.0f; q2 *= SIGN(r21 + r12); q3 *= SIGN(r13 + r31); } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) { q0 *= SIGN(r13 - r31); q1 *= SIGN(r21 + r12); q2 *= +1.0f; q3 *= SIGN(r32 + r23); } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) { q0 *= SIGN(r21 - r12); q1 *= SIGN(r31 + r13); q2 *= SIGN(r32 + r23); q3 *= +1.0f; } else { printf("coding error\n"); } float r = NORM(q0, q1, q2, q3); q0 /= r; q1 /= r; q2 /= r; q3 /= r; cv::Mat quaternion = (cv::Mat_<float>(4, 1) << q0, q1, q2, q3); return quaternion; } cv::Vec4d ArucoMarkerDetections::rotMat2Quat(const cv::Vec3d& rot_vec) // m4 { cv::Mat mr; cv::Mat m; cv::Mat quaternion(4, 1, CV_64F); cv::Rodrigues(rot_vec, mr); // Normalize rotation matrix cv::SVD svd(mr); A.2 rs_aruco_detector 89 // The rotation part is the product of U and Vt m = svd.u * svd.vt; double trace = cv::trace(m)[0]; if (trace > 0.0) { double s = 0.5 / sqrt(trace + 1.0); quaternion.at<double>(0) = 0.25 / s; quaternion.at<double>(1) = (m.at<double>(2, 1) - m.at<double>(1, 2)) * s; quaternion.at<double>(2) = (m.at<double>(0, 2) - m.at<double>(2, 0)) * s; quaternion.at<double>(3) = (m.at<double>(1, 0) - m.at<double>(0, 1)) * s; } else { if (m.at<double>(0, 0) > m.at<double>(1, 1) && m.at<double>(0, 0) > m.at<double>(2, 2)) { double s = 2.0 * sqrt(1.0 + m.at<double>(0, 0) - m.at<double>(1, 1) - m.at<double>(2, 2)); quaternion.at<double>(0) = (m.at<double>(2, 1) - m.at<double >(1, 2)) / s; quaternion.at<double>(1) = 0.25 * s; quaternion.at<double>(2) = (m.at<double>(0, 1) + m.at<double >(1, 0)) / s; quaternion.at<double>(3) = (m.at<double>(0, 2) + m.at<double >(2, 0)) / s; } else if (m.at<double>(1, 1) > m.at<double>(2, 2)) { double s = 2.0 * sqrt(1.0 + m.at<double>(1, 1) - m.at<double>(0, 0) - m.at<double>(2, 2)); quaternion.at<double>(0) = (m.at<double>(0, 2) - m.at<double >(2, 0)) / s; quaternion.at<double>(1) = (m.at<double>(0, 1) + m.at<double >(1, 0)) / s; quaternion.at<double>(2) = 0.25 * s; quaternion.at<double>(3) = (m.at<double>(1, 2) + m.at<double >(2, 1)) / s; } else { double s = 2.0 * sqrt(1.0 + m.at<double>(2, 2) - m.at<double>(0, 0) - m.at<double>(1, 1)); quaternion.at<double>(0) = (m.at<double>(1, 0) - m.at<double >(0, 1)) / s; quaternion.at<double>(1) = (m.at<double>(0, 2) + m.at<double >(2, 0)) / s; quaternion.at<double>(2) = (m.at<double>(1, 2) + m.at<double >(2, 1)) / s; quaternion.at<double>(3) = 0.25 * s; } } cv::Vec4d quaternion_cv( 96 Capítulo A. Códigos return; } else { for (int i = 0; i < poses_frames.size(); i++) { auto it = std::find(topic_list.begin(), topic_list.end(), poses_frames[i]); if (it != topic_list.end()) { int index = std::distance(topic_list.begin(), it); ROS_DEBUG_STREAM( "Publishing topic: " << topic_list[index] << " = " << "Pose frame: " << poses_frames[i]); publisher_list[index].publish(poses_vector[i]); } else { ROS_WARN("No matching topic found for frame: %s", poses_frames[i].c_str()); } } } // Check for NaN or Inf values for (const auto& o : poses_vector) { if (std::isnan(o.pose.orientation.x) || std::isinf(o.pose. orientation.x) || std::isnan(o.pose.orientation.y) || std::isinf(o.pose.orientation.y) || std::isnan(o.pose. orientation.z) || std::isinf(o.pose.orientation.z) || std::isnan(o.pose.orientation.w) || std::isinf(o.pose. orientation.w)) { ROS_WARN("Pose has NaN or Inf values"); continue; } } } void ArucoMarkerPosition::publishTfsVector( const ros::Publisher& publisher, const std::vector<geometry_msgs::TransformStamped>& tfs_vector) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); if (tfs_vector.size() <= 0) { return; } else { static tf2_ros::TransformBroadcaster br; for (const auto& aruco : tfs_vector) { publisher.publish(aruco); br.sendTransform(aruco); } } } A.2 rs_aruco_detector 97 void ArucoMarkerPosition::publishStaticTfsVector(const std::vector< geometry_msgs::TransformStamped>& static_tfs_vector) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); if (static_tfs_vector.empty()) { ROS_WARN("No static tfs to publish."); return; } else { static tf2_ros::StaticTransformBroadcaster static_broadcaster; static_broadcaster.sendTransform(static_tfs_vector); } } // void ArucoMarkerPosition::publishCustomDetections(const std_msgs:: Header& header, ArucoDetectionsInfo aruco_detections) // { // ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); // ROS_DEBUG_STREAM("Ids size: " << aruco_detections.ids.size()); // if (aruco_detections.ids.size() <= 0) { // ROS_WARN("No aruco markers detected."); // return; // } // suministra_msgs::MarkerDetectionArray detections_msg; // detections_msg.header.stamp = header.stamp; // detections_msg.header.frame_id = header.frame_id; // if (aruco_detections.ids.size() == 0) { // ROS_DEBUG_STREAM("Not messages to publish "); // detections_msg.detections.resize(0); // } else { // for (int i = 0; i < aruco_detections.ids.size(); i++) { // suministra_msgs::MarkerDetection detection; // detection.marker_id = aruco_detections.ids[i]; // detection.child_frame_id = aruco_detections.frames[i]; // detection.pose.position.x = aruco_detections.tvecs[i][0]; // detection.pose.position.y = aruco_detections.tvecs[i][1]; // detection.pose.position.z = aruco_detections.tvecs[i][2]; // detection.pose.orientation.x = aruco_detections.rvecs[i ][0]; // detection.pose.orientation.y = aruco_detections.rvecs[i ][1]; // detection.pose.orientation.z = aruco_detections.rvecs[i ][2]; // detection.pose.orientation.w = aruco_detections.rvecs[i ][3]; 98 Capítulo A. Códigos // detections_msg.detections.push_back(detection); // } // } // _ar_detections_publisher_custom.publish(detections_msg); // } } // namespace catec_suministra Código A.17 aruco_marker_config.cpp. #include <rs_aruco_detector/aruco_marker_config.h> #include <yaml-cpp/yaml.h> namespace catec_suministra { ArucoMarkerConfig::ArucoMarkerConfig(std::string& config_filename) { markerInfo(config_filename); } ArucoMarkerConfig::~ArucoMarkerConfig() { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); } void ArucoMarkerConfig::markerInfo(const std::string& config_filename) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); cv::FileStorage fs(config_filename, cv::FileStorage::READ); if (!fs.isOpened()) { ROS_ERROR("Could not open file %s", config_filename.data()); throw std::runtime_error("Could not open file"); } ROS_INFO( "[%s] Markers configuration stored in file ’%s’", ros:: this_node::getName().data(), config_filename.data()); cv::FileNode markers_node = fs["markers"]; if (markers_node.empty()) { throw std::runtime_error("Could not find ’markers’ node in file: " + config_filename); } cv::FileNode frame_pattern_node = fs["frame_pattern"]; if (frame_pattern_node.empty()) { throw std::runtime_error("Could not find ’frame_pattern’ node in file: " + config_filename); A.2 rs_aruco_detector 99 } _frame_pattern = (std::string)frame_pattern_node; int num_markers = 0; std::vector<std::string> marker_name_list; for (auto it = markers_node.begin(); it != markers_node.end(); it++) { num_markers++; } marker_name_list.reserve(num_markers); for (int i = 0; i < num_markers; i++) { marker_name_list.push_back("marker" + std::to_string(i)); } for (const auto& marker_name : marker_name_list) { cv::FileNode marker_node = markers_node[marker_name]; ArucoMarker marker; marker._marker_id = (int)marker_node["id"]; marker._length = (double)marker_node["length"]; marker._distance_x = (double)marker_node["distance_x"]; marker._distance_y = (double)marker_node["distance_y"]; marker._distance_z = (double)marker_node["distance_z"]; marker._roll = (double)marker_node["roll"]; marker._pitch = (double)marker_node["pitch"]; marker._yaw = (double)marker_node["yaw"]; marker._marker_frame_id = (std::string)marker_node["frame"]; _markers_config.push_back(marker); _store_marker_ids.push_back(marker_node["id"]); _store_marker_lengths.push_back(marker_node["length"]); _store_marker_frames.push_back(marker_node["frame"]); ROS_INFO_STREAM( "Marker ID: " << marker._marker_id << "\n" << "Length: " << marker._length << "\n" << "Distance X: " << marker._distance_x << "\ n" << "Distance Y: " << marker._distance_y << "\ n" << "Distance Z: " << marker._distance_z << "\ n" << "Roll: " << marker._roll << "\n" << "Pitch: " << marker._pitch << "\n" << "Yaw: " << marker._yaw << "\n" 100 Capítulo A. Códigos << "Marker Frame ID: " << marker. _marker_frame_id << "\n" << "------------------------"); } } } // namespace catec_suministra Código A.18 rs_calibration_yaml.cpp. #include "rs_aruco_detector/rs_calibration_yaml.h" namespace catec_suministra { RsCalibration::RsCalibration(std::string& config_filename) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); readCalibrationParameters(config_filename); } RsCalibration::~RsCalibration() { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); } void RsCalibration::readCalibrationParameters(const std::string& calibration_filename) { ROS_DEBUG("[%s]", __PRETTY_FUNCTION__); cv::FileStorage fs(calibration_filename, cv::FileStorage::READ); if (!fs.isOpened()) { ROS_ERROR("[%s] Could not open file ‘%s‘", ros::this_node:: getName().data(), calibration_filename.data()); throw std::runtime_error("Could not open file"); } cv::FileNode color_node = fs["color"]; if (color_node.empty()) { throw std::runtime_error("Could not find ’color’ node in file: " + calibration_filename); } const cv::FileNode intrinsics_node = color_node["intrinsics"]; const cv::FileNode distortion_node = color_node[" distortion_coefficients"]; const double fx = intrinsics_node[0]; const double fy = intrinsics_node[1]; A.3 suministra_pose_estimation 101 const double cx = intrinsics_node[2]; const double cy = intrinsics_node[3]; const double k1 = distortion_node[0]; const double k2 = distortion_node[1]; const double p1 = distortion_node[2]; const double p2 = distortion_node[3]; _intrinsics_matrix = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); _dist_coeffs_vector = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, 0); ROS_INFO( "[%s] Calibration parameters loaded from file ‘%s‘", ros::this_node::getName().data(), calibration_filename.data()); ROS_INFO("fx: %f, fy: %f", _intrinsics_matrix.at<double>(0, 0), _intrinsics_matrix.at<double>(1, 1)); ROS_INFO("cx: %f, cy: %f", _intrinsics_matrix.at<double>(0, 2), _intrinsics_matrix.at<double>(1, 2)); ROS_INFO("k1: %f, k2: %f", _dist_coeffs_vector.at<double>(0, 0), _dist_coeffs_vector.at<double>(0, 1)); ROS_INFO("p1: %f, p2: %f", _dist_coeffs_vector.at<double>(0, 3), _dist_coeffs_vector.at<double>(0, 4)); } } // namespace catec_suministra A.3 suministra_pose_estimation Código A.19 pose_estimator.launch. <?xml version="1.0"?> <launch> <arg name="ekf_config" default="suministra"/> <arg name="desired_state" default="suministra"/> <!-- Subscribed Topics --> <arg name="topic_ar_pose0" default="/rs_aruco_detection/ pattern/ar_marker_0"/> <arg name="topic_ar_pose1" default="/rs_aruco_detection/ pattern/ar_marker_1"/> <arg name="topic_ar_pose2" default="/rs_aruco_detection/ pattern/ar_marker_2"/> <arg name="topic_ar_pose3" default="/rs_aruco_detection/ pattern/ar_marker_3"/> <arg name="topic_ar_pose4" default="/rs_aruco_detection/ pattern/ar_marker_1"/> 102 Capítulo A. Códigos <arg name="topic_ar_pose5" default="/rs_aruco_detection/ pattern/ar_marker_2"/> <arg name="topic_ar_pose6" default="/rs_aruco_detection/ pattern/ar_marker_3"/> <arg name="topic_ar_pose7" default="/rs_aruco_detection/ pattern/ar_marker_4"/> <!-- Publised Topics --> <arg name="topic_estimated_pose" default="/catec_ekf/pose"/> <!-- TFs --> <arg name="base_frame_id" default=" summit_xl_front_rgbd_camera_base_link"/> <arg name="odom_frame_id" default=" suministra_rs_color_optical_frame"/> <arg name="map_frame_id" default="map"/> <arg name="target_frame_id" default=" suministra_rs_color_optical_frame"/> <arg name="publish_local_body_tf" default="true"/> <node name="suministra_pose_estimation" pkg=" suministra_pose_estimation" type="suministra_pose_estimation_node " respawn="false" output="screen" launch-prefix="gdb -ex run -- args"> <param name="ekf_config" value="$(arg ekf_config)"/> <param name="desired_state" value="$(arg desired_state) "/> <param name="topic_ar_pose0" value="$(arg topic_ar_pose0) "/> <param name="topic_ar_pose1" value="$(arg topic_ar_pose1) "/> <param name="topic_ar_pose2" value="$(arg topic_ar_pose2) "/> <param name="topic_ar_pose3" value="$(arg topic_ar_pose3) "/> <param name="topic_ar_pose4" value="$(arg topic_ar_pose4) "/> <param name="topic_ar_pose5" value="$(arg topic_ar_pose5) "/> <param name="topic_ar_pose6" value="$(arg topic_ar_pose6) "/> <param name="topic_ar_pose7" value="$(arg topic_ar_pose7) "/> <param name="topic_estimated_pose" value="$(arg topic_estimated_pose)"/> <param name="base_frame_id" value="$(arg base_frame_id) "/> A.3 suministra_pose_estimation 103 <param name="odom_frame_id" value="$(arg odom_frame_id) "/> <param name="map_frame_id" value="$(arg map_frame_id) "/> <param name="target_frame_id" value="$(arg target_frame_id )"/> <param name="publish_local_body_tf" value="$(arg publish_local_body_tf)"/> </node> </launch> Código A.20 suministra_pose_estimation_node.cpp. #include "suministra_pose_estimation/ros/nodes/ suministra_pose_estimation_node.h" #include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/TwistStamped.h> namespace catec { catecPoseEstimatorNode::catecPoseEstimatorNode() { LOG_TRACE(__PRETTY_FUNCTION__); _nh = std::make_shared<ros::NodeHandle>("~"); _ros_helper = std::make_unique<RosHelper>(_nh); std::string desired_state; desired_state = fetchMandatoryParam("desired_state"); std::string ekf_config; _nh->param<std::string>("ekf_config", ekf_config, "default"); const auto configuration = loadEKFConfig(ekf_config); _ekf = std::make_shared<EKF>(configuration); _ekf_sources = std::make_unique<EKFSources>(desired_state); std::string estimated_pose_topic; _nh->param<std::string>("estimated_pose_topic", estimated_pose_topic, "/catec_ekf/pose"); _nh->param<std::string>("base_frame_id", _base_frame_id, "base_link "); _nh->param<std::string>("odom_frame_id", _odom_frame_id, "odom"); _nh->param<std::string>("map_frame_id", _map_frame_id, "map"); _nh->param<std::string>("target_frame_id", _target_frame_id, "odom") ; 104 Capítulo A. Códigos _nh->param<bool>("publish_local_body_tf", _publish_local_body_tf, false); for (auto& source : _ekf_sources->sources) { std::string associated_launch_argument = source.second. associated_launch_argument; std::string topic = fetchMandatoryParam( associated_launch_argument); ros::Subscriber subscriber; // clang-format off if (associated_launch_argument.find("pose") != std::string::npos) { subscriber = _nh->subscribe<geometry_msgs::PoseStamped>(topic , 1, callback<geometry_msgs::PoseStamped, PoseStampedElement>(_ekf, source.second)); } // clang-format on _subscribers.insert({topic, subscriber}); } _ekf_pose_publisher = _nh->advertise<geometry_msgs::PoseStamped>( estimated_pose_topic, 1); _period = _ekf->getUpdateRate(); _track_thread = std::thread(&catecPoseEstimatorNode::trackThread, this); _previous_timestamp = std::chrono::system_clock::now(); LOG_INFO("Suministra Pose Estimation Node initialized. Waiting for topics to be published..."); ROS_INFO_ONCE("Suministra Pose Estimation Node initialized. Waiting for topics to be published..."); } catecPoseEstimatorNode::~catecPoseEstimatorNode() { LOG_TRACE(__PRETTY_FUNCTION__); _stop_atomic.store(true, std::memory_order_relaxed); if (_track_thread.joinable()) { _track_thread.join(); } } void catecPoseEstimatorNode::trackThread() { LOG_TRACE(__PRETTY_FUNCTION__); _start_time = std::chrono::system_clock::now(); A.3 suministra_pose_estimation 105 while (!_stop_atomic.load(std::memory_order_relaxed)) { if (!_ekf->isInitialized()) { _previous_timestamp = std::chrono::system_clock::now(); continue; } _ekf->predictStep(computeEKFTimeStep()); const struct EKFState st = _ekf->getEKFState(); ///\todo: Refactor this auto hasInf = [](const Eigen::VectorXd& vec) { for (unsigned int i = 0; i < vec.size(); i++) { if (std::isinf(vec(i))) { return true; } } return false; }; auto isNanOrInf = [&hasInf](const Eigen::VectorXd& vec) { return vec.hasNaN() || hasInf(vec); }; if (isNanOrInf(st.pos) || isNanOrInf(st.ori)) { LOG_WARN("EKF state has NaN or Inf values. Not publishing ..."); continue; } ROS_INFO_ONCE("Done! Publishing state..."); if (_publish_local_body_tf) { _ros_helper->publishLocalBodyTF(st.pos, st.ori, _odom_frame_id, _base_frame_id); ROS_WARN_ONCE("Publishing odom frame to body frame TF"); } // clang-format off _ros_helper->publishKalmanPose(st.pos, st.ori, _ekf_pose_publisher, _target_frame_id, _odom_frame_id); // clang-format on // Calculate the next start time based on the completed periods that have passed std::this_thread::sleep_until(calculateNextIteration()); } } EKFConfig catecPoseEstimatorNode::loadEKFConfig(const std::string& ekf_config) { LOG_TRACE(__PRETTY_FUNCTION__); 112 Bibliografía [15] G. N. Muchiri and S. Kimathi. A review of applications and potential applications of uav. In Proceedings of the Sustainable Research and Innovation Conference, pages 280–283, April 2022. [16] P. Radoglou-Grammatikis, P. Sarigiannidis, T. Lagkas, and I. Moscholios. A compilation of uav applications for precision agriculture. Computer Networks, 172:107148, 2020. [17] B. J. Yoon, M. W. Park, and J. H. Kim. Ugv (unmanned ground vehicle) navigation method using gps and compass. In 2006 SICE-ICASE International Joint Conference, pages 3621– 3625. IEEE, October 2006. [18] RobotShop. Chasis de robot 6x4 ugv todo terreno con rieles de extensión y esp32 esclavo, 2024. Accessed: 2024-07-14. [19] Encyclopedia. Article title, 2024. Accessed: 2024-07-14. [20] A. K. H. I. L. Cherukuri and A. S. Neti. Unmanned ground vehicle for military purpose. International Journal of Pure and Applied Mathematics, 19(l2):13189–13193, 2018. [21] Agencia SINC. La investigadora que utilizó robots de rescate en el 11-s y en el huracán katrina visita castellón, 2024. Accessed: 2024-07-14. [22] Defensa. Vehículos no tripulados terrestres para el ejército de tierra español, 2024. Accessed: 2024-07-14. [23] Y. Shen and C. Wei. Target tracking and enclosing via uav/ugv cooperation using energy estimation pigeon-inspired optimization and switchable topology. Aircraft Engineering and Aerospace Technology, 95(5):768–783, 2023. [24] El Houssein Chouaib Harik, François Guérin, Frédéric Guinand, Jean-François Brethé, and Hervé Pelvillain. Uav-ugv cooperation for objects transportation in an industrial area. In 2015 IEEE International Conference on Industrial Technology (ICIT), pages 547–552, 2015. [25] Bojan Arbanas, Alan Ivanovic, Mirko Car, et al. Decentralized planning and control for uav–ugv cooperative teams. Autonomous Robots, 42(7):1601–1618, 2018. [26] S. Ivaldi, V. Padois, and F. Nori. Tools for dynamics simulation of robots: A survey based on user feedback. arXiv preprint arXiv:1402.7050, 2014. [27] A. Staranowicz and G. L. Mariottini. A survey and comparison of commercial and open-source robotic simulator software. In Proceedings of the 4th International Conference on PErvasive Technologies Related to Assistive Environments, pages 1–8, May 2011. [28] H. C. Kam, Y. K. Yu, and K. H. Wong. An improvement on aruco marker for pose tracking using kalman filter. In 2018 19th IEEE/ACIS International Conference on Software Engineering, Artificial Intelligence, Networking and Parallel/Distributed Computing (SNPD), pages 65–69. IEEE, June 2018. [29] OpenCV. Aruco detection, 2024. Accessed: 2024-07-14. [30] OpenCV. Opencv documentation, 2024. Accessed: 2024-07-14. [31] Katarzyna Filus, Łukasz Sobczak, Joanna Domańska, Adam Domański, and Rafał Cupek. Realtime testing of vision-based systems for agvs with aruco markers. In 2022 IEEE International Conference on Big Data (Big Data), pages 6290–6298, 2022. Bibliografía 113 [32] Adrian Rosebrock. Generating aruco markers with opencv and python, December 2020. Accessed: 2024-07-14. [33] L. Ruifeng, W. Jiasheng, Z. Haolong, and T. Mengfan. Research progress and application of behavior tree technology. In 2019 6th International Conference on Behavioral, Economic and Socio-Cultural Computing (BESC), pages 1–4. IEEE, October 2019. [34] R. Ghzouli, T. Berger, E. B. Johnsen, A. Wasowski, and S. Dragule. Behavior trees and state machines in robotics applications. IEEE Transactions on Software Engineering, 49(9):4243– 4267, September 2023. [35] SDFormat. Sdformat tutorials. http:// sdformat.org/ tutorials? [Último acceso: 8 de julio de 2024]. [36] PCComponentes. Dell precision 7540 - intel core i7-9850h, 16gb, 512gb ssd, quadro t2000, 2024. Accessed: 2024-07-14. [37] Open Source Robotics Foundation. Ros (robot operating system), 2024. Accessed: 2024-07-07. [38] Open Source Robotics Foundation. Ros noetic installation on ubuntu, 2024. Accessed: 2024-07-07. [39] Robtech Impaciente. Ros (robot operating system): Fundamentos. 2020. Accessed: 2024-07- 14. [40] Open Source Robotics Foundation. Gazebo simulator, 2024. Accessed: 2024-07-07. [41] H. Chao, Y. Cao, and Y. Chen. Autopilots for small unmanned aerial vehicles: a survey. International Journal of Control, Automation and Systems, 8:36–44, 2010. [42] ArduPilot. Ardupilot: Open source autopilot. https:// ardupilot.org/ , 2023. Accessed: 2024- 07-15. [43] Fernando Real, Alejandro Torres-González, Pablo Ramón-Soria, Jesús Capitán, and Aníbal Ollero. Unmanned aerial vehicle abstraction layer: An abstraction layer to operate unmanned aerial vehicles. International Journal of Advanced Robotic Systems, 17(4), 2020. [44] D. Saccani, L. Cecchin, and L. Fagiano. Multitrajectory model predictive control for safe uav navigation in an unknown environment. IEEE Transactions on Control Systems Technology, 31(5):1982–1997, Sept 2023. [45] S. Balakirsky and Z. Kootbally. Usarsim/ros: A combined framework for robotic control and simulation. In International Symposium on Flexible Automation, volume 45110, pages 101–108. American Society of Mechanical Engineers, June 2012. [46] V-REP forum. Fail to publish to ROS from plugin. En línea. Disponible en: http://www. forum.coppeliarobotics.com/ viewtopic.php?t=5766. [Último acceso: 07-julio-2024]. [47] J.-K. Lee, H. Jung, H. Hu, and D. H. Kim. Collaborative control of uav/ugv. In 2014 11th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI), pages 641–645, Kuala Lumpur, Malaysia, 2014.