He decidido usar ROS2 (explicar de qué se trata y el por qué....)

Explicar URDF, RVIZ, GAZEBO, ROS2_CONTROL, DETALLADAMENTE DE QUE SE COMPONE EL PAQUETE QUE ESTOY HACIENDO, LOS TOPICS, el paquete cómo lo he distribuido... !!!

Movimiento de cada eje en gazebo:

Gazebo ignora articulaciones que no tienen propiedades inerciales y de colisión cuando son son fijas.

Imagen creada con Explain Everything

En relación a ROS2_CONTROL:

• Hardware interface: Representa nuestro hardware
• Command interfaces: Lo que podemos controlar
• State interfaces: Lo que podemos monitorear

A ros2_control le da igual si se trata de un coche diferencia, un brazo o cualquier cosa. Todo se puede definir.

• URDF se refiere más a la parte hardware
• .yaml se usa para el controller

Estoy usando ROS Humble en Ubuntu 22.04 LTS

Siguiendo los pasos del tutorial de youtube he podido crear mi primer paquete y poder visualizar la figura en RVIZ

Debido a los comentarios que he añadido al urdf, me daba error al cargarlo en rviz. Gracias a XML online editor he podido darme cuenta que las dos primeras lineas del código SIEMPRE TIENEN QUE ESTAR AHÍ.

Finalmente, esto es lo que he conseguido del tutorial:

Antes de ejecutar es importante repetir los siguientes pasos:

En el directorio juloau@juloau-VivoBook:~/Desktop/TFG/tfg-jlopez/code/ros2$

rm -rf build/ install/ log/

colcon build

source ./install/setup.bash

ros2 launch <mi_paquete>  <nombre_del_launcher>.launch.py 

Para crear un paquete:

ros2 pkg create --build-type ament_cmake <nombre-paquete>

IMPORTANTE!!!: Cada vez que abra la terminal tengo que hacer source del directorio donde tengo el paquete.

Para poder conseguir que el robot se pudiera visualizar tanto en RVIZ y poder hacer simulaciones con él en Gazebo, es preferible que el robot se pueda ver primero en gazebo y luego en RVIZ; ya que si funciona para gazebo, funcionará si o sí para RVIZ. Por eso, ha sido necesario continuar con los URDF pero más complejos. Gracias a este repositorio, he conseguido hacer funcionar mi ejemplo:

Para poder ejecutarlo tienes que seguir los siguientes pasos

colcon build
source ./install/setup.bash
ros2 launch pibotj one_robot_gz_launch.py 

En el día 5/08 he podido hacer los siguientes avances:

• Hacer funcionar el ejemplo del repo tras muchas peleas con las dependencias de bashrc:

• Tras entender cómo funcionan los URDF/XACRO, he conseguido modificar parte del robot a mi gusto:

• He podido ver que se han cargado bien los topics:

En el día 6/08 he podido hacer los siguientes avances:

Ya me conseguido simular bien el robot en rviz y en gazebo sin errores. A falta de colocar bien la posición de la cámara y de situar el módulo GPS.

Gracias a estos enlaces, he conseguido solucionar que se pudiera ver todas las partes tanto en Gazebo como en RVIZ

https://answers.gazebosim.org//question/25166/problem-changing-joint-from-fixed-to-revolute/ https://en.wikipedia.org/wiki/List_of_moments_of_inertia

Todo está almacenado en pibotj. Sin embargo, no he podido conseguir mover el motor de la cámara y voy a usar ros2_control para intentar conseguirlo.

En el día 7/08 he podido hacer los siguientes avances:

He conseguido gracias a este canal y la serie de tutoriales que ha creado y con los conocimientos que tenía previamente, he conseguido con ros2 control controlar por posición la cámara.

ros2 topic pub /pos_cont/commands std_msgs/Float64MultiArray "data: [1.0]"

En el video a continuación puedes ver lo explicado anteriormente:

Gracias a la documentación oficial y sus demos también lo he podido entender.

Finalmente, juntando todo, este es el resultado:

En el día 8/08 he podido hacer los siguientes avances:

Ya he podido torcer la cámara en su lugar correcto, cambiar las inercias y masas y definir un cubo que representa la cámara:

En el día 9/08 he podido hacer los siguientes avances:

Ya he conseguido arreglar la rotación de la cámara cuando hay movimiento y que rote sobre su propio eje. Estaba mal definido en el urdf/xacro la posición del joint y también he tenido que cambiar los valores de los límites de la rotación que va desde -pi/2 a pi/2. La imagen a continuación define esto.

Imagen creada con Explain Everything

En relación al GPS, gracias a las siguientes fuentes he podido conseguir que aparezcan los campos de latitude y longitud en el robot e nsimulación:

• https://github.com/utiasASRL/cpo?tab=readme-ov-file
• https://github.com/NovaRoverTeam/gps (GPS desde Rapsberry usando ROS)
• http://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatFix.html (topic usado)
• https://docs.ros2.org/latest/api/sensor_msgs/msg/NavSatFix.html (mensaje usado)
• https://docs.ros.org/en/rolling/p/gazebo_plugins/generated/classgazebo__plugins_1_1GazeboRosGpsSensor.html (snippet de código usado para definir gps)
• https://oa.upm.es/76046/1/TFM_CARLOS_FERREIRA_GONZALEZ.pdf (documentación importante para futuras consultas)

El video de acontinuación, muestra el lanzamiento correcto y los topics bien definidos:

Instalación de Ubuntu Desktop 22.04 y ROS2 Humble gracias a este tutorial

En el día 14/08 he podido hacer los siguientes avances:

He conseguido conectar por ssh el robot real (servidor) con el ordenador que estoy usando (cliente) gracias a este tutorial

Además, he podido comprobar que la cámara física está bien conectada y detectada por la placa. Para ello, he tenido que descargar una serie de librerías y habilitar legacy camera en raspy-config. Lo he conseguido gracias a este tutorial. No he podido ver la imagen por que estoy conectada por ssh pero al hacer echo de \image_raw y muevo la mano delante de ella, cambian los valores.

Como estoy usando ROS2 los nodos dentro de una misma red están visisbles desde cualquier dispositivo, así que lanzando el nodo de la cámara desde el robot, he podido lanzar \rqt_image_view desde el ordenador y ver que la cámara funciona perfectamente.

Entre los días 20/08-22/08 he podido hacer los siguientes avances: He creado un topic llamado topic_image_raw que publica mensajes de tipo cámara y puedo ver la cámara del robot. Todos los avances están en pibotj_rr en publisher node. Lo he podido crear usando el publisher node de este tutorial. La cámara de mi Raspberry es la 0.

Además, he estado trabajando en el aprendizaje automático. Todos los avances lo puedes ver aquí

He conseguido juntar el topic de la cámara con el modelo entrenado versión 1. El nodo resultante se puede ejecutar:

ros2 run pibotj_rr pothole_detection_node

Como demostración de lo anterior, te adjunto los siguientes videos:

Vista desde el ordenador

Vista desde el mundo real

Se puede ver que va muy lento y necesito mejorar esa reactividad.

Entre los días 23/08-26/08 he podido hacer los siguientes avances:

He conseguido sincronizar los motores y la cámara creando un topic que publica una string con yes o no desde el nodo cámara y los motores se suscriben a ese string para ver su estado y se mueven en linea reacta si no hay bache y si lo detecta paran de moverse.

A continuación puedes ver un video en funcionamiento:

Mientras espero mi Google AI Accelerator, estoy haciendo las primeras versiones del filtro de imagen para poder obtener los puntos de las coordenadas del bache. Estoy siguiendo este tutorial para conseguir el filtro correcto y se aplica solo si se detecta bache. Los pasos que estoy siguiendo son los siguientes:

1º: Suavizar la imagen y reducir el ruido antes de procesarla

 img_blur = cv2.GaussianBlur(image,(7,7),1)

2º: Convertir la imagen a escala de grises para poder facilitar la obtención de bordes y contornos.

img_gray = cv2.cvtColor(img_blur, cv2.COLOR_BGR2GRAY)

3º: Obtener las dimensiones de la imagen en escala de grises

height, width = img_gray.shape

4º: Para poder evitar procesamientos innecesarios, he decidido excluir de la detección los 100 píxeles de la parte superior y los 100 píxeles de la parte inferior debido a las condiciones físicas de la cámara (está torcida mirando hacia el suelo con un ángulo en específico). Para poder conseguirlo he creado una máscara de valores blancos (255):

min_distance_from_top_bottom = 100 
mask = np.zeros_like(img_gray)
mask[min_distance_from_top_bottom:height - min_distance_from_top_bottom, 0:width] = 255

5º: Hay que aplicar el filtro Canny Es importante recordar que estoy trabajando por ssh y que los deslizadores no funcionan por lo que los valores del filtro Canny, han tenido que ser escogidos a mano y siendo muy dificultoso este proceso.

Tras varias horas intentando fijarlo, he elegido los siguientes valores:

img_canny = cv2.Canny(img_gray, 80, 180)

6º: Se aplica una máscara and y poder asegurar que solo los bordes dentro de la zona de interés (definida por la máscara) se mantengan.

img_canny_masked = cv2.bitwise_and(img_canny, mask)

7º: Se aplica una dilatación para expandir los bordes, lo que puede ayudar a cerrar pequeñas brechas en los bordes detectados.

kernel = np.ones((5,5))
img_dilated = cv2.dilate(img_canny_masked, kernel, iterations=1)

8º: Detectar y dibujar los contornos encontrados en la imagen dilatada.

def get_contours(self, img, img_contour):

    # Esta función devuelve una lista de contornos y una jerarquía que describe las relaciones entre ellos.
    contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)


    # Itera sobre cada contorno encontrado.
    for cnt in contours:

        # Calcula el área del contorno actual.
        area = cv2.contourArea(cnt)

        
        # Si el área del contorno es mayor a 1500 píxeles, entonces procede a procesarlo.
        if area > 1500:  # Ignorar pequeños contornos para reducir ruido


            # Dibuja todos los contornos
            cv2.drawContours(img_contour, cnt, -1, (255,0,255), 5)

            # Calcula la longitud del perímetro del contorno cerrado
            peri = cv2.arcLength(cnt, True)

            # Este método intenta simplificar el contorno manteniendo su forma general.
            approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)

            # Imprime el número de vértices de la forma aproximada.
            print(len(approx))

            # Imprime las coordenadas de los vértices de la forma aproximada.
            print(approx)

9º Finalmente se devuelve la imagen que contiene los contornos dibujados para poder comprobar donde caen dichos puntos.

TIP: Si para depurar se quiere mostrar una imagen con el filtro Canny o Dilatada, será necesaria convertirla a BGR antes de mostrala para no tener problema de los canales

newframe = cv2.cvtColor(newframe, cv2.COLOR_GRAY2BGR)

Resultados de la primera versión:

He estado todo el día con el filtro de imagen para conseguir las coordenadas de la imagen y ya que estoy conectada por ssh no puedo usar los deslizadores para poder probar todos los posibles valores de umbrales para Canny, lo he estado haciendo a ojo. Conectando un monitor tampoco me funcionaba, así que tuve que seguir con la táctica de probar a ojo.

Entre el día 15/08 y el 18/08 he podido hacer los siguientes avances:
He estado intentando desarollar una hardware interface personalizada para poder controlar mi robot ya diseñado virtualmente usando ros2 control pero ahora físicamente. Sin embargo, todos los documentos al respecto usan los motores a través de una arduino y usando el puerto serie, cosa que no me puedo plantear en mi proyecto. Partiendo de este repo, de los videos del canal de Articulated Robotics y de muchos tutoriales y foros he intentado fusionar los pines GPIO con la hardware interface pero no ha sido de forma satisfactoria. He subido al repo el paquete con la hardware interface que estaba intentando personalizar: incluye un COLCON_IGNORE para que a la hora de compilar no de problemas, lo he llamado ros2controlnw que significa ros2controlnotworking. Otra fuente de información que he usado para intentar conseguir esta parte es esta.

Por otro lado, he creado otro paquete pibotj_rr que ocntiene un nodo controlador inicializando los pines de los motores y permitiendo que se muevan en el robot real. En el gif de más abajo, se puede ver que funciona el motor izquierdo y dependiendo del valor entre 0 y 180, se mueve hacia adelante o hacia atrás. Mi paquete parte del siguiente repo

También, tras insistir mucho en intentar que detectase los pines GPIO, la única solución que encontré fue añadir dialout a groups

¡¡¡Importantísimo!!! que antes de ejecutar el código hay que inicializar el demonio:

sudo pigpiod

También había algunas cosas deprecated al ejecutar el código y he tenido que volver a la anterior versión:

pip install setuptools==58.2.0

Lo he conseguido solventar gracias al siguiente tutorial

En el día 19/08 he podido hacer los siguientes avances:

He conseguido configurar el puerto serie y he creado dentro del paquete pibotj_rr un gps_node que me permite obtener los datos del puerto serie y parsear los datos hasta obtener la latitud y longitud. ¡¡¡Importante recordar!!! El gps no está funcionando hasta que la luz azul no parpadea.

si haces un:

ros2 topic echo \gps_node

podrás ver los valores de latitud y longitud. Aquí puedes ver la demostración:

Si ves como paquete "servomotors" es un antiguo nombre que tenía de prueba, ahora es pibotj_rr

Los valores de latitud y longitud son correctos

En el día 22/08 he podido hacer los siguientes avances:

He conseguido finalmente crear una interfaz web donde poder ver la posición del robot en un mapa. Para ello, he tenido que seguir los siguientes pasos:

He descubierto que en ros existe rosbridge_server y trata de dar comunicación bidireccional entre clientes (buscadores web) y servidores usando una conexión Websocket. Para poder usarlo, primero hay que instalarlo y ejecutarlo en mi Raspberry:

sudo apt-get install ros-humble-rosbridge-suite
ros2 launch rosbridge_server rosbridge_websocket_launch.xml

Para comprobar que escucha bien en el puerto 9090 rosbridge:

sudo lsof -i TCP:9090 | grep LISTEN

En otra terminal de la Raspberry , abre la ejecución del publicador del nodo gps:

source ./install/setup.bash
ros2 run pibotj_rr gps_node

En otra terminal de la Raspberry , crea un fichero .xml como este y ejecútalo en el directorio donde se encuentre (evita que sea dentro de ./src!!) de la siguiente forma:

python3 -m http.server 8000

En una nueva ventana del navegador insertar lo mismo que la imagen de abajo, cambiando la ip de tu raspberry.

Resultado final:

• pibotj_r2c para el robot simulado

• pibotj_rr para el robot real

En pibotj_rr he ido creando una serie de nodos con sus respectivos publicadores y suscriptores:

• \pothole_detection_node -> contiene información de la cámara y de la IA entrenada

  • topic_cameratf_image: publica mensajes de tipo Image para poder visualizar la cámara con la IA entrenada y el filtro de imagen. Usar: rqt_image_view para verlo

  • topic_tf_detected: publica mensajes de tipo String que muestran si se ha detectado los baches o no usando un mensaje de "Yes" o "No"

• \motors_controller_node -> contiene el funcionamiento de los motores. Se suscribe al topic \topic_tf_detected y mueve los motores en función si detecta bache o no

• posibles aplicaciones

https://www.youtube.com/watch?v=MvXZLoQRL9E

https://www.youtube.com/watch?v=IX_b-e52dGg

• ssh usuario@ip_maquina
• Para copiar los paquetes e ir actualizando este repo: scp -r