Desarrollo de un sistema de navegación para un robot social

UNIVERSIDAD DE VALLADOLID ESCUELA DE INGENIERIAS INDUSTRIALES Grado en Ingeniería Electrónica Industrial y Automática Desarrollo de un sistema de navegación para un robot social. Autor: García Gómez, Miguel Tutor(es): Zalama Casanova, Eduardo Gómez Ramos, Raúl Departamento de Ingeniería de Sistemas y Automática Valladolid, Julio 2022 Agradecimientos A mis tutores Eduardo y Raúl, por su preocupación y ayuda diaria en la elaboración del proyecto, dedicando su tiempo a resolverme dudas y transmitirme sus conocimientos sobre el tema. A mis padres y mis abuelos, por formarme como persona y apoyarme en todo momento. Gracias a vosotros he podido llegar hasta aquí. A mis amigos de la carrera, con los que he compartido durante estos cuatro años alegrías y agobios. A mis amigos de siempre, mi segunda familia, por ser siempre una vía de escape ante cualquier mal momento. A Celia, por ser uno de mis pilares fundamentales desde primero de carrera, con su apoyo y respaldo siempre que lo he necesitado. Resumen El presente trabajo consiste en la familiarización con la programación en ROS (“Robot Operating System”) y en el desarrollo de un sistema de navegación autónoma para el robot Turtlebot2 mediante la generación de mapas de entorno. Este robot podrá navegar a destinos establecidos de acuerdo a una agenda, ante la llamada del usuario o ante decisiones del sistema de control. Para la familiarización con ROS, se han desarrollado diversos programas y se ha estudiado el funcionamiento de este entorno con la programación de distintos nodos y el manejo de mensajes y servicios. Además, se ha estudiado a fondo el “stack” de navegación de ROS. También se han estudiado los diferentes componentes del Hardware del robot y su comunicación con ROS. Palabras clave: ROS, Turtlebot, Topic, Nodos, Odometría. Abstract This work consists of familiarisation with programming in ROS ("Robot Operating System") and the development of an autonomous navigation system for the Turtlebot2 robot through the generation of environment maps. This robot will be able to navigate to established destinations according to an agenda, upon the user's call or upon decisions of the control system. In order to become familiar with ROS, various programmes have been developed and the operation of this environment has been studied with the programming of different nodes and the handling of messages and services. In addition, the ROS navigation stack was studied in depth. The different components of the robot hardware and its communication with ROS have also been studied. Keywords: ROS, Turtlebot, Topic, Nodes, Odometry Índice 1. Capítulo 1. Introducción. .........................................................................................1 1.1. Contexto ..........................................................................................................1 1.2. Motivación ......................................................................................................2 1.3. Objetivos .........................................................................................................3 1.4. Elementos del proyecto ...................................................................................3 1.5. Estructura de la memoria .................................................................................5 2. Capítulo 2. Antecedentes.........................................................................................7 2.1. Robótica ..........................................................................................................7 2.2. Evolución de ROS (Robot Operating System) .................................................. 12 2.3. La familia TurtleBot ........................................................................................ 14 3. Capítulo 3. Descripción del sistema ....................................................................... 15 3.1. Robot Operating System (ROS)....................................................................... 15 3.1.1. Arquitectura y conceptos ....................................................................... 16 3.1.1.1. El sistema de ficheros ..................................................................... 16 3.1.1.2. Grafo de procesos .......................................................................... 19 3.1.1.3. Comunidad de ROS ......................................................................... 23 3.1.2. Herramientas y comandos ...................................................................... 23 3.1.2.1. RVIZ ................................................................................................ 23 3.1.2.2. Simulador Gazebo .......................................................................... 24 3.1.2.3. Comandos útiles ............................................................................. 25 3.2. TurtleBot2 ..................................................................................................... 27 3.2.1. Base móvil .............................................................................................. 28 3.2.2. PC Controlador ....................................................................................... 31 3.2.3. Cámara 3D ............................................................................................. 32 3.2.4. Telémetro láser de escaneo ................................................................... 33 3.2.5. Brazo robótico........................................................................................ 34 3.3. Pila de navegación de ROS ............................................................................. 35 3.3.1. Odometría (odometry source) ................................................................ 36 3.3.2. Algoritmo de localización adaptativo de Monte Carlo (AMCL) ................ 36 3.3.3. Transformadas de los sensores............................................................... 37 3.3.4. Funcionamiento de la pila ...................................................................... 37 3.3.4.1. Mapas de costo .............................................................................. 37 3.3.4.2. Planificadores ................................................................................. 38 3.3.4.3. Recovery Behaviors ........................................................................ 38 3.3.4.4. Parámetros .................................................................................... 39 3.4. Puesta en marcha del robot ........................................................................... 39 3.4.1. Comunicación entre ordenadores .......................................................... 39 3.4.2. Conectar ROS Máster ............................................................................. 40 3.4.3. Espacio de trabajo .................................................................................. 41 3.4.4. Preparar Turtlebot ................................................................................. 41 3.4.5. Teleoperación por teclado ..................................................................... 42 3.4.6. Verificar el funcionamiento de la cámara ............................................... 44 3.4.7. Construir un mapa (SLAM) ..................................................................... 45 3.4.8. Navegación autónoma en un mapa conocido ......................................... 49 3.4.9. Aparcamiento automático...................................................................... 49 4. Capítulo 4. Sistema de mapeo y navegación. ......................................................... 53 4.1. Mapeo automático. ....................................................................................... 53 4.1.1. Descripción del problema....................................................................... 53 4.1.2. Diagrama de flujo del programa ............................................................. 53 4.1.3. Explicación del código ............................................................................ 55 4.1.4. Mejoras para el programa bumper.py .................................................... 59 4.1.5. Paquete “explore_lite” ........................................................................... 60 4.2. Sistema de navegación según agenda, llamada de usuario o decisiones del sistema de control..................................................................................................... 62 4.2.1. Descripción del problema....................................................................... 62 4.2.2. Método de escritura de los destinos en el fichero .................................. 62 4.2.2.1. Explicación del código .................................................................... 63 4.2.3. Librería “Schedule” ................................................................................ 68 4.2.4. Sistema de navegación ........................................................................... 68 4.2.4.1. Diagrama de flujo ........................................................................... 69 4.2.4.2. Descripción de funciones ................................................................ 73 5. Capítulo 5. Puesta en marcha y resultados. ........................................................... 85 5.1. Exploración .................................................................................................... 85 5.1.1. Puesta en marcha bumper.py ................................................................. 85 5.1.2. Resultados bumper.py ........................................................................... 86 5.1.3. Puesta en marcha “explore_lite” ............................................................ 86 5.1.4. Resultados “explore_lite” ....................................................................... 87 5.2. Sistema de navegación por agenda ................................................................ 88 5.2.1. Puesta en marcha................................................................................... 88 5.2.2. Resultados ............................................................................................. 90 6. Capítulo 6. Gestión del trabajo. ............................................................................. 91 6.1. Planificación del proyecto .............................................................................. 91 6.2. Estudio económico ........................................................................................ 92 6.2.1. Recursos empleados .............................................................................. 92 6.2.2. Costes directos ....................................................................................... 93 6.2.2.1. Coste del personal .......................................................................... 93 6.2.2.2. Coste de amortización de equipos y programas .............................. 94 6.2.2.3. Costes del material utilizado ........................................................... 95 6.2.2.4. Costes directos totales ................................................................... 96 6.2.3. Costes indirectos .................................................................................... 96 6.2.4. Costes totales......................................................................................... 97 7. Capítulo 7. Conclusiones y líneas de trabajo futuras. ............................................. 99 7.1. Conclusiones generales .................................................................................. 99 7.2. Conclusiones sobre los objetivos planteados.................................................. 99 7.3. Propuestas de trabajo futuro ....................................................................... 101 8. Bibliografía .......................................................................................................... 103 9. Anexos ................................................................................................................ 107 9.1. Código ......................................................................................................... 107 9.1.1. Scripts .................................................................................................. 107 9.1.1.1. bumper.py .................................................................................... 107 9.1.1.2. clickedpoint.py ............................................................................. 109 9.1.1.3. posinicial.py ................................................................................. 111 9.1.1.4. sch_pos_with_battery_v4.py ........................................................ 112 9.1.2. Archivos launch .................................................................................... 120 9.1.2.1. minimalv2.launch del paquete scheduleposition .......................... 120 9.1.2.2. minimal_with_hokuyo.launch....................................................... 120 código fuente puede ser estudiado, modificado y utilizado libremente con cualquier finalidad. Proporciona librerías y herramientas para el desarrollo de programas robóticos. • Turtlebot2 Robot personal de código abierto capaz de navegar por el espacio. Cuenta con una cámara 3D para ver el entorno. (Ilustración 1) Ilustración 1 Turtlebot2 [3] • Base “Kobuki” Base móvil del robot Turtlebot2. Cuenta con sensores de choque y detectores de precipicios izquierdos, delanteros y derechos, además de sensores de caída de rueda. • Rviz Herramienta de visualización de ROS que puede mostrar una gran variedad de información, como puede ser el mapa de la sala, la trayectoria que va a realizar el robot o la imagen que transmite la cámara a tiempo real. También es capaz de mostrar una representación del robot utilizado. • Gazebo Entorno de simulación de ROS (Ilustración 2). Permite simular comportamientos del robot como si fuera real. Es de gran utilidad a la Capítulo 1. Introducción. 5 hora de probar programas antes de meterlos en el robot real, evitando que pueda romperse debido a algún fallo de programa. Ilustración 2 Gazebo [4] 1.5. Estructura de la memoria La memoria se estructura en 8 capítulos: • Capítulo 1. Introducción Es el capítulo actual, sirve para poner un contexto sobre el proyecto y una justificación del mismo, además de sus objetivos y la presentación de los elementos más importantes. • Capítulo 2. Antecedentes. El objetivo de este capítulo es situar en un marco histórico las tecnologías que se van a utilizar en este proyecto. • Capítulo 3. Descripción del sistema. En este capítulo se presenta el robot Turtlebot2 con sus componentes y el entorno de desarrollo “Robot Operating System”, explicando sus conceptos y teoría. Además, se expone la puesta en marcha del robot. • Capítulo 4. Sistema de mapeo y navegación El fin de este capítulo es describir el problema a solucionar y explicar poco a poco el código en que se basa la solución a este asunto, para ayudar a su comprensión. • Capítulo 5. Puesta en marcha y resultados Se expone la puesta en marcha de los programas realizados para el proyecto y muestra un análisis de los resultados obtenidos. • Capítulo 6. Gestión del trabajo Informe de cómo se ha gestionado el tiempo a la hora de realizar el proyecto y el estudio económico de éste. • Capítulo 7. Conclusiones Conclusiones sobre el proyecto en general, sobre los objetivos a realizar y sobre posibles líneas de trabajo futuras. Para finalizar, se adjunta la bibliografía y anexos. 7 2. Capítulo 2. Antecedentes. El objetivo de este capítulo es situar en un marco histórico las tecnologías que se van a utilizar en este trabajo. 2.1. Robótica La robótica combina diversas disciplinas como la mecánica, electrónica, informática, matemáticas o física. Antes de existir la palabra robot, se denominaban autómatas a los artefactos construidos con el objetivo de descargar de trabajos tediosos o peligrosos a los humanos. El término ROBOT se utilizó por primera vez en una obra teatral denominada R.U.R. (Rossum's Universal Robots) del escritor checo Karel Capek en 1920. Robot proviene de la palabra checa “robota”, que significa “esclavo, trabajo forzoso, simplemente trabajar”. En ella, los robots eran máquinas que se parecían al hombre, las cuales trabajaban sin descanso, lo que conlleva a una revolución contra sus creadores. El científico y escritor de origen ruso Isaac Asimov, conocido por sus obras de ciencia ficción, escribió las “las tres leyes de la robótica” en 1942 y establecen lo siguiente: 1. Un robot no hará daño a un humano ni, por inacción, permitirá que un humano sufra daño. 2. Un robot debe obedecer cualquier orden dada por los humanos, excepto aquellas que entren en conflicto con la primera ley. 3. Un robot debe proteger su propia existencia, siempre y cuando esta protección no entre en conflicto con la primera y la segunda ley. En la última etapa de los años 40 aparecen los primeros robots móviles, Elmer y Elsie (Ilustración 3), ideados por el neurólogo americano William Grey Walter. Son consideradas como las primeras máquinas robóticas autónomas de la historia. Estas “tortugas robot” (denominados así por su parecido a una tortuga) podían moverse de manera autónoma en función de los focos de luz y esquivando obstáculos. Tenían dos motores, uno para avanzar y otro de giro. 8 Ilustración 3 Robot Elmer/Elsie. [5] En la década de los 50 aparece el primer robot industrial comercial, conocido como “Unimate” (Ilustración 4). Un manipulador esférico controlado por interruptores de fin de carrera y levas. Utiliza motores hidráulicos. Ilustración 4 Robot Unimate. [6] En la década de los 70 aparece el primer robot del mundo con accionamiento totalmente eléctrico, el robot IRB6 (Ilustración 5) de la empresa sueca ASEA (empresa de la cual acaba surgiendo ABB) Capítulo 2. Antecedentes. 9 Ilustración 5 Robot IRB6. [7] En esta época, aparecía el primer robot inteligente de la historia, “Shakey” (Ilustración 6), capaz de percibir su entorno, generar sus rutas y moverse por su cuenta. Podía crear un mapa del entorno y decidir la trayectoria más rápida desde donde se encontraba hasta donde tenía que llegar. Fue el primer paso hacia una robótica con sistemas integrados de inteligencia artificial. Ilustración 6 Robot Shakey. [8] En la actualidad, el desarrollo de la robótica ha conseguido que los robots tengan un comportamiento cada vez más inteligente, siendo capaces de integrarse con el entorno y tomar decisiones por sí mismos en tiempo real más rápido de lo que lo haría cualquier humano. 10 Podemos definir robot como una máquina automática programable capaz de realizar determinadas operaciones de manera autónoma y sustituir a los seres humanos en algunas tareas, en especial las pesadas, repetitivas o peligrosas; puede estar dotada de sensores, que le permiten adaptarse a nuevas situaciones. [9] Una manera de clasificar los robots es según su entorno de trabajo, donde se diferencia entre robots móviles y robots fijos. (Ilustración 7) Los robots fijos suelen ser industriales, ya que en su mayoría trabajan en una celda solos o cooperando con otros robots. En cambio, los robots móviles, como su propio nombre indica, pueden trasladarse de un lado a otro. Ilustración 7 Clasificación de los robots en función de su entorno de trabajo (Elaboración propia) Otra manera de clasificar los robots es según la función que van a realizar, en los que se distinguen dos grupos: industriales y de servicios. (Ilustración 8) Ilustración 8 Clasificación de los robots según su función (Elaboración propia) Capítulo 2. Antecedentes. 11 Los robots industriales son aquellos que están destinados a realizar trabajos relacionados con procesos industriales, suelen ser fijos. Los robots de servicio suelen ser móviles y ayudan al ser humano a realizar trabajos que son repetitivos y tediosos, como pueden ser las tareas domésticas. La Federación Internacional de la Robótica define estos robots como aquel robot que realiza tareas útiles para humanos, excluyendo aplicaciones de automatización industrial. Estos pueden dividirse en diferentes tipos, como doméstico o para investigación, medicina, seguridad, etc. [10] 12 2.2. Evolución de ROS (Robot Operating System) ROS (Robot Operating System) a pesar de su nombre, no es un sistema operativo, sino que es un marco de trabajo o “framework” utilizado para el desarrollo de aplicaciones robóticas. Suministra un conjunto de librerías y herramientas para el desarrollo de programas con los que podremos crear de una manera más fácil y rápida proyectos de robótica. Es un ‘kit’ para el desarrollo de software que ofrece bloques ya hechos necesarios para crear las aplicaciones robóticas. Es de código abierto, lo que quiere decir que el modelo de desarrollo de software está basado en la colaboración abierta. “Software libre”, cuyo código fuente puede ser estudiado, modificado y utilizado libremente con cualquier finalidad. Esta es otra de las ventajas de utilizar ROS en los proyectos, ya que ROS lleva existiendo más de 10 años y está siendo utilizado por millones de desarrolladores y usuarios, los cuales contribuyen a la mejora de este con sus proyectos. Esto hace que se pueda encontrar muchos trabajos de otros desarrolladores en Internet, pudiendo coincidir con lo que se requiera que haga tu robot, agilizando el proceso de pensar y escribir el programa. Permite desarrollar computación distribuida, es decir, los programas pueden utilizar recursos en varios nodos de cálculo distintos para conseguir un objetivo compartido común. Su historia comienza en 2007, cuando el Laboratorio de Inteligencia Artificial de Stanford desarrolla ROS con el nombre de ‘switchyard’ en un inicio, para dar soporte a los proyectos ‘STAIR’ (STanford Artificial Intelligence Robot) y ‘Personal Robotic Program’. A partir de aquí, se sigue desarrollando principalmente en Willow Garage, un laboratorio de investigación robótica dedicado a la creación de software de código abierto para robots personales. Además, diferentes investigadores ajenos a Willow Garage contribuyeron en este desarrollo. El 2 de marzo de 2010 se lanza la primera versión de ROS con el nombre Box Turtle (Ilustración 9), distribuyendo paquetes para uso público. Meses después saldría la segunda versión ROS ‘C Turtle’, como una gran actualización de las librerías ya lanzadas en ‘Box Turtle’. Ilustración 9 Logotipo “Box Turtle” Capítulo 2. Antecedentes. 13 En marzo de 2011 salió ROS ‘Diamondback’, que contendría nuevos paquetes, incluyendo soporte para la cámara Kinect. Con la siguiente versión sacada en agosto de ese mismo año (ROS ‘Electric EMys’) aumentaban el soporte hacía nuevas plataformas como Android y Arduino. Además, fue el año del lanzamiento de ROS Answers, foro de preguntas y respuestas de usuarios de ROS. Tras esto, cada año se iría sacando una nueva versión hasta 2018, incrementando considerablemente el número de usuarios que utilizan ROS. Los nombres de las versiones van en orden alfabético en función del orden de salida. Actualmente la última versión es ROS Noetic Ninjemys (Ilustración 10), siendo la versión final de los que se conoce como ROS1, con fecha de salida el 23 de Mayo de 2020 y con soporte hasta 2025. Ilustración 10 Logotipo "ROS Noetic Ninjemys" [11] En el año 2017 surge ROS2 como una mejora de ROS1, con el nombre ROS2 ‘Ardent Apalone’, ofreciendo, por ejemplo, el salto a Windows 10. A partir de aquí, año a año han ido saliendo nuevas versiones, siguiendo la norma del orden alfabético de ROS1. Actualmente la última distribución es ROS2 ‘Humble Hawksbill’ con fecha de salida el 23 de mayo de 2022 y con soporte hasta 2027. 20 Ilustración 16 Conceptos del nivel de grafo de procesos [16] Los conceptos más importantes de este nivel son los nodos, mensajes, topics, servicios, el nodo máster y los bags (Ilustración 16) 3.1.1.2.1. Nodos Son la unidad de procesamiento, cada nodo es un proceso que realiza una tarea. Lo que se nombraba anteriormente como procesos, en nomenclatura de ROS se denominan nodos. Es un ejecutable dentro de un paquete ROS, escrito con la biblioteca de C++ (roscpp) o de Python (rospy). Normalmente se utilizan varios nodos, estos se combinan dentro de un grafo compartiendo información (Grafo de procesos). Por lo general, cada nodo tiene una función específica dentro del objetivo común. Esto es mejor que tener un único nodo con muchas funciones, ya que puede permitir detectar errores más fácilmente y una menor carga computacional. Por ejemplo, a la hora de mover un robot, un nodo puede encargarse de planificar una ruta mientras otro se encarga de visualizar el entorno. 3.1.1.2.2. Máster El ROS Master permite la comunicación entre los nodos (Ilustración 17). Proporciona un registro de nombres que permite hacer una búsqueda dentro del grafo de computación. Sin el ROS Máster, los diferentes nodos del grafo no Capítulo 3. Descripción del sistema. 21 se podrían encontrar unos a otros, y, como consecuencia, no podrían intercambiar mensajes ni invocar servicios. Al ejecutar el ROS Máster, también se ejecuta el servidor de parámetros (Parameter Server), el cual utilizan los nodos para almacenar y recuperar parámetros en tiempo de ejecución. Además, se ejecuta el nodo “rosout”, el cual se comporta como la salida estándar. Estos tres nodos siempre van de la mano. Ilustración 17 Gráfico ROS Máster y nodos 3.1.1.2.3. Mensajes Los nodos se pueden comunicar entre ellos a través del uso de los mensajes. Estos mensajes son estructuras de datos con la información que se quiere comunicar. Admite los tipos estándar (integer, floating point, boolean, etc.) pero también se puede crear tipos personalizados. 3.1.1.2.4. Topics Los nodos se pueden comunicar a través de un patrón de interacción publicador-suscriptor mediante los mensajes (Ilustración 18). Para poder comunicarse, se crean los topics, que son canales de información entre nodos. Luego un nodo puede comunicarse con otro publicando mensajes en un topic al cual el otro nodo tiene que estar suscrito (suscriptor). Los nodos que quieran compartir una información en un determinado tipo de datos publicarán en un topic. 22 Los nodos que estén interesados en un determinado tipo de datos se suscribirán al topic correspondiente. Un topic puede tener diversos publicadores y suscriptores concurrentes y un nodo puede publicar y/o suscribirse a varios topics. Los publicadores y los suscriptores no son conscientes de la existencia de los demás. Ilustración 18 Comunicación en ROS [17] 3.1.1.2.5. Servicios Los nodos también se pueden comunicar a través de un patrón de interacción entre nodos cliente-servidor, apropiado para las interacciones de peticiónrespuesta. Utilizan dos mensajes, uno para petición y otro para respuesta. Un nodo ofrece un servicio y otro nodo lo utiliza enviando una petición y esperando una respuesta. 3.1.1.2.6. Bolsas Las bolsas (Bags) son un formato de almacenamiento de datos de ROS que permiten guardar datos enviados a través de mensajes, suscribiéndose al topic que se requiera y guardando los mensajes que se publiquen en un fichero. Estos ficheros también se pueden volver a reproducir en el mismo topic en el que fueron grabados. Son muy utilizadas para estudiar mensajes que publican los láseres, ya que a tiempo real publican mensajes Capítulo 3. Descripción del sistema. 23 3.1.1.3. Comunidad de ROS Una de las mayores ventajas de utilizar ROS es que es de código abierto, lo que quiere decir que el modelo de desarrollo de software está basado en la colaboración abierta. El nivel de comunidad de ROS se compone de recursos que permiten que diversos desarrolladores de cualquier parte del mundo puedan compartir sus proyectos y su conocimiento. Estos recursos incluyen: − Distribuciones: Colecciones de distintas versiones de pilas que se pueden descargar − Repositorios: ROS se basa en una red de repositorios de códigos, donde diferentes instituciones pueden desarrollar y lanzar sus propios componentes de software para robots. [14] − La enciclopedia de ROS: Principal foro (wiki) para la documentación de información sobre ROS. − Sistema de fallos de tickets: Sistema para indicar fallos en ROS. − Listas de correo: Es el principal canal de comunicación sobre las nuevas actualizaciones de ROS. − ROS Answers: Un foro de preguntas y respuestas donde los distintos usuarios de ROS exponen dudas y responden otras. 3.1.2. Herramientas y comandos A continuación, se van a exponer las herramientas y comandos con los que cuenta ROS y van a ser importantes en la realización de este proyecto. 3.1.2.1. RVIZ RVIZ proviene de la abreviatura de ROS Visualization, y es una herramienta gráfica que puede mostrar una gran variedad de información del sistema (Ilustración 19). Con esta herramienta se puede visualizar la imagen que transmite la cámara 3D, ver la lectura de los sensores como el láser, el mapa 2D que obtiene el robot, la trayectoria que va a seguir dentro de este mapa, entre otras cosas. Además, es capaz de mostrar una representación del robot utilizado, pudiendo modelar cualquier robot en formato URDF. Además de permitir visualizar gran información del sistema, también permite publicar mensajes sobre diversos topics. 24 Ilustración 19 RVIZ 3.1.2.2. Simulador Gazebo Gazebo es una herramienta de simulación que tiene un motor de físicas muy robusto, gráficos de gran calidad y una buena interfaz gráfica (ilustración 20). Permite la simulación de programas de control en un entorno 3D. Para esto lo principal es tener un fichero mundo “.world”, que contenga los elementos de la simulación (robots, sensores, luces, objetos). Estos mundos pueden ser creados en la propia herramienta, con gran variedad de objetos ya creados. Es una aplicación sencilla, en pocos minutos se es capaz de realizar un mundo que imita una habitación. El otro concepto importante son los ficheros de modelo de los objetos y robots de la simulación. Con el robot Turtlebot2 no se va a tener ningún problema a la hora de implantar el modelo del robot, ya que es un robot muy común y la comunidad de ROS provee de este modelo bien realizado. Gazebo se sincroniza con ROS de forma que el robot simulado puede publicar y recibir información de los diferentes topics al igual que si fuera un robot real. Por lo tanto, es una buena herramienta a la hora de realizar pruebas de programas de manera realista, ya que se evita utilizar el robot real, el cual puede romperse. Capítulo 3. Descripción del sistema. 25 Ilustración 20 Simulación del robot Turtlebot en una habitación en Gazebo [18] 3.1.2.3. Comandos útiles En este apartado se exponen diversos comandos de ROS que han sido útiles para la realización de este proyecto: − roscore: inicia el máster, parameter server y rosout. − rospack list: muestra paquetes instalados en ROS − rospack find nombre-paquete: busca la ruta de un paquete. − rosrun nombre-paquete nombre-ejecutable: ejecuta nodo de un paquete. Se puede pasar el valor de algún parámetro − rosnode list: muestra nodos ejecutándose en ese instante − rosnode info nombre-nodo: muestra información sobre el nodo (publicaciones, suscripciones, servicios…) − rqt_graph: muestra el grafo de comunicación (Ilustración 21). Aquí los óvalos son los nodos y las etiquetas de las flechas son los topics. Las flechas representan las relaciones de publicación-suscripción. 26 Ilustración 21 Grafo de comunicación rqt_graph − rostopic list: muestra topics activos. − rostopic echo nombre-topic: muestra los mensajes que se están publicando. − rostopic hz nombre-topic: velocidad a la que se publican los mensajes. − rostopic bw nombre-topic: ancho de banda consumido. − rostopic info nombre-topic: información del topic (tipo de mensaje, publicadores, suscriptores) − rosmsg show nombre-tipo-mensaje: detalles del tipo del mensaje. − rostopic pub -r rate-in-hz nombre-topic tipo-mensaje contenido-mensaje: publica mensaje en el topic determinado a una velocidad determinada. − roslaunch nombre-paquete fichero.launch: Lanza el fichero.launch determinado. Si se escribe este comando sin haber utilizado roscore antes, inicia el máster, parameter server y rosout. Capítulo 3. Descripción del sistema. 27 3.2. TurtleBot2 Tras la explicación del software, se va a describir el hardware utilizado. El robot que se ha empleado es la versión 2 del robot TurtleBot (Ilustración 22), al cual se le han añadido componentes para ampliar su rango de posibles actividades y mejorarle, como son un brazo robótico “PhantomX Reactor” y un telémetro láser (sensor LiDAR 1 ). Como se ha comentado en el apartado “2.3. La familia TurtleBot”, el robot Turtlebot es un robot de bajo precio y código abierto, lo cual hace que sea muy utilizado para la educación y la investigación. Ilustración 22 Robot TurtleBot2 utilizado Este robot se compone de una base móvil “Kobuki”, un miniordenador de controlador “Intel NUC 7I5BNK” y una cámara 3D “Orbbec Astra”, el cual se ha complementado con un telémetro láser de escaneo “Hokuyo URG-04LX” y un brazo robótico “PhantomX Reactor”. 1 Light Imagine Detection and Ranging 28 3.2.1. Base móvil La base móvil del robot es denominada “iClebo Kobuki”. Esta base de la empresa “Yujin Robot” (Ilustración 23) es de bajo precio y diseñada para la educación e investigación. Ilustración 23 Base móvil "iClebo Kobuki" [19] “iClebo Kobuki” cuenta con cuatro ruedas, dos de ellas en configuración diferencial (pueden girar a diferentes velocidades para girar), situadas en paralelo a la izquierda y derecha de la base, y otras dos como puntos de apoyo (Ilustración 24). Además, tiene diferentes sensores para evitar accidentes, tales como 3 sensores de presión colocados en la parte derecha, parte izquierda y parte frontal de la base; 2 sensores de detección de caída de rueda izquierda y derecha; y 3 sensores de detección de precipicio colocados también a la izquierda, derecha y parte frontal. Para conseguir un sistema de navegación preciso, cuenta con sensores para la odometría (encoders en las ruedas muy precisos, se hablará sobre ella en el apartado “3.3. Pila de Navegación de ROS”) y un giroscopio calibrado de fábrica, lo que permite reconocer fácilmente dónde se encuentra el robot según sus movimientos. La base está diseñada para que corra en ROS, con lo que todos estos sensores publican mensajes en determinados topics con sus respectivos estados. Capítulo 3. Descripción del sistema. 29 Ilustración 24 Parte inferior de la base Además, provee de tres fuentes de alimentación (una de 5V y dos de 12V), luces LED programables, tres botones y audio. Depende siempre de un ordenador de control, sin este no podría moverse, así que cuenta con una fuente para darle alimentación de 19V y un USB para su conexión. Posee una estación de carga a la cual puede llegar automáticamente si se encuentra cerca gracias a sensores infrarrojos En la guía de usuario de su página oficial, se pueden encontrar las especificaciones funcionales y del hardware. [20] Especificaciones funcionales • Velocidad lineal maxima de 0.7 m/s • Velocidad angular máxima de 180 grados/s • Capacidad de carga de 5kg en suelo duro, y de 4 Kg en alfombra. • Detección de precipicios, no andará si hay una caída mayor de 5cm. • Capaz de superar desniveles de 12 mm máximo (por ejemplo, subirse a una alfombra). • Tiempo de operación de hasta 3/7 horas (batería pequeña/grande). • Tiempo de carga de 1.5/2.6 horas (batería pequeña/grande). • Capacidad de acoplarse a la base de carga por sí solo en un área de 2mx5m frente a la base. 36 3.3.1. Odometría (odometry source) La odometría es el estudio de la estimación de la posición de vehículos con ruedas durante la navegación. Esta estimación se basa en la rotación de las ruedas para calcular los cambios de posición a lo largo del tiempo (ilustración 30). Ilustración 30 Modelo de sistema de odometría de un robot [27] Para la generación de la odometría, el robot Turtlebot2 posee dos ruedas en configuración diferencial, donde sus respectivos encoders proporcionan la información con la posición de cada rueda. El problema de la odometría es que no siempre se cumple ya que, por ejemplo, no tiene en cuenta el deslizamiento en las ruedas. Por esto, la estimación de la localización del robot se complementa con el algoritmo de localización adaptativo de Monte Carlo. 3.3.2. Algoritmo de localización adaptativo de Monte Carlo (AMCL) El algoritmo de localización de Monte Carlo se basa en un filtro de partículas para que los robots puedan localizarse dentro de un entorno dado. Este filtro de partículas representa la distribución de estados posibles, es decir, cada partícula representa una hipótesis de dónde se encuentra el robot. Cada vez que el robot se mueve o detecta algo, las partículas se desplazan intentando predecir su nuevo estado, viendo si es coherente el estado que han previsto con el real obtenido por los sensores. Al final, las partículas deberían converger hacia la posición real del robot. Para esto, trabaja en paralelo con la odometría. Tras el movimiento del robot, el algoritmo coge la información de los sensores y compara con qué posición del robot dentro del mapa se ajusta más, dando así una mejora en la localización del robot y una mayor precisión a lo largo del Capítulo 3. Descripción del sistema. 37 tiempo, evitando los errores que existirían si la única fuente de la localización del robot fuera la odometría. 3.3.3. Transformadas de los sensores Debido a la utilización de la odometría y de la información recibida de los sensores, es necesario que el sistema sepa en que posición en el cuerpo del robot se encuentran las ruedas o los láseres. Para ello se utilizan diferentes sistemas de referencia para cada elemento. Se establece un sistema de referencia “padre” (suele ser el centro de la base), y a partir de él se establecen los demás. Para definir la relación entre el sistema de referencia “padre” con cualquier otro se utilizan las transformadas. 3.3.4. Funcionamiento de la pila Tras haber explicado las bases para la localización de un robot dentro de un mapa, podemos explicar la generación de trayectorias que ofrece la pila explicando varios conceptos importantes: • Local_costmap • Global_costmap • Local_planner • Global_planner • Recovery_behaviors 3.3.4.1. Mapas de costo Para la generación de las trayectorias, la pila utiliza dos mapas. Con el mapa que se le suministra al paquete (fichero .yaml), el sistema crea un mapa de costo denominado “mapa global” (global_costmap), utilizado para la posición general. Además, crea otro mapa de costo llamado “mapa local” (local_costmap) que supone una reconstrucción del mapa a corta distancia, generado por la lectura de los sensores, posicionando obstáculos si los hay. A medida que se encuentran nuevos obstáculos, el mapa de costo global (global_costmap) es actualizado por el sistema con estos obstáculos, pero no reescribe el fichero .yaml de dónde se obtiene este mapa. Esto hace que obstáculos imprevistos puedan ser borrados sin necesidad de cambiar el mapa. Por ejemplo, si el mapa está guardado con las puertas abiertas de una sala y de repente se encuentra el obstáculo de que una puerta está cerrada, 38 esto haría que el sistema actualizase el mapa de costo global añadiendo un obstáculo ahí, pero no modificaría el mapa. Además, al abrir la puerta este obstáculo se borraría por sí solo. 3.3.4.2. Planificadores El sistema de navegación va a estar pendiente todo el rato al topic “move_base_simple/goal”, donde se publicarán las coordenadas del punto final al que se quiere llegar dentro del mapa. Cuando se recibe un mensaje en este topic, el sistema crea una primera trayectoria global a seguir a largo plazo con los datos recibidos del mapa de costo global (global_planner). Para poder sortear obstáculos que no están contemplados en el mapa de costo global, se toman los datos del mapa de costo local y, con ellos, el sistema intenta encontrar una trayectoria a seguir de manera inmediata (trayectoria local) para sortear el obstáculo (local_planner, encargado de publicar los comandos de velocidad). Para que el robot se mueva, es necesario tener ambas trayectorias. 3.3.4.3. Recovery Behaviors El sistema cuenta con mecanismos de recuperación si el robot no consigue llegar a la meta establecida. Ilustración 31 Recovery Behaviors [28] Si no se encuentra trayectoria posible debido a que hay obstáculos de por medio, el robot se para en una posición. En esa posición, el robot resetea los mapas de costo a partir de un radio dado desde el centro del robot (3 metros por defecto, pero se puede cambiar en el parámetro “conservative_reset”), borrando los obstáculos que pueda haber fuera de ese radio. Si sigue atrapado, Capítulo 3. Descripción del sistema. 39 da una vuelta de 360º actualizando los mapas de costo con lo que vea, comprobando si tiene salida por algún otro lado. Si no la encuentra, reinicia los mapas de costo de manera más agresiva, borrando todos los obstáculos en un radio menor que el anterior (1,84 metros si no se ha cambiado el parámetro “aggressive_reset”), y, si con esto sigue igual, da otra vuelta de 360º. Tras esta, si no encuentra ninguna salida, el robot aborta el plan debido a que no se puede llegar a la meta. En la ilustración 31 se puede observar un gráfico con los diferentes estados del mecanismo de recuperación. 3.3.4.4. Parámetros Para optimizar la navegación autónoma, los diferentes mapas y planificadores tienen diversos parámetros que se pueden editar en función de lo que se requiera. Como parámetros importantes se pueden encontrar la distancia mínima que puede haber entre el robot y los diferentes obstáculos (inflation_radius), la dimensión del mapa local o los límites de velocidad y aceleración del robot. 3.4. Puesta en marcha del robot En este apartado se describen los pasos a realizar para la puesta en marcha del robot Turtlebot. Cómo establecer la comunicación entre el PC controlador del robot con otro ordenador y cómo hacer que el robot empiece a funcionar. Además, se describirán paquetes básicos que se van a utilizar. 3.4.1. Comunicación entre ordenadores Al no poder tener una pantalla conectada al mini ordenador controlador del robot mientras está en movimiento, se necesita que éste se pueda manejar desde otro ordenador. Para ello se utiliza el protocolo SSH (Secure Shell), el cual utiliza una arquitectura cliente/servidor y permite al usuario conectarse a un host remotamente por medio de un canal seguro en el que toda la información está cifrada, siempre y cuando esté Para ello, primeramente, hay que tener instalado el servidor SSH en el ordenador de la Turlebot. Tras esto, ya se puede acceder vía SSH al ordenador. Para ello en la terminal se utilizará el siguiente comando: > ssh turtle@<TURTLEBOTP_IP> 40 Donde “turtle” es el nombre de usuario al que se quiere entrar y “<TURTLEBOTP_IP>” es la dirección IP de la Turtlebot. Tras ponerlo, pedirá la contraseña y se abrirá la terminal del PC del robot. No se tiene acceso a la visualización del escritorio, sino solo a su consola. Por cada terminal que se requiera, se necesitará ejecutar el comando SSH en otra terminal del ordenador fijo. 3.4.2. Conectar ROS Máster Tras conseguir utilizar el ordenador de la Turtlebot de manera remota, ahora lo importante es conseguir que los nodos lanzados desde un ordenador u otro se puedan comunicar entre ellos. Para esto se necesita que ambos ordenadores puedan encontrar el ROS Máster y que sea el mismo. Para ello, hay que utilizar los siguientes comandos. En el ordenador de la Turtlebot se exportan las variables ROS_MASTER_URI Y ROS_HOSTNAME con los siguientes valores: export ROS_MASTER_URI=http://localhost:11311 export ROS_HOSTNAME=IP_OF_TURTLEBOT Donde localhost e IP_OF_TURTLEBOT son la dirección IP del ordenador de la Turtlebot. Mientras que en el ordenador fijo hay que exportar estas variables con los siguientes valores: export ROS_MASTER_URI=http://IP_OF_TURTLEBOT:11311 export ROS_HOSTNAME=IP_OF_PC Donde IP_OF_TURTLEBOT es la dirección IP del ordenador de la Turtlebot e IP_OF_PC es la dirección IP del ordenador fijo. Estos comandos habría que escribirlos cada vez que se abra una nueva terminal. Para evitar esto, se escribirá en el /.bashrc de cada ordenador, sus respectivos comandos. Esto se debe a que el archivo /.bashrc es un script que se ejecuta cada vez que se inicia una nueva terminal, ejecutando todos los comandos que tenga en su interior. Luego si se le añaden estos comandos, se ejecutarán nada más abrir la terminal, evitando que se requiera ponerlos cada vez que necesitamos una nueva terminal. Tras esto, para comprobar que ambos ordenadores están conectados al mismo ROS Máster, se lanza roscore y se mira la lista de topics de ambos ordenadores, donde deberían aparecer los mismos topics en los dos. Capítulo 3. Descripción del sistema. 41 3.4.3. Espacio de trabajo Lo más cómodo a la hora de trabajar con ROS es crear un espacio de trabajo, es decir, un directorio donde vayan a estar guardados todos los paquetes que se hayan realizado para el proyecto. ROS guarda por defecto los paquetes instalados en la ruta /opt/ros/kinetic/. Se podrían guardar aquí los paquetes realizados, pero para modificar documentos de este directorio son necesarios privilegios de súper usuario, además de que queda tener directorio propio para los paquetes del proyecto hace que esté mejor organizado. ROS posee una herramienta denominada catkin, la cual se utiliza para construir paquetes. Para poder utilizarlo es recomendado tener un espacio de trabajo. Esta herramienta permite crear paquetes de manera más cómoda que haciendo todo a mano, creando los ficheros necesarios con las dependencias que requiera el paquete. En el proyecto se ha creado un espacio de trabajo llamado “catkin_ws”, en él se crea una carpeta denominada “src” donde se guardan los paquetes creados y/o copiados de la comunidad de ROS para el proyecto. Siempre que se vayan a utilizar nodos de algún directorio, hay que compilar el archivo de configuración del entorno. Para ello, cada vez que se utilice una nueva terminal, se escribirán los siguientes comandos: $ source /opt/ros/%YOUR_ROS_DISTRO%/setup.bash $ source /root/catkin_ws/devel/setup.bash Siendo %YOUR_ROS_DISTRO% el nombre de la distribución instalada de ROS (por ejemplo, kinetic), ya que en esta ruta se guardan todos los nodos que han sido instalados. El segundo comando compila los archivos de configuración del espacio de trabajo. Para no tener que escribir estos comandos cada vez que se inicie una nueva terminal, se pueden escribir estos comandos en el archivo /.bashrc, como en el caso anterior. 3.4.4. Preparar Turtlebot Una vez instalados todos los paquetes necesarios, en especial de la pila “Turtlebot”; habiendo realizado la comunicación entre ambos ordenadores y compilado los archivos de configuración del entorno, es posible comenzar a manejar el robot. 42 Para el inicio del Robot, existe un paquete denominado “Turtlebot_bringup”. Este paquete contiene un archivo Launch que se llama “minimal.launch” y se encarga de ejecutar los nodos básicos. Uno de estos nodos básicos es el “kobuki_node”, encargado de realizar la comunicación entre la base móvil y ROS. Este archivo Launch se ha modificado de cara al proyecto, creando uno nuevo, para que también ejecute el nodo que permita utilizar el sensor LiDAR “Hokuyo” en lugar de la cámara para el escaneo del entorno, debido a sus ventajas con respecto a esta (“Apartado 3.2.3. Cámara 3D”). El sensor comienza a publicar la información que obtiene del entorno en el topic /scan. Además, se modifican los valores de los parámetros que ajustan el campo de visión debido a que el sensor LiDAR tiene un mayor campo de visión que la cámara (Ilustración 32) Ilustración 32 minimal_with_hokuyo.launch Para poder ejecutar este Launch se escribirá el siguiente comando: $ roslaunch turtlebot_bringup minimal_with_hokuyo.launch Siendo “minimal_with_hokuyo.launch” el archivo Launch creado para el uso del sensor LiDAR. 3.4.5. Teleoperación por teclado El movimiento del robot puede ser controlado por teclado, ya sea vía SSH o desde el PC fijo (si la comunicación se ha configurado bien), siempre y cuando se haya arrancado la Turtlebot como se ha explicado en el apartado anterior. Capítulo 3. Descripción del sistema. 43 Para ello, la pila “Turtlebot” contiene un paquete denominado “turtlebot_teleop” con el script “turtlebot_teleop_key”, que puede ser ejecutado como nodo con el siguiente comando: $ roslaunch turtlebot_teleop keyboard_teleop.launch Siendo este archivo Launch el encargado de ejecutar el nodo. La interfaz de usuario se vería como en la ilustración 33. Ilustración 33 Interfaz de teleoperación por teclado del robot 1 El paquete “kobuki_keyop” contiene otro script encargado de la teleoperación por teclado. En este los movimientos en función de las teclas son diferentes. Se ejecuta con el comando: $ roslaunch kobuki_keyop keyop.launch En este nodo, el robot se controla por incrementos de velocidad, incrementando la velocidad lineal o angular en función de la entrada del teclado (ilustración 34). Ilustración 34 Interfaz de teleoperación por teclado del robot 2 44 El usuario puede elegir cualquiera de estos para mover el robot. 3.4.6. Verificar el funcionamiento de la cámara Para utilizar la cámara si se requiere ver a través de ella, el paquete “turtlebot_bringup” tiene un archivo Launch llamado “3dsensor.launch” que permite a la cámara publicar en topics destinados a la visualización de la imagen de esta. Para ello, hay que pasarle el tipo de cámara que se está utilizando, en el caso de este proyecto es una astra: $ roslaunch turtlebot_bringup 3dsensor.launch 3dsensor:=astra El paquete “image_view” contiene un nodo con el mismo nombre que permite visualizar lo que está viendo la cámara traduciendo los datos que recibe en el topic indicado. El topic /camera/rgb/image_raw recibe la imagen de la cámara a color, luego para poder visualizarlo se tiene que escribir el siguiente comando: $ rosrun image_view image_view image:=/camera/rgb/image_raw Este comando hay que escribirlo en el ordenador fijo, ya que la comunicación SSH solo permite visualizar la terminal y no sería capaz de abrir un display con la imagen. Otro método para visualizar la imagen que recibe de la cámara es utilizar RViz. Para ello se ejecutaría RViz en el ordenador fijo. Tras esto, se cambiaría la opción global Fixed Frame a camera_rgb_optical_frame, ya que solo se va a utilizar la cámara, no importa lo demás. Después, se añade un display tipo “camera” y en “Image topic” se añade el topic que se requiere visualizar, en este caso /camera/rgb/image_raw. El resultado sería una ventana donde se puede visualizar la imagen que manda la cámara a tiempo real (ilustración 35). Capítulo 3. Descripción del sistema. 45 Ilustración 35 Visualización de la imagen que manda la cámara en RViz Para poder realizar todo esto se tiene que haber arrancado la Turtlebot antes (Apartado “3.4.4. Preparar Turtlebot”). 3.4.7. Construir un mapa (SLAM) El paquete “turtlebot_navigation” proporciona las herramientas necesarias para crear un mapa a partir de los datos recibidos del entorno por medio de los sensores y la odometría. Un algoritmo SLAM (del inglés, “Simultaneous Localization And Mapping”) es aquel que consigue que el robot construya un mapa del entorno y que se localice mientras navega en él de manera simultánea. Esto se consigue gracias al archivo Launch del paquete “turtlebot_navigation” llamado “gmapping_demo.launch”, el cual ejecuta un algoritmo SLAM. Este algoritmo SLAM ejecutado es el que proporciona el paquete “gmapping” proporcionado por ROS, denominado “slam_gmapping”. 52 53 4. Capítulo 4. Sistema de mapeo y navegación. En este capítulo se expone el sistema de navegación que requería este proyecto, junto con la explicación del código. Todo el código está escrito en lenguaje Python, bajo la versión 2.7.11. 4.1. Mapeo automático. 4.1.1. Descripción del problema En primer lugar, se ha querido realizar un programa que consiguiera mapear el entorno sin necesidad de que el usuario estuviera controlando el robot. Para una primera aproximación, se ha realizado un programa que, utilizando los sensores del parachoques, consiga que el robot pueda moverse por toda la habitación mapeándola sin quedarse atrapado. Esto se realizó pensando en un futuro realizar un programa con el que, en vez de llegar a chocar con las paredes, el robot pudiera reconocer que hay una pared cerca y girar antes, gracias a la lectura de sus sensores. 4.1.2. Diagrama de flujo del programa Este programa, escrito en lenguaje python, se llama “bumper.py”, se adjunta el diagrama de flujo para comenzar la explicación (Ilustración 41). 54 Ilustración 41 Diagrama de flujo del programa "bumper.py" Este es un programa sencillo, para comenzar a tantear el manejo del robot mediante código. El robot irá siempre hacia delante hasta que choque contra alguna pared. Si el choque proviene por la izquierda, el sensor del parachoques publicará en el topic /mobile_base/events/bumper un mensaje del tipo BumperEvent (Ilustración 42). Dentro de este mensaje, el valor de la variable state indica si el parachoques está presionado o no y la variable bumper nos indica el lado por el que se ha chocado. Entonces, si se choca por la derecha, para poder retomar el camino, el robot tendrá que dar marcha atrás y rotar en sentido antihorario un ángulo pequeño, ya que poco a poco conseguirá despegarse del obstáculo con el que está chocando. Si se choca por la izquierda haría exactamente lo mismo, cambio el sentido de la rotación al de las agujas del reloj. Si se choca de frente, necesitamos un ángulo de giro más grande para sortear este obstáculo, luego se ha impuesto que dé marcha atrás y rote en sentido horario un ángulo más grande (de casi unos 90º). Capítulo 4. Sistema de mapeo y navegación. 55 Además, notificará por pantalla cada vez que haya una colisión y por dónde. Ilustración 42 kobuki_msgs/BumperEvent.msg 4.1.3. Explicación del código El código entero se adjunta en los anexos (“9.1. Código”), en este apartado se explicará poco a poco este código. Ilustración 43 Constructor de objetos de la clase bumpernode() del programa bumper.py En la ilustración 43 se puede observar la función constructor de los objetos de la clase bumpernode(). 56 La función rospy.on_shutdown(self.shutdown) se utiliza para llamar a una función (en este caso la función “shutdown”) cuando se termina el programa por medio de recibir un Ctl+C en la terminal. Tras esto, se crea un subscriptor que se subscribe al topic ‘/mobile_base/events/bumper’ y se le avisa que los mensajes serán del tipo BumperEvent, ya que, como hemos explicado antes, los sensores del parachoques publicarán aquí. Además, self.callback() es la función a la que se llamará cada vez que aparezca un mensaje en este topic. Por último, se crea un publicador, indicándole que va a publicar mensajes del tipo ‘Twist’ en el topic ‘cmd_vel_mux/input/navi’ y se limita el número de mensajes que puede haber en cola. El robot se mueve en función del contenido de los mensajes que se publiquen este topic, siempre y cuando se haya realizado el arranque de la Turlebot con el Launch minimal o minimal_with_hokuyo del paquete Turtlebot_bringup, el cual crea este topic. Ilustración 44 Función callback() del programa bumper.py En la ilustración 44 se muestra la función que es llamada cada vez que se recibe un mensaje en el topic ‘/mobile_base/events/bumper’ de la que se ha hablado antes. Si la variable state recibida de este mensaje tiene el valor 1, significa que hay colisión, entonces se cambia la variable colision a 1 y se guarda el instante de tiempo en el que se produce esta colisión (variables que se utilizarán después). Tras esto, se comprueba el valor de la variable bumper para ver de dónde viene el impacto. En función del lado de la colisión, se cambia el valor de la variable angulo, la cual será utilizada para determinar el ángulo de giro del robot. Capítulo 4. Sistema de mapeo y navegación. 57 Ilustración 45 Función navegar() del programa bumper.py La ilustración 45 muestra el código de la función navegar() de este programa. Esta función es la encargada de hacer que el robot se mueva. Las variables “move_cmd” y “colision_cmd” son mensajes del tipo Twist. Este tipo de mensaje suele ser encargado del control de la velocidad del robot, ya que tiene dos vectores de tres elementos tipo float cada uno. Un vector destinado a la velocidad lineal (desplazarse) y otro para la angular (girar). El contenido de estos vectores son las coordenadas x, y, z. Entonces, se ha definido que la variable “move_cmd” sea la encargada de mover el robot hacia delante, mientras que la variable “colision_cmd” se encarga de dar marcha atrás y girar un número de grados en función del tipo de choque. Si no ha detectado colisión (variable colision igual a 0), se publica el contenido de la variable “move_cmd” en el topic ‘cmd_vel_mux/input/navi’ del que se ha hablado antes. Esto hace que la Turtlebot vaya hacia delante, ya que el valor de este mensaje es 0.2 en la x de la velocidad lineal y en el resto es 0. Si se detecta colisión o ha sido hace poco (variable colision igual a 1), se publica el contenido de la variable “colision_cmd”, el cual será -0.1 en la x de la velocidad lineal y un valor en la z de la velocidad angular que depende del lado por el que se ha producido la colisión. Esto hace que vaya para atrás y gire. Cuando se produce una colisión se guarda el momento en el que se produce en la variable “actual” como se ha explicado antes. Así el programa comprueba cuanto tiempo ha pasado desde el momento del impacto hasta la situación actual con la diferencia entre este valor y el valor del instante actual. Si son más de dos segundos, se reinicia esta variable a 0 y se pone la variable “colision” a 0, la cual se había cambiado a 1 al recibir el impacto, ya que se supone que ya no habrá colisión. Esto se hace para que se publique el contenido de la variable “colision_cmd” durante 2 segundos, consiguiendo que 58 el robot retroceda y rote durante 2 segundos antes de comenzar a ir hacia delante otra vez. Ilustración 46 Función shutdown() del programa bumper.py La función shutdown(), mostrada en la ilustración 46, es llamada cuando se recibe CTL+C en la terminal, es decir, cuando se requiera terminar el programa. Se muestra un mensaje por pantalla diciendo que la Turtlebot está parando y se publica en el topic cmd_vel_mux/input/navi’ el mensaje Twist(), el cual es un mensaje tipo Twist con todas sus variables a 0, lo que hace que se pare el robot. Tras esto se esperan 5 segundos para salir, para que dé tiempo a que el robot se detenga. Ilustración 47 Programa bumper.py En la ilustración 47 se muestra lo que realiza el programa bumper.py al ser ejecutado en una terminal. Si no hay ningún error (excepciones), con la función rospy.init_node(‘bumpernode’, anonymous=True), se inicia un nodo llamado bumpernode. Si no se sabe si ese nombre lo utiliza otro nodo, se pone la variable anonymous = True. Esto hace que se añada un número al azar detrás del nombre del nodo al crearlo. Tras esto, se crea un objeto de la clase bumpernode() llamado explorer y, mientras no se cierre el programa, se va a estar ejecutando en bucle la función navegar(). Al iniciar en una terminal el mapeo con el paquete turtlebot_navigation que se ha explicado antes, y en otra terminal este programa, se conseguiría un mapeo Capítulo 4. Sistema de mapeo y navegación. 59 de la habitación automático, ya que el usuario puede mapear la habitación con la Turtlebot sin necesidad de controlarla. El usuario tendría que guardar el mapa cuando vea que ha conseguido el mapa objetivo que quería. 4.1.4. Mejoras para el programa bumper.py El mayor problema que tiene este programa son las esquinas, ya que el robot puede quedarse atrapado en ellas al entrar en un bucle chocando primero con la pared derecha, luego con la pared izquierda y luego otra vez con la pared derecha (Ilustración 48). El siguiente problema que tendría este programa, es que el robot puede pasar por el mismo sitio muchas veces, ya que el recorrido que hace es aleatorio. Por esto, el proceso de mapear se puede alargar. Ilustración 48 Robot atascado en una esquina Para el problema de las esquinas, se planteó una mejora del programa añadiendo un contador de choques recientes. Se adjunta el diagrama de flujo del programa diseñado en la ilustración 49. En él podemos ver el funcionamiento de este programa. Si hay un choque, el sistema comprueba si ha habido otro choque recientemente con un temporizador. Si no lo ha habido, el programa haría lo mismo que el anterior y luego reiniciaría el temporizador (para calcular el tiempo hasta el siguiente choque) y pondría el contador de choques a 1. Si hay un choque y ha ocurrido otro recientemente, el sistema comprobaría si han ocurrido 5 choques recientes entre sí, y, si no es así, haría lo mismo que 60 en el programa anterior añadiendo el reinicio del temporizador e incrementando el contador de choques en 1. Si hay un choque y el programa detecta que ha habido 5 choques recientes, el sistema mandaría al robot una acción aleatoria de evasión, al darse cuenta de que se encuentra encajado. Esta sería dar marcha atrás y girar un número de grados aleatorio. Tras esto, se reiniciaría el temporizador y el contador de choques se pondría a 0. Ilustración 49 Diagrama de flujo del programa "bumper.py" mejorado Con este programa se conseguía evitar el problema de las esquinas, pero no se llegó a implantar, ya que se encontró un paquete realizado por la comunidad con el que el robot es capaz de mapear el entorno de manera automática, evitando pasar por los lugares que ya ha pasado y con capacidad para no chocarse con las cosas, que era justo lo que se requería. El paquete en cuestión tiene el nombre de “explore_lite”. 4.1.5. Paquete “explore_lite” Este paquete realizado por Jiri Horner, permite al robot explorar el entorno sin necesidad de que el usuario esté manejando el robot. Además, evita los espacios cerrados que ya ha mapeado. [30] Capítulo 4. Sistema de mapeo y navegación. 61 Cuando el nodo se está ejecutando, el robot va a explorar el entorno buscando las fronteras desconocidas del mapa a las que pueda llegar. Cuando no haya más fronteras, se para. Este paquete utiliza el nodo “move_base” para navegar, luego es imprescindible a la hora de utilizar explore_lite. Para poder explorar, el paquete se subscribe al mapa de costo publicado por el nodo “move_base” o al mapa generado por algún algoritmo de mapeo (SLAM) y con la información obtenida construye un mapa en el que busca las fronteras (Ilustración 50). Dependiendo del entorno se obtienen mejores resultados si se usa el mapa del algoritmo de mapeo o el mapa de costo publicado por “move_base”. Ilustración 50 Grafo del paquete "explore_lite" [30] Una de las ventajas que tiene utilizar el mapa de costo publicado por “move_base” es que es capaz de llegar a fronteras muy pequeñas. Lo que hace este paquete es, una vez se ha lanzado el nodo “move_base”, comienza a buscar fronteras en el mapa. Al encontrarlas, envía un punto de destino cercano para que el robot intente llegar a él trazando una trayectoria. Si no consigue llegar, vuelve a buscar fronteras. Una de las ventajas de este paquete, es que también se puede navegar de forma manual a través de Rviz con “move_base”, publicando el punto al que se quiere llegar. Cuando el mapa obtenido sea el que se requiere, el usuario tendrá que guardar el mapa. Si el programa ha funcionado bien y el robot ha sido capaz de llegar a todos los rincones del entorno, el mapa estará completo cuando no detecte más fronteras. El problema de este paquete es que, en espacios pequeños, el robot detecta colisiones cuando no las tiene. Esto se debe al parámetro “inflation_radius”, el cual es la distancia mínima que puede haber entre el robot y los diferentes obstáculos. Luego para conseguir que funcione correctamente, habrá que minimizar el valor de este parámetro. Además, tiene algún fallo, ya que a veces no detecta fronteras cuando sí que las hay. Esto haría que el programa dejase de funcionar porque se pensaría que ya ha terminado, luego lo único que habría que hacer es reiniciarlo. Lo 68 4.2.3. Librería “Schedule” Tras haber conseguido escribir el horario, ahora es necesario estudiar cómo se seguirá. Para realizar acciones a hora establecidas se ha utilizado la librería “Schedule” de Python. Esta librería es capaz de ejecutar funciones del programa a horas establecidas. Puede ejecutar funciones cada cierto tiempo de manera periódica, realizar una función cierto día a cierta hora todas las semanas, o marcar plazos para realizar una función (si se pasa el plazo no la realizaría). Esta librería es perfecta para este proyecto, ya que se ha definido una función exclusiva para que el sistema planee e intente realizar una trayectoria a un punto dado. Guarda en un objeto la planificación establecida para realizar las diversas funciones y funciona como interrupciones del sistema. Cuando se requiere hacer la tarea, parará cualquier cosa que esté haciendo el programa para realizarla (siempre y cuando el planificador esté activo). 4.2.4. Sistema de navegación El robot tiene que ser capaz de seguir llegar a posiciones establecidas en función de una agenda dada. Esta agenda es el fichero del que se ha hablado antes, en el cual se escribe la posición y orientación que hay que alcanzar a una hora determinada. A la vez, tiene que ser capaz de moverse a los puntos que le indique el usuario vía RViz. Además, como mejoras al proyecto se ha añadido que el Robot sea consciente de la batería que le queda, volviendo a la posición de carga si le queda poca y no saliendo de allí hasta que haya recargado al menos un pequeño porcentaje. También se ha utilizado uno de los botones de la Turtlebot de manera que, si se pulsa, el robot vuelve automáticamente a la estación de carga. Siempre que se requiera volver automáticamente a la estación de carga, el robot va a realizar una trayectoria a un punto cercano de la estación (1 metro delante de ella, por ejemplo). Tras esto, intentará aparcar automáticamente. Como manejo de excepciones se ha añadido que, si no consigue llegar al punto establecido, emita un pitido de error y vuelva a la estación de carga. Si no consigue volver a la estación, vuelve a emitir el pitido y lo vuelve a intentar. Si no lo consigue en 3 intentos, el programa se cerrará. Capítulo 4. Sistema de mapeo y navegación. 69 Para realizar este sistema de navegación, se requiere de los nodos que son lanzados por el Launch amcl_demo.launch del paquete turtlebot_navigation para la navegación autónoma del robot en un mapa conocido (Apartado 3.4.8. Navegación autónoma en un mapa conocido) y de los nodos lanzados por el minimal.launch (utilizando Hokuyo preferiblemente) del paquete turtlebot_bringup (Apartado 3.4.4. Preparar Turtlebot). Además, se necesitará el nodo “dock_drive” para el aparcamiento automático (Apartado 3.4.9. Aparcamiento Automático). Como se utiliza la navegación autónoma en un mapa conocido, al ejecutar el nodo amcl por primera vez, el robot no sabría en qué posición está (como se ha explicado en el Apartado 3.4.8. mencionado antes). Por lo cual, se ha creado un programa llamado “posinicial.py” que, si se ejecuta, indica al nodo amcl la posición inicial en la que está el robot (en lugar de tener que usar la flecha de RViz). Este programa solamente tiene sentido si siempre iniciamos el amcl desde la misma posición, sino habría que editarlo. Se ha programado de tal forma que, si se modifica el fichero con el horario establecido, el programa, sin necesidad de reiniciarse, actualiza su horario con lo último escrito en el fichero. 4.2.4.1. Diagrama de flujo Se van a presentar dos diagramas de flujo, uno para la actualización del horario y otro con los diversos estados del robot. Para la actualización del horario, el sistema va a estar cada segundo revisando la última fecha de modificación del fichero (excepto cuando esté realizando una tarea, que esperará a que acabe para revisarlo). Si la fecha de modificación es diferente a la de la anterior pasada, el sistema detecta que es necesario actualizar el horario. Para esto, borra todos los trabajos que tenía que realizar con el anterior horario y crea el nuevo con los del fichero actualizado. Así se consigue que solo se tenga que abrir el fichero cuando se sabe de verdad que ha sido modificado (Ilustración 59). 70 Ilustración 59 Diagrama de flujo para actualizar el horario Para poder explicar mejor las acciones del robot, se adjunta el diagrama de flujo con sus diferentes estados (Ilustración 60). Capítulo 4. Sistema de mapeo y navegación. 71 Ilustración 60 Diagrama de flujo de los estados del Robot El robot cuando está parado se está preguntando todo el rato si tiene batería baja o no. Si la tiene y no está en la estación de carga, intenta volver hasta ella. Si se encuentra en ella, se mantiene en ella, sin permitir que haga algo hasta que la batería supere un porcentaje marcado. Si no tiene batería baja, el robot puede salir de la estación de carga. Primero comprueba si tiene algún movimiento planificado en la agenda a esa hora. Si lo tiene intenta realizar el movimiento. Si la posición que se requiere alcanzar es la estación de carga (‘home’ en el fichero, como se ha explicado antes), entonces comprueba si está en la estación de carga ya o no. Si lo está se queda parado, si no lo está intenta volver a la estación. 72 Si la posición que quiere alcanzar es otra cualquiera, primero comprueba si está en la estación de carga o no. Si lo está, da marcha atrás lentamente para despegarse de ella y tras esto, intenta llegar a la posición. Si no está en la estación de carga, intenta llegar al destino directamente. Si no consigue llegar al destino, reproduce un sonido indicando un error y se vuelve a la estación de carga. Si lo consigue, se queda parado en el destino esperando la siguiente orden. Si no tiene batería baja ni movimientos planificados en la agenda a esa hora, puede realizar otras acciones. Pulsar el botón B0 indica al sistema que se requiere la vuelta a la estación de carga. Si se pulsa y no hay ningún movimiento planificado en la agenda en menos de 3 minutos, el robot comprueba si está en la estación de carga. Si no lo está, intenta volver a la estación de carga, si lo está se queda allí. Si hay algún movimiento planificado en la agenda en menos de 3 minutos desde que se pulsa el botón, el robot se queda parado, indicando por pantalla que no hace nada porque hay una tarea que se va a realizar en los siguientes tres minutos. Esto se hace porque la librería Schedule trabaja como una interrupción del sistema. Entonces puede pasar que el robot esté volviendo a la base y, antes de conseguirlo, salte una tarea programada anteriormente en el fichero. Esto haría que el sistema pueda fallar, ya que, tras la interrupción, va a seguir por la parte del código donde estaba antes. Es mejor evitar esta situación, impidiendo volver a la base vía el botón B0 si hay una actividad programada en los 3 minutos siguientes a la pulsación del botón. Cuando se requiera ir a la estación de carga, primero comprobará si está cerca de ella. Si lo está, intentará aparcar. Si lo consigue, el robot se quedará parado en la base. Si no lo consigue, se quedará parado en el sitio en el que esté cuando acabe el tiempo máximo para aparcar. Si no lo consigue debido a que hay obstáculos de por medio y no puede encontrar la estación vía infrarrojos, dará error de sistema y finalizará el programa. Si no está cerca de la estación, se irá a un punto cercano a ella y después intentará aparcar. Siempre que se intente volver a la estación de carga y no se pueda, se va a volver a intentar. Si tras tres intentos no se consigue, se reproducirá un sonido indicando el final de programa y lo finalizará. Además, en todo momento el usuario puede acceder desde el ordenador a controlar el robot, ya sea vía teclado o mandando destinos vía RViz. Capítulo 4. Sistema de mapeo y navegación. 73 4.2.4.2. Descripción de funciones En el apartado de código en los anexos (“9.1. Código”), se encuentran los códigos importantes escritos y comentados. Aquí se va a hacer una descripción de lo que se quiere hacer con cada función. 4.2.4.2.1. posinicial.py El programa posinicial.py es el encargado de indicar al sistema de navegación dónde se encuentra el robot en un inicio. Este programa se basa en la llamada a la única función que tiene “pub_posini()” (ilustración 61) Ilustración 61 pub_posini() del programa posinicial.py En esta función, se ha guardado como variables las coordenadas y orientación del robot cuando se encuentra en la base de carga en un mapa conocido. Si se utiliza otro mapa o se cambia la base de sitio habría que modificar estos parámetros. Estos valores se han obtenido escuchando el topic “initialpose” cuando se utilizaba la herramienta “2D Pose Estimate” de RViz para estimar la posición actual del robot cuando se encontraba en la base de carga. Inicia el nodo “PosicionInicial” y crea un publicador capaz de publicar mensajes del tipo “PoseWithCovarianceStamped” en el topic “initialpose”. Se crea una variable que guarda el mensaje del tipo “PoseWithCovarianceStamped” y se guardan los parámetros requeridos a publicar. Tras esto espera 1 segundo ya que crear el publicador no es un proceso instantáneo y, si se intenta publicar cuando no está creado, ni publicará ni saldrá un error indicando que no se puede realizar. 74 Tras esperar este segundo, se publica el mensaje y se escribe por pantalla indicando que la posición inicial se ha publicado. Esta función es llamada una vez cada vez que se inicia el programa. 4.2.4.2.2. sch_pos_with_battery_v4.py El programa sch_pos_with_battery_v4.py es la base del sistema de navegación en función de una agenda establecida. Este programa se adjunta entero con comentarios en los anexos. Para la realización de este programa, se ha intentado que la programación sea lo más modular posible, consiguiendo que, si hay algún fallo, se pueda encontrar rápidamente. Para ello se ha dividido en muchas funciones, las cuales se van a explicar ahora. Ilustración 62 Constructor de la clase SchedulePosition() del programa sch_pos_with_battery_v4 En la ilustración 62 se expone la función constructor de objetos de la clase SchedulePosition(), los cuales tendrán como métodos todas las funciones de este programa. En el constructor se le indica que no hay ningún destino enviado y se le indica la función a llamar cuando se obtiene un CTL+C de la terminal, siendo esta la denominada shutdown(). Tras esto, se crea un cliente para el servidor “move_base” y se espera a que este servidor esté creado, ya que va a ser la base de la navegación autónoma. También se crea un publicador de mensajes tipo “Sound” en el topic “/mobile_base/commands/sounds” para poder reproducir sonidos desde la Turtlebot. Por último, se crea un subscriptor al topic “/mobile_base/sensors/core” donde se publican los datos de la batería y se le indica la función a la que tiene que llamar cada vez que se publique un mensaje, denominada SensorStateCallback(). Además, se crea otro subscriptor al topic “/mobile_base/events/button” donde se publican los mensajes relacionados Capítulo 4. Sistema de mapeo y navegación. 75 con los botones, y, al igual que antes, se le indica que la función “ButtonEventCallback” es a la que tiene que llamar cada vez que se publique un mensaje en este topic. Se espera un segundo para dar tiempo a que se inicialice todo. Ilustración 63 Función shutdown() del programa sch_pos_with_battery_v4 La función shutdown() (ilustración 63) es llamada cuando se recibe un CTL+C desde la terminal. Si hay un destino enviado, se cancela, se envía por pantalla un mensaje diciendo que se para el programa y llama a la función ReproducirSonido() con el argumento 1. Esta función reproduce sonidos dependiendo el argumento que se le pase. Con lo cual, la Turtlebot reproduce un sonido de apagado. Se espera un segundo a que se pare todo y se cierra el programa. 76 Ilustración 64 Función LeerFichero() del programa sch_pos_with_battery_v4 La función LeerFichero() de la ilustración 64 es de las más importantes del programa. En esta, se abre el fichero con la agenda establecida, se guarda línea a línea en una lista de Python (vector), siendo cada línea una posición de esta lista, y se cierra el fichero. Tras esto, para cada posición de la lista se borran los espacios del inicio y del final para evitar errores. Entonces, si la posición no está vacía, lo que querría decir una línea en blanco en el fichero, se borran los espacios que pueda haber en el medio de las líneas por algún fallo del usuario al escribirlas, evitando errores a la hora de leerlas. Solo borra un espacio, si se han puesto más daría errores. Tras esto, se empieza a descomponer la línea, la cual está dividida en puntos y comas. Lo que hay dentro de cada punto y coma se mete en un string, que en el caso de las coordenadas y las orientaciones se convierten en una variable float. Capítulo 4. Sistema de mapeo y navegación. 77 Con la línea ya dividida, se meten en una lista denominada tarea las variables obtenidas, siendo la primera posición la hora en formato string, seguida de la coordenada ‘x’, la coordenada ‘y’ y los elementos del cuaternio. Esta lista, se mete en otra lista denominada Schedule, consiguiendo realizar una lista de listas en el cual cada línea es una tarea del horario. Esta tarea tiene dividido en diferentes posiciones las diferentes variables requeridas para navegar a una determinada hora. Una vez metida la tarea en la lista Schedule, se borra la tarea y se pasa a hacer lo mismo con la siguiente línea del fichero, añadiendo la siguiente tarea en la siguiente posición de la lista Schedule. Cuando termina con el fichero, retorna la lista Schedule, consiguiendo una matriz donde las filas son las tareas a realizar y las columnas son la hora a la que se requiere ir al destino, la coordenada ‘x’, la coordenada ‘y’ y los elementos del cuaternio del destino. Ilustración 65 Función ult_modificacion() del programa sch_pos_with_battery_v4 Para conseguir saber si se ha realizado alguna modificación en el fichero sin tener que abrirlo todo el rato, se ha definido la función ult_modificacion() de la ilustración 65. Con esta, gracias a las librerías os y time, se puede retornar la fecha de la última modificación de un fichero. Ilustración 66 Función goto() del programa sch_pos_with_battery_v4 La función goto(pos, quat) (ilustración 66) es otra de las más importantes del programa. Al ser llamada, primero, si la posición a la que quiere llegar no es la 84 Esta variable se le pasa como argumento a la función planifica() de ese objeto, creando el planificador. Tras esto, mientas no se cierre el programa, el programa va a estar en bucle comprobando que la última modificación del fichero no ha cambiado. Si ha cambiado significa se ha modificado el fichero, por lo que hay que actualizar el horario. Si el robot no necesita cargarse (comprobándolo con la función NeedCharge()) y el horario está actualizado (no ha habido cambios en el fichero), se ejecuta el planificador con todas las tareas que tenga guardadas. Si el fichero no está actualizado o necesita carga, se borran todas las tareas del planificador, se vuelve a leer el fichero y se guardan otra vez. 85 5. Capítulo 5. Puesta en marcha y resultados. En este capítulo se va a exponer la puesta en marcha del software desarrollado, así como un análisis de los resultados obtenidos. 5.1. Exploración 5.1.1. Puesta en marcha bumper.py Suponiendo que la comunicación con el ROS Máster se ha configurado bien, en primer lugar, para poder mapear utilizando el programa bumper.py realizado, se necesita arrancar la Turtlebot. Para ello se utilizará el comando explicado en el apartado “3.4.4. Preparar Turtlebot” con el que se consigue inicializar el sensor LiDAR: $ roslaunch turtlebot_bringup minimal_with_hokuyo.launch Tras esto, en otro terminal se ejecuta el archivo launch con el que se consigue mapear con el LiDAR, explicado en el apartado “3.4.7. Construir un mapa (SLAM)”: $ roslaunch turtlebot_navigation gmapping_demo_hokuyo.launch Por último, en otra terminal se ejecuta el script bumper.py (ya que no se implantó en ningún paquete) con el siguiente comando: $ python /ruta/bumper.py Con esto se conseguiría empezar a mapear la sala, consiguiendo que el robot se mueva de manera aleatoria sin necesidad de la presencia del usuario. Por pantalla aparecerían las diversas colisiones indicando el lado por el que se producen (Ilustración 77). 86 Ilustración 77 Terminal programa bumper.py Para poder visualizar el mapa que se va dibujando, habría que escribir en una terminal en el ordenador que se pueda ejecutar RViz lo siguiente: $ roslaunch turtlebot_rviz_launchers view_navigation.launch Y para guardar el mapa, en otra terminal, se debe ejecutar el siguiente comando, sin haber cerrado la terminal en la que se había lanzado el archivo launch de mapeo: $ rosrun map_server map_saver -f /tmp/my_map Estos dos últimos explicados en el apartado “3.4.7. Construir un mapa (SLAM)”. 5.1.2. Resultados bumper.py Los resultados obtenidos con este programa fueron satisfactorios para el inicio del proyecto. Se consiguió una forma de que el robot mapeara la sala de manera automática. Los principales problemas que tenía este programa eran las esquinas, quedarse encerrado en una habitación mucho tiempo hasta conseguir salir (ya que el movimiento era pseudo aleatorio) y que podía pasar por zonas ya mapeadas. Además, el robot se pone en marcha hacia delante y solo se da cuenta de que hay un objeto delante si toca el parachoques, pudiendo ser un problema si hay algo elevado. Soluciones a estos problemas han sido expuestas en el apartado “4.1.4. Mejoras para el programa bumper.py” 5.1.3. Puesta en marcha “explore_lite” Como se ha comentado en el apartado “4.1.5. Paquete “explore_lite””, se descubrió este paquete realizado por la comunidad capaz de explorar un entorno desconocido mientras realiza un mapa. Con este paquete se solucionaban todos los problemas del programa “bumper.py”. Tras modificar Capítulo 5. Puesta en marcha y resultados. 87 este paquete para implementarlo en el robot del proyecto, se puede comenzar la puesta en marcha. Para comenzar a mapear, al igual que antes, hay que abrir dos terminales, una para arrancar el robot y otra para arrancar los nodos encargados de mapear: $ roslaunch turtlebot_bringup minimal_with_hokuyo.launch $ roslaunch turtlebot_navigation gmapping_demo_hokuyo.launch Tras esto, se ejecuta el Launch “explore.launch” del paquete “explore_lite”: $ roslaunch explore_lite explore.launch Para poder visualizar el mapa creado y conseguir guardar el mapa se realizaría como con el programa “bumper.py”. 5.1.4. Resultados “explore_lite” Este paquete ha aportado justo lo que se requería para el proyecto. El robot es capaz de reconocer las partes del mapa que tiene sin cerrar, es decir, las fronteras abiertas, e intentar llegar hasta ellas. Además, utiliza el nodo “move_base” de la pila de navegación de ROS para conseguir moverse, lo cual hace que consiga sortear obstáculos sin necesidad de chocarse. El principal problema que se encuentra a la hora de utilizar este paquete es la gran cantidad de tiempo que le toma encontrar las fronteras abiertas e intentar llegar hasta ellas, ya que tiene que intentar encontrar una trayectoria para llegar con cada frontera. Si existen fronteras que no son alcanzables, el programa se ralentiza ya que no es capaz de llegar y, al usar el nodo “move_base” para moverse, realizaría los mecanismos de recuperación mencionados en el apartado “3.3.4.3. Recovery behaviors” para intentar llegar a estos puntos. Además, en espacios estrechos el robot se puede quedar atascado, al creer que, si se mueve, colisionaría contra algo. A pesar de estos problemas, el resultado es bastante satisfactorio, ya que en un principio se quería realizar un programa por el cual un robot “tonto” mapease la sala moviéndose de manera aleatoria. Al final se ha conseguido que el robot sea inteligente, reconociendo las partes del mapa que le quedan por descubrir y llegando a ellas. 88 5.2. Sistema de navegación por agenda 5.2.1. Puesta en marcha Para conseguir ejecutar el sistema de navegación autónoma a lugares establecidos en función de una agenda, es necesario haber arrancado la Turtlebot y los nodos encargados de la navegación autónoma en un mapa conocido. Luego lo primero de todo es realizar un mapa del entorno, por lo que el software implantado para la exploración y mapeo de lugares desconocidos viene muy a cuento. Tras tener un mapa, se arranca la Turtlebot y el sensor LiDAR: $ roslaunch turtlebot_bringup minimal_with_hokuyo.launch Lo siguiente sería ejecutar los nodos encargados de la navegación autónoma en un entorno conocido. Para ello se utiliza el siguiente comando, explicado en el apartado “3.4.8. Navegación autónoma en un mapa conocido”: $ roslaunch turtlebot_navigation amcl_demo_hokuyo.launch map_file: =/ruta/mymap.yaml Para visualizar la posición en el mapa del robot, así como las trayectorias que va a realizar, habría que utilizar el siguiente comando en el ordenador que pueda ejecutar RViz: $ roslaunch turtlebot_rviz_launchers view_navigation.launch En un primer instante, habría que posicionar al robot en el sitio en el mapa donde se encuentra en ese momento. Para ello se podría utilizar la herramienta “2D Pose Estimate” de RViz, consiguiendo estimar la posición a base de pruebas, como se ha explicado en el apartado “3.4.8. Navegación autónoma en un mapa conocido”. Para no tener que hacer esto así, se creó el nodo “posinicial.py”. En este se ha guardado la posición en el mapa realizado donde se encuentra la Turtlebot si está en la base de carga y al ejecutarse la publica indicando que el sitio donde se encuentra el robot es la base de carga. Luego para poder utilizarlo y que funcione bien, el robot tiene que estar en la base de carga y utilizar el mapa realizado del que se han sacado las coordenadas del robot cuando está en la base. Si la base de carga se mueve de sitio, habría que modificar estas coordenadas. Para poder ejecutarlo se utiliza el siguiente comando: Capítulo 5. Puesta en marcha y resultados. 89 $ rosrun scheduleposition posinicial.py Tras publicar la posición inicial, se cierra el programa automáticamente. El siguiente paso sería realizar el horario. Para ello se puede ejecutar el nodo “clickedpoint.py” con el comando: $ rosrun scheduleposition clickedpoint.py Una vez ejecutado, con RViz abierto con el mapa, se utiliza la herramienta “Publish Point” para guardar los puntos a los que se requiere ir en el fichero “Schedule.txt”. Como se ha explicado en el apartado “4.2.2. Método de escritura de los destinos en el fichero”, el primer click es el destino final y el segundo click será la posición hacia donde mirará el robot. Este nodo se seguirá ejecutando hasta que se pare manualmente con CTL+C o cerrando la terminal. Tras esto, habría que modificar el fichero “Schedule.txt” cambiando la hora que pone por defecto por la hora que se requiera ir a cada posición. También se podría realizar este fichero a mano, pero es más tedioso. Una vez realizado todo esto, se podría comenzar a ejecutar el nodo realizado para la navegación en función de la agenda. Para ello se ha realizado un archivo Launch en el que se ejecuta el nodo que activa el servidor para el aparcamiento automático (ya que se le va a llamar cuando se requiera volver a la base de carga) y el nodo realizado. El contenido se muestra en la ilustración 78. Ilustración 78 minimalv2.launch del paquete scheduleposition Para ejecutarlo, se escribiría el siguiente comando: $ roslaunch scheduleposition minimalv2.launch Con esto, el programa estaría iniciado. Es compatible con la teleoperación vía teclado o vía RViz, pero siempre da prioridad a las tareas de la agenda. 90 5.2.2. Resultados Tras haber sometido a numerosas pruebas al programa, intentando hacer que falle para encontrar sus inconsistencias, se ha llegado a unos resultados altamente satisfactorios. Gracias a este programa el robot es capaz de navegar a destinos previamente especificados en una agenda, además de por llamadas del usuario vía RViz o teleoperación y de por decisiones de sistema de control, como la vuelta a la estación si la batería es baja. Además, se han analizado todos los posibles fallos que puede haber, creando una vía de escape si estos suceden. A parte, la creación de los programas “posinicial.py” y “clickedpoint.py” consiguen que la preparación previa para utilizar este programa sea menos tediosa. El mapa utilizado para estas pruebas se llama “UltimateMap.yaml”, el cual se puede observar en la ilustración 79. Este mapa se ha creado con el software de exploración y mapeo de entornos desconocidos implementado en este proyecto. Se ha cumplido con creces el objetivo propuesto para este proyecto. Ilustración 79 Mapa utilizado en el proyecto 91 6. Capítulo 6. Gestión del trabajo. En este apartado se va a realizar un estudio de la gestión del trabajo, es decir, la planificación que se ha seguido a la hora de llevar a cabo este proyecto, así como un estudio del coste económico que supone su elaboración. 6.1. Planificación del proyecto Para poder exponer la planificación del proyecto para alcanzar los diferentes objetivos, se ha realizado un diagrama de Gantt (Ilustración 80). Este diagrama se ha agrupado en diferentes fases del proyecto que se van a explicar ahora, pero no se ha querido subdividir más porque quedaba un diagrama muy extenso y difícil de visualizar. Ilustración 80 Planificación del proyecto Las primeras semanas se comenzó con una preparación para hacer frente al proyecto. Se empezó con la introducción al entorno de programación ROS, con el aprendizaje de los conceptos y la realización de tutoriales. Tras comprender bien esto, se comenzó el estudio de la Turtlebot2, donde se realizaron diferentes tutoriales y se analizó el Hardware y los diversos topics que conectaban el Hardware con ROS. Teniendo conocimientos sobre ROS y el robot, la quinta semana se empezó a investigar la realización de un programa explorador que mapee de forma automática, lo que conllevó, poco después del inicio, a estudiar la pila de navegación de ROS. Tras realizar el programa bumper.py e intentar implementar diversos paquetes de la comunidad, se eligió el paquete explore_lite, ya explicado en el apartado 4.1.5. Paquete “explore_lite“. Tras esto, se comenzó a realizar el sistema de navegación en base a una agenda, a la vez que se analizaba la pila de navegación de ROS cada vez que surgía algún problema. 92 Se inició realizando un programa que leyese una posición de un fichero e intentase llegar hasta ella. Luego, se hizo una segunda versión consiguiendo que leyese del fichero las posiciones y las horas a las que había que llegar a estos sitios, planificase un horario y realizara las acciones correspondientes. En la tercera versión se añadió que el robot fuera consciente de la batería que le quedaba y que si tenía poca volviera a la base de carga. Una vez finalizado esto, se añadieron las excepciones, controlando los movimientos del robot si no se consiguiera llegar a una posición establecida (se eligió que volviera a la base). Como último aporte se escribió el script que inicializa la posición inicial en el mapa. También se realizó el programa para guardar los puntos del mapa en el fichero. En un principio solo tenía en cuenta las coordenadas del punto final y llegaba con cualquier orientación. Luego se escribió otra versión que conseguía tener en cuenta esta orientación. Mientras tanto, ya se había comenzado a escribir la memoria. La duración del proyecto total ha sido de 20 semanas. Sumando un total de unas 600 horas aproximadamente. 6.2. Estudio económico 6.2.1. Recursos empleados A continuación, se muestra un resumen de los componentes hardware y software empleados en el desarrollo del proyecto: • Software: - Sistema operativo: Ubuntu 20.04, Ubuntu 16.04 y Windows 10 - Pack de Microsoft Office - Entorno de programación “Robot Operating System” • Hardware: - Robot TurtleBot2 - Brazo robótico WidowX - Sensor LiDAR Hokuyo URG-04LX - Controlador Intel NUC 7I5BNK - Ordenador fijo HP Compaq 8100 Elite i5. Capítulo 6. Gestión del trabajo. 93 • Material ofimático. 6.2.2. Costes directos Los costes directos son aquellos que se asignan de manera clara a un proyecto, se asocian directamente con un producto terminado o su elaboración. Estos pueden ser el coste de material, costes amortizables de equipos y coste del personal. 6.2.2.1. Coste del personal La realización del presente proyecto ha sido llevada a cabo por un estudiante de ingeniería, encargado de programar el sistema de navegación del robot y de escribir la memoria, bajo la supervisión de dos ingenieros. Se estima el sueldo anual teórico del estudiante como si de un ingeniero júnior se tratara, teniendo en cuenta el sueldo medio anual de éste en España, para analizar el coste de su trabajo en función de las horas empleadas. Este coste incluye: - Sueldo bruto anual, así como los posibles incentivos por su trabajo. - Cotización a la Seguridad Social, que es un 35% del sueldo bruto. Teniendo en cuenta esto, el coste anual del ingeniero será de 34.931,25 € (Tabla 6.1). COSTE ANUAL Sueldo bruto más incentivos 25.875,00 € Seguridad Social (35% sueldo bruto) 9.056,25 € Coste total 34.931,25 € Tabla 6.1. Coste anual del personal Se calcula una estimación de los días efectivos trabajados en un año. (Tabla 6.2). DÍAS EFECTIVOS POR AÑO Año medio 365,25 días Sábados y Domingos -104,36 días Días de vacaciones efectivos -20,00 días Días festivos reconocidos -15,00 días Días perdidos estimados -5,00 días Total días efectivos estimados 220,89 días Tabla 6.2. Días efectivos por año 100 Este objetivo se ha cumplido con creces, ya que a mayores de esto se han añadido más funcionalidades, como es el manejo de excepciones si no consigue llegar al punto indicado o que esté atento a la batería que le queda. Respecto a los objetivos intermedios que se impusieron al inicio del proyecto, se adjunta una tabla con los resultados obtenidos (Tabla 7.1). Requerimiento ¿Objetivo obtenido? Familiarización con el entorno de programación ROS Si Comprender la pila de navegación de ROS Si Aprender conceptos sobre el robot Turtlebot2 Si Conseguir que el robot sea capaz de mapear el entorno Si Desarrollar un sistema de planificación para que el robot pueda dirigirse a ciertas ubicaciones a una hora determinada Si Tabla 7.1 Objetivos cumplidos Ya no sólo se ha conseguido una familiarización con el entorno de programación de ROS, sino que se ha obtenido un gran conocimiento sobre este. Además de comprender de manera minuciosa su pila de navegación, tras haber hecho frente a una gran cantidad de problemas en la navegación autónoma del robot. Se ha hecho un estudio amplio de los conceptos del robot Turtlebot2, tanto de su Hardware como de su Software. Se ha conseguido que el robot sea capaz de mapear el entorno y, además, se ha conseguido que este mapeo sea automático, sin necesidad de que el usuario esté atento a él. El desarrollo del sistema de navegación en función de una agenda planificada se ha cumplido perfectamente, estudiando todos los fallos que pueden cometerse y proponiendo una solución. Capítulo 7. Conclusiones y líneas de trabajo futuras. 101 7.3. Propuestas de trabajo futuro Al ser el inicio de un proyecto, este puede tener una gran cantidad de líneas de trabajo futuro. Por lo cual, se ha realizado una programación altamente escalable, con la mayor parte del código comentada para evitar dudas. La principal sería conseguir una interfaz de usuario mejor a la hora de guardar los destinos y las horas en el horario, ya que la actual es bastante tediosa. Se estudió durante varios días la posibilidad de conseguir esto añadiendo alguna herramienta a RViz por la cual se pudiera añadir un display con la que se pasase la hora y el punto elegido o por medio de un servidor web que realice esto, pero se salía del alcance del proyecto. Se intentó realizar una interfaz con la librería de Python “tkinter”, pero no fue convincente del todo al no poder añadir un mapa. El primer paso hacia una interfaz mejor podría ser mejorar el programa que guarda los puntos en el fichero, haciendo que, el programa, una vez el usuario haya elegido el destino, pregunte al usuario la hora a la que quiere ir a ese sitio. Tras escribirla, el programa podría guardar en el fichero la hora y el destino. Otra posible línea de trabajo futuro es la implementación de un servidor web para el manejo del robot Turtlebot, de manera que no haya que utilizar la terminal para realizar movimientos o mapear. Además, tras realizar este servidor, se podría estudiar lo de mejorar la interfaz que se ha propuesto antes. Por otro lado, se podría hacer un estudio del brazo robótico, de su puesta en marcha y posibles funciones. Se podría plantear el cambio de la cámara Astra de sitio a estar encima de este brazo robótico, de manera que viese lo que está en frente de él. De esta forma, se podrían realizar programas basados en visión artificial para que la pinza pueda agarrar objetos que hay en frente. El cambio de posición de la cámara no sería un problema, ya que para mapear y navegar es capaz de utilizar el sensor LiDAR. El único fallo posible es la limitación de precisión que tendrá este brazo robótico, al estar formado por servomotores. Además, otra propuesta de trabajo futuro sería implementar los programas en Python 3. Todas estas posibles líneas de trabajo futuro podrían ser buenas opciones para trabajos de fin de grado de futuros estudiantes. 102 103 8. Bibliografía [1] M. Ben-Ari, Elements of Robotics, ISBN 978-3-319-62532-4, Springer, 2017. [2] E. B. Kuipers, «Shakey: From Conception to History,» 2017. [En línea]. Available: http://ai.stanford.edu/~nilsson/OnlinePubs-Nils/General%20Essays/Shakey-aimag- 17.pdf. [Último acceso: Mayo 2022]. [3] «TurtleBot,» [En línea]. Available: https://www.turtlebot.com/turtlebot2/. [Último acceso: Junio 2022]. [4] «O'Reilly,» [En línea]. Available: https://www.oreilly.com/library/view/rosprogramming-building/9781788627436/9ddba456-a610-4f81-a0ef- 80ac7ccceee8.xhtml. [Último acceso: Junio 2022]. [5] A. Polanco Masa, «Tecnología Obsoleta,» 2015. [En línea]. Available: https://alpoma.net/tecob/?p=11359. [Último acceso: Junio 2022]. [6] «Kawasaki Robotics,» [En línea]. Available: https://kawasakirobotics.com/euafrica/company/history/?wovn=es. [Último acceso: Junio 2022]. [7] «Museum of Transport and Technology,» [En línea]. Available: https://collection.motat.nz/objects/104032/industrial-robot-irb6-allmanna- svenska-elektriska-aktiebolaget-asea. [Último acceso: Junio 2022]. [8] «Proyecto Idis,» [En línea]. Available: https://proyectoidis.org/shakey/. [Último acceso: Mayo 2022]. [9] Oxford Languages, Diccionario Oxford Spanish, ISBN 978-0199543403, 2008. [10] «International Federation of Robotics,» [En línea]. Available: https://ifr.org/service-robots. [Último acceso: Junio 2022]. [11] «WikiROS, Distribuciones,» [En línea]. Available: http://wiki.ros.org/Distributions. [Último acceso: Junio 2022]. [12] «TurtleBot,» [En línea]. Available: https://www.turtlebot.com/about/. [Último acceso: Junio 2022]. 104 [13] «WikiROS, Introducción,» [En línea]. Available: http://wiki.ros.org/ROS/Introduction. [Último acceso: Junio 2022]. [14] «WikiRos, Conceptos,» [En línea]. Available: http://wiki.ros.org/es/ROS/Conceptos. [Último acceso: Junio 2022]. [15] «WikiROS, Mensajes,» [En línea]. Available: http://wiki.ros.org/msg. [Último acceso: Junio 2022]. [16] L. Joseph, Mastering ROS for Robotics Programming, ISBN 978-1788478953, Packt, 2015. [17] M. S. H. Achmad, «Tele-Operated Mobile Robot for 3D Visual Inspection Utilizing Distributed Operating System Platform,» 2017. [18] M. Ferber, «MARVINFERBER BLOG,» 2017. [En línea]. Available: https://marvinferber.net/?p=128. [Último acceso: Junio 2022]. [19] «Dabit Industries,» [En línea]. Available: https://dabit.industries/products/iclebokobuki. [Último acceso: Junio 2022]. [20] Yujin Robot, «Guía de Usuario Kobuki,» [En línea]. Available: http://kobuki.yujinrobot.com/wiki/online-user-guide/. [Último acceso: Junio 2022]. [21] «Intel NUC,» [En línea]. Available: https://www.intel.es/content/www/es/es/products/details/nuc.html. [Último acceso: Junio 2022]. [22] «ROS Components,» [En línea]. Available: https://www.roscomponents.com/es/camaras/76-orbbec.html. [Último acceso: Junio 2022]. [23] «HOKUYO,» [En línea]. Available: https://hokuyo-usa.com/products/lidar-obstacle- detection/urg-04lx. [Último acceso: Junio 2022]. [24] «WikiROS, PhantomX Reactor Arm,» [En línea]. Available: http://wiki.ros.org/phantomx_reactor_arm. [Último acceso: Junio 2022]. [25] «Trossen Robotics,» [En línea]. Available: https://www.trossenrobotics.com/p/phantomx-ax-12-reactor-robot-arm.aspx. [Último acceso: Junio 2022]. 105 [26] «Pila de navegación ROS,» [En línea]. Available: http://wiki.ros.org/navigation. [Último acceso: Junio 2022]. [27] R. M. D. Silva, «Modelo del sistema de odometría de un robot,» [En línea]. Available: https://www.researchgate.net/figure/Figura-2-Modelo-do-sistema-de- odometria-do-robo_fig2_343646784. [Último acceso: Junio 2022]. [28] J. Zhang, 2020. [En línea]. Available: https://matheecs.tech/study/2020/01/08/move-base.html. [Último acceso: Junio 2022]. [29] «WikiROS, AutoAparcamiento,» [En línea]. Available: http://wiki.ros.org/kobuki/Tutorials/Automatic%20Docking. [Último acceso: Junio 2022]. [30] «Explore_lite,» [En línea]. Available: http://wiki.ros.org/explore_lite. [Último acceso: Junio 2022]. [31] M. Quigley, B. Gerkey y W. D. Smart, Programming Robots With ROS, ISBN 9781449323899, O'Reilly Media, 2015. [32] J. M. O’Kane, A Gentle Introduction to ROS, ISBN 9781492143239, Columbia, South Carolina, 2016. 106 107 9. Anexos 9.1. Código 9.1.1. Scripts 9.1.1.1. bumper.py # !/usr/bin/env python #import roslib import rospy #Para escribir un nodo de ROS en pythoni import time from std_msgs.msg import String #para usar el tipo de mensaje String from geometry_msgs.msg import Twist #para usar el tipo de mensaje Twist (velocidad) from kobuki_msgs.msg import BumperEvent #para usar el tipo de mensaje BumperEvent (sensores de presion) from math import radians class bumpernode(): colision = 0 actual = 0 angulo = 0 move_cmd = Twist() colision_cmd = Twist() def __init__(self): #self representa la instancia del objeto en si mismo #Mensaje que obtenemos por pantalla rospy.loginfo('Programa choques') #Funcion que llamamos si terminamos el programa (ctrl+c) rospy.on_shutdown(self.shutdown) #Suscriber que lee datos del parachoques #Se subscribe al topic '/mobile_base/events/bumper' usando el tipo de mensaje BumperEvent #callback es la funcion a la que se llamara al recibir el mensaje en el topic rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, self.callback) #Publisher que se encarga de los movimientos del robot #Va a publicar en el topic cmd_vel_mux/input/navi, encargado del movimiento de la turtlebot, el tipo de mensaje Twist #queue_size limita el numero de mensajes en cola si algun subscriptor no esta recibiendo los mensajes suficientemente rapido self.turtle_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 108 def navegar(self): self.move_cmd.linear.x = 0.2 #velocidad publicada si no hay choque self.colision_cmd.linear.x = -0.1 #velocidad publicada si hay choque if self.colision == 0: self.turtle_vel.publish(self.move_cmd) #si no hay choque va hacia delante else: self.colision_cmd.angular.z = radians(self.angulo) self.turtle_vel.publish(self.colision_cmd) #si hay choque va hacia atras y gira if time.time()-self.actual>2: #esto lo hace durante 2seg desde el choque self.colision = 0 self.actual = 0 #Funcion que se reproduce cada vez que hay una colision #data es el mensaje que recibe BumperEvent def callback(self,data): if data.state == 1: #Si es un 1, significa que hay colision self.colision = 1 self.actual = time.time() #Guarda el instante del choque if data.bumper == 0: #colision por la izquierda print "COLISION IZQUIERDA" self.angulo = -15 elif data.bumper == 1: #colision por el centro print "COLISION CENTRAL" self.angulo = -45 elif data.bumper == 2: print "COLISION DERECHA" #colision por la derecha self.angulo = 15 def shutdown (self): rospy.loginfo("STOP TURTLEBOT") print "Saliendo..." self.turtle_vel.publish(Twist()) #Paramos el robot rospy.sleep(5) #Esperamos a que se detenga para salir. if __name__ == '__main__': try: rospy.init_node('bumpernode',anonymous=True) explorer = bumpernode() while not rospy.is_shutdown(): explorer.navegar() #navega en bucle mientras no paremos el programa except: rospy.loginfo ("bumpernode node terminated") 109 9.1.1.2. clickedpoint.py #!/usr/bin/env python import rospy from geometry_msgs.msg import PointStamped from datetime import datetime from math import * class Meta(): x = [] y = [] #Constructor def __init__(self): rospy.on_shutdown(self.shutdown) rospy.Subscriber('/clicked_point', PointStamped, self.callback) #subscriptor a los mensajes del topic clicked_point #Funcion encargada de generar el cuaternio con la orientacion def orientacion(self,x1,y1,x2,y2): quat = [] v = [x2-x1,y2-y1] #vector de direccion del eje x nuevo productoescalar = v[0]*1+v[1]*0 #producto escalar entre la direccion nueva del eje x y la direccion del eje x de la base (1,0) mod_v = sqrt(v[0]*v[0] + v[1]*v[1]) #modulo del vector ang = acos(productoescalar/mod_v) #angulo entre los dos vectores arccos(productoescalar/producto de modulos) en radianes quat = [cos(ang/2),0,0,sin(ang/2)] #cuaternio = [q0,q1,q2,q3] siendo q0 la componente escalar self.x[:] = [] #borra el vector x para los siguientes puntos self.y[:] = [] #borra el vector y para los siguientes puntos return quat #Funcion que es llamada cuando se recibe un mensaje del topic clicked_point def callback(self,msg): rospy.loginfo("coordinates:x=%f y=%f" %(msg.point.x,msg.point.y)) self.x.append(msg.point.x) #Guarda la posicion x en el vector x self.y.append(msg.point.y) #Guarda la posicion y en el vector y #Funcion encargada de escrbir en el fichero def escribefichero(self,x,y,cuaternio): f = open("/home/tb2/catkin_ws/src/scheduleposition/scripts/Schedule.tx t", "a") #Abre el fichero de la ruta en modo escritura f.write(datetime.now().strftime('%H:%M')) #Escribe fecha actual en formato HORA:MINUTOS f.write(";") f.write(str(x)) f.write(";") f.write(str(y)) 116 #Hasta que no este por encima del 20% no dejamos que salga de la estacion #Asi evitamos que se ponga a cargar y al instante salga if ((round(float(data.battery)- float(self.battery_dangerous)) / (float(self.max_carga)- float(self.battery_dangerous)) * 100) > 20) : if(self.LowBattery): rospy.loginfo("Puede salir de la estacion") self.LowBattery = False #Ya no hay bateria baja #Funcion encargada de dar marcha atras si estamos en la base y recibimos un destino def BackUp_DockStation(self): if(self.DockStation == True): rospy.loginfo("Robot en estacion. Dando marcha atras antes de empezar") self.DockStation = False self.home = False cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) #Twist es el tipo de mensaje move_cmd = Twist() #Marcha atras a 0.1 m/s move_cmd.linear.x = -0.1 move_cmd.angular.z = 0 r = rospy.Rate(10); #10Hz temp_count = 0 #Marcha atras durante 30 segundos while (not rospy.is_shutdown() and temp_count < 300): cmd_vel.publish(move_cmd) temp_count = temp_count + 1 r.sleep() #Espera hasta 0.1 s (10 HZ) y publica otra vez #Nos aseguramos de que para publicando un Twist() cmd_vel.publish(Twist()) return True #Funcion para ir a la base de carga def GoToDockStation(self): #Primero nos aproximamos a la base if self.DockStation: #Si ya esta en la base no hace nada rospy.loginfo("Ya estamos en la base") result = True else: while (self.home == False): #mientras no estemos cerca de la base, intentamos llegar a ese punto cercano if(self.intentos<3): #se intenta 3 veces rospy.loginfo("Going HOME") self.BorrarCostmaps() #se borran mapas de costo goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose = Pose(Point(self.home_x, self.home_y, 0.000), Quaternion(0.000,0.000, 0.000, 1.000)) 117 self.move_base.send_goal(goal) #se le manda el punto cercano a la base como destino success = self.move_base.wait_for_result(rospy.Duration(500)) #se le deja 500 segundos para llegar state = self.move_base.get_state() result = False if success and state == GoalStatus.SUCCEEDED: #si llega al punto result = True self.home = True #indica que estamos cerca de la base self.intentos = 0 #reinicia intentos else: #si no llega al punto rospy.loginfo("ERROR AL LLEGAR A LA BASE, VOLVIENDO A INTENTARLO") self.move_base.cancel_goal() #cancela el destino cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) cmd_vel.publish(Twist()) #paramos el robot self.intentos +=1 #incrementamos el valor del intento self.ReproducirSonido(4) #reproduce sonido de error rospy.loginfo("Numero intentos: %d", self.intentos) #muestra el numero de intentos que lleva else: #si no lo consigue en tres intentos self.move_base.cancel_goal() #cancela el destino cmd_vel.publish(Twist()) #para el robot result = False rospy.signal_shutdown("ERROR. Imposible llegar a HOME") #sale del programa #Si ha llegado cerca de la base, aparcamos self._client = actionlib.SimpleActionClient('/dock_drive_action', AutoDockingAction) #cliente del servidor AutoDockingAction rospy.loginfo("Esperando al servidor auto_docking") self._client.wait_for_server() #espera hasta encontrar el servidor por si no se ha ejecutado rospy.loginfo("Servidor auto_docking encontrado") goal = AutoDockingGoal() rospy.loginfo("APARCANDO... (cancelando si pasa de 180s)") self._client.send_goal(goal) #envia el destino que es la base de carga #Le damos 180seg para aparcar success = self._client.wait_for_result(rospy.Duration(180)) if success: rospy.loginfo("Aparcamiento conseguido") self.DockStation = True #Estamos en la base return True else: 118 self._client.cancel_goal() #cancela el aparcamiento rospy.loginfo("Fallo en el aparcamiento") return False #Funcion que va a comprobar si se necesita ir a la base de carga def NeedCharge(self): #Si la bateria es baja pero estamos en la base, nos quedamos hasta que este suficientemente cargado if(self.DockStation and self.LowBattery): rospy.loginfo("Robot en la base de carga") rospy.loginfo("Esperando a que este suficientemente cargado") time.sleep(30) return True #Si la bateria es baja y no estamos en la base, va a ella if(not self.DockStation and self.LowBattery): rospy.loginfo("Bateria baja. Yendo a la base de carga") self.GoToDockStation() return True return False #Funcion llamada tras mensaje de los botones def ButtonEventCallback(self, data): if (data.state == ButtonEvent.PRESSED): #Si un boton ha sido presionado if (data.button == ButtonEvent.Button0): #Y ha sido el boton 0 if (schedule.idle_seconds() <= 180): #Segundos hasta la siguiente tarea #Esto lo hago porque las tareas del schedule entran al programa parando cualquier cosa #que este haciendo. Si esta yendo a la base y salta una tarea, parara y realizara la tarea rospy.loginfo("QUEDAN MENOS DE 3 MINUTOS PARA LA SIGUIENTE TAREA") rospy.loginfo("HASTA QUE NO LA REALICE, NO PUEDE IR A HOME POR SEGURIDAD") else: #si no hay tareas cerca if(not self.DockStation): rospy.loginfo("Yendo a la base...") self.GoToDockStation() #si no esta en la base, vuelve a ella else: rospy.loginfo("Ya estamos en la base") #Funcion encargada de borrar los mapas de costo def BorrarCostmaps(self): rospy.wait_for_service('/move_base/clear_costmaps') clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) if(clear_costmaps.call()): #Borra los mapas rospy.loginfo('GLOBAL COSTMAPS BORRADOS') else: 119 rospy.loginfo('Error al llamar al servicio /move_base/clear_costmaps') #Funcion encargada de reproducir sonidos def ReproducirSonido(self, num): self.sound.publish(num) #publica el sonido que se le pase como argumento rospy.sleep(5) if __name__ == '__main__': try: rospy.init_node('SchedulePosition', anonymous=False) #inicia el nodo navigator = SchedulePosition() #crea objeto horario = navigator.LeerFichero() #guarda en horario lo que hay dentro del fichero en una matriz de matrices primeramod = navigator.ult_modificacion() #guarda el valor de la ultima modificacion del fichero navigator.planificar(horario) #planifica while not rospy.is_shutdown(): #mientras no se cierre, entra en bucle ultmod = navigator.ult_modificacion() #comprueba la fecha actual de la ultima modificacion del fichero if (ultmod != primeramod): #si es diferente con la guardada anteriormente navigator.ficheromodificado = True #el fichero ha sido modficiado primeramod = ultmod #guardamos la ultima fecha de modificacion print("Fichero modificado") else: navigator.ficheromodificado = False #sino, no ha sido modificado if (not navigator.NeedCharge() and not navigator.ficheromodificado): #si no necesita carga y el fichero no ha sido modificado. schedule.run_pending() #comienza a realizar tareas en funcion de la planificacion else: #si el fichero ha sido modificado o necesita bateria print("Actualizando horario") schedule.clear() #borra el plan anterior horario = navigator.LeerFichero() #lee el fichero navigator.planificar(horario) #vuelve a planificarlo time.sleep(1) except rospy.ROSInterruptException: rospy.loginfo("Ctrl-C caught. Quitting") 120 9.1.2. Archivos launch 9.1.2.1. minimalv2.launch del paquete scheduleposition <launch> <!-- KOBUKI_AUTO_DOCKING minimal.launch --> <node pkg="nodelet" type="nodelet" name="dock_drive" args="load kobuki_auto_docking/AutoDockingNodelet mobile_base_nodelet_manager"> <rosparam file="$(find kobuki_auto_docking)/param/auto_docking.yaml" command="load"/> <remap from="dock_drive/odom" to="odom"/> <remap from="dock_drive/core" to="mobile_base/sensors/core"/> <remap from="dock_drive/dock_ir" to="mobile_base/sensors/dock_ir"/> <remap from="dock_drive/motor_power" to="mobile_base/commands/motor_power"/> <remap from="dock_drive/velocity" to="mobile_base/commands/velocity"/> </node> <!-- SCHEDULEPOSITION --> <node pkg="scheduleposition" name="sch_pos_with_battery" type="sch_pos_with_battery_v4.py" output="screen"> </node> </launch> 9.1.2.2. minimal_with_hokuyo.launch <launch> <!-- Turtlebot --> <arg name="base" default="$(env TURTLEBOT_BASE)" doc="mobile base type [create, roomba]"/> <arg name="battery" default="$(env TURTLEBOT_BATTERY)" doc="kernel provided locatio for battery info, use /proc/acpi/battery/BAT0 in 2.6 or earlier kernels." /> <arg name="stacks" default="$(env TURTLEBOT_STACKS)" doc="stack type displayed in visualisation/simulation [circles, hexagons]"/> <arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)" doc="3d sensor types [kinect, asux_xtion_pro]"/> <arg name="simulation" default="$(env TURTLEBOT_SIMULATION)" doc="set flags to indicate this turtle is run in simulation mode."/> <arg name="serialport" default="$(env TURTLEBOT_SERIAL_PORT)" doc="used by create to configure the port it is connected on [/dev/ttyUSB0, /dev/ttyS0]"/> <param name="/use_sim_time" value="$(arg simulation)"/> <include file="$(find turtlebot_bringup)/launch/includes/robot.launch.xml"> <arg name="base" value="$(arg base)" /> 121 <arg name="stacks" value="$(arg stacks)" /> <arg name="3d_sensor" value="$(arg 3d_sensor)" /> </include> <include file="$(find turtlebot_bringup)/launch/includes/mobile_base.launch.xml"> <arg name="base" value="$(arg base)" /> <arg name="serialport" value="$(arg serialport)" /> </include> <include unless="$(eval arg('battery') == 'None')" file="$(find turtlebot_bringup)/launch/includes/netbook.launch.xml"> <arg name="battery" value="$(arg battery)" /> </include> <node name="hokuyo" pkg="urg_node" type="urg_node" respawn="false" output="screen"> <param name="calibrate_time" type="bool" value="true"/> <param name="port" type="string" value="/dev/ttyACM0"/> <param name="intensity" type="bool" value="false"/> <param name="min_ang" value="-2.35619449615"/> <param name="max_ang" value="+2.09234976768"/> <param name="cluster" value="1"/> <param name="frame_id" value="hokuyo_laser_frame"/> </node> </launch> 9.1.2.3. gmapping_demo_hokuyo.launch <launch> <!-- 3D sensor --> <arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)"/> <!-- r200, kinect, asus_xtion_pro --> <include file="$(find turtlebot_bringup)/launch/3dsensor.launch"> <arg name="rgb_processing" value="false" /> <arg name="depth_registration" value="false" /> <arg name="depth_processing" value="false" /> <arg name="scan_processing" value="false" /> <!-- We must specify an absolute topic name because if not it will be prefixed by "$(arg camera)". Probably is a bug in the nodelet manager: https://github.com/ros/nodelet_core/issues/7 --> <arg name="scan_topic" value="/scan" /> </include> <!-- Gmapping --> <arg name="custom_gmapping_launch_file" default="$(find turtlebot_navigation)/launch/includes/gmapping/$(arg 3d_sensor)_hokuyo_gmapping.launch.xml"/> <include file="$(arg custom_gmapping_launch_file)"/> <!-- Move base --> <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/> </launch> 122 9.1.2.4. amcl_demo_hokuyo.launch <launch> <!-- 3D sensor --> <arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)"/> <!-- r200, kinect, asus_xtion_pro --> <include file="$(find turtlebot_bringup)/launch/3dsensor.launch"> <arg name="rgb_processing" value="false" /> <arg name="depth_registration" value="false" /> <arg name="depth_processing" value="false" /> <arg name="scan_processing" value="false" /> <!-- We must specify an absolute topic name because if not it will be prefixed by "$(arg camera)". Probably is a bug in the nodelet manager: https://github.com/ros/nodelet_core/issues/7 --> <arg name="scan_topic" value="/scan" /> </include> <!-- Map server --> <arg name="map_file" default="$(env TURTLEBOT_MAP_FILE)"/> <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> <!-- AMCL --> <arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg 3d_sensor)_amcl.launch.xml"/> <arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation --> <arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation --> <arg name="initial_pose_a" default="0.0"/> <include file="$(arg custom_amcl_launch_file)"> <arg name="initial_pose_x" value="$(arg initial_pose_x)"/> <arg name="initial_pose_y" value="$(arg initial_pose_y)"/> <arg name="initial_pose_a" value="$(arg initial_pose_a)"/> </include> <!-- Move base --> <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/$(arg 3d_sensor)_costmap_params.yaml"/> <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"> <arg name="custom_param_file" value="$(arg custom_param_file)"/> </include> </launch>