Skip to content

Parte 6: Node de Detección de Objetos

El Código

Copia todo el código a continuación en tu archivo object_detection.py, ¡y asegúrate de leer las anotaciones!

object_detection.py
#!/usr/bin/env python3

import rclpy 
from rclpy.node import Node # (1)!

import cv2
from cv_bridge import CvBridge, CvBridgeError # (2)!

from sensor_msgs.msg import Image # (3)!

from pathlib import Path # (4)!

class ObjectDetection(Node):

    def __init__(self): # (5)!
        super().__init__("object_detection")

        self.camera_sub = self.create_subscription(
            msg_type=Image,
            topic="/camera/image_raw",
            callback=self.camera_callback,
            qos_profile=10
        )

        self.waiting_for_image = True # (6)!

    def camera_callback(self, img_data): # (7)!
        cvbridge_interface = CvBridge() # (8)!
        try:
            cv_img = cvbridge_interface.imgmsg_to_cv2(
                img_data, desired_encoding="bgr8"
            ) # (9)!
        except CvBridgeError as e:
            self.get_logger().warning(f"{e}")

        if self.waiting_for_image: # (10)!
            height, width, channels = cv_img.shape

            self.get_logger().info(
                f"Obtained an image of height {height}px and width {width}px."
            )

            self.show_image(img=cv_img, img_name="step1_original")

            self.waiting_for_image = False # (15)!
            cv2.destroyAllWindows() # (16)!

    def show_image(self, img, img_name, save_img=True): # (11)!

        self.get_logger().info(f"Opening {img_name}...")
        cv2.imshow(img_name, img) # (12)!

        cv2.waitKey(4000) # (14)!

        if save_img: # (13)!
            self.save_image(img, img_name)

    def save_image(self, img, img_name): # (17)!
        self.get_logger().info(f"Saving the image...")

        base_image_path = Path.home().joinpath("object_detection")
        base_image_path.mkdir(parents=True, exist_ok=True) # (18)!
        full_image_path = base_image_path.joinpath(
            f"{img_name}.jpg") # (19)!

        cv2.imwrite(str(full_image_path), img) # (20)!

        self.get_logger().info(
            f"\nSaved an image to '{full_image_path}'\n"
            f"  - image dims: {img.shape[0]}x{img.shape[1]}px\n"
            f"  - file size: {full_image_path.stat().st_size} bytes"
        ) # (21)!

def main(args=None):
    rclpy.init(args=args)
    node = ObjectDetection()
    while node.waiting_for_image:
        rclpy.spin_once(node) # (22)!
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

  1. Nada nuevo aquí, continuamos...

  2. Estamos importando la biblioteca OpenCV para Python (recuerda la API de Python de la que hablamos antes), que se llama cv2, y también esa interface puente entre ROS y OpenCV de la que hablamos antes: cv_bridge.

    De cv_bridge estamos importando las clases CvBridge y CvBridgeError de la biblioteca cv_bridge específicamente.

  3. Necesitamos subscribirnos a un topic de imagen para obtener los datos que se publican en él. Ya deberías haber identificado el tipo de interface que se publica en el topic /camera/image_raw, así que importamos ese tipo de interface aquí (del package sensor_msgs) para poder construir un subscriber al topic más adelante.

  4. También estamos importando la clase Python Path del módulo pathlib. Una herramienta muy útil para realizar operaciones de archivos.

  5. Inicializando nuestra Clase ObjectDetection() (debería serte muy familiar a estas alturas):

    1. Dando un nombre a nuestro node.
    2. Creando un subscriber al topic /camera/image_raw, proporcionando el tipo de interface utilizado por el topic (sensor_msgs/msg/Image, como se importó anteriormente), y apuntándolo a una función callback (camera_callback, en este caso), para definir los procesos que deben realizarse cada vez que se obtiene un mensaje en este topic (en este caso, los mensajes serán nuestras imágenes de cámara)

  6. Estamos creando una bandera para indicar si el node ha obtenido una imagen todavía o no. Para este ejercicio, solo queremos obtener una única imagen, así que estableceremos la bandera waiting_for_image en False una vez que se haya obtenido y procesado una imagen, para evitar capturar más.

    Esta bandera se usará luego para cerrar el node cuando haya terminado su trabajo.

  7. Aquí estamos definiendo una función callback para nuestro subscriber self.camera_sub...

  8. Aquí creamos una instancia de la clase CvBridge que importamos anteriormente, y que usaremos más adelante para convertir datos de imagen ROS a un formato que OpenCV pueda entender.

  9. Estamos usando la interface CvBridge para tomar nuestros datos de imagen ROS y convertirlos a un formato que OpenCV pueda entender. En este caso especificamos la conversión (o "encoding") a un formato de imagen BGR (Azul-Verde-Rojo) de 8 bits: "bgr8".

    Sin embargo, contenemos esto dentro de un bloque try-except, que es el procedimiento recomendado al hacer esto. Aquí intentamos convertir una imagen usando el encoding deseado, y si se genera un CvBridgeError entonces imprimimos un mensaje de advertencia en el terminal. Si esto sucede, esta ejecución particular de la función callback de la cámara se detendrá.

  10. Luego verificamos la bandera waiting_for_image para ver si esta es la primera imagen recibida por el node. Si es así, entonces:

    1. Obtenemos la altura y el ancho de la imagen (en píxeles), así como el número de canales de color.
    2. Imprimimos un mensaje de registro con las dimensiones de la imagen.
    3. Pasamos los datos de la imagen a la función show_image() (definida a continuación). También pasamos un nombre descriptivo para la imagen a esta función (img_name).

  11. Este método de clase nos presenta la imagen en una ventana emergente y también llama a otro método que guarda la imagen en un archivo para nosotros.

  12. Muestra la imagen real en una ventana emergente:

    1. Los datos de la imagen se pasan a la función a través del argumento img,
    2. Necesitamos darle un nombre a la ventana emergente, así que en este caso usamos el argumento img_name que se pasa a este método de clase.

  13. El método de clase show_image() tiene un argumento save_img, que está establecido en True por defecto, de modo que esta condición if se activa, y se llama a otro método de clase para guardar la imagen en un archivo.

  14. Proporcionamos un valor de 4000 aquí, lo que le dice a esta función que mantenga esta ventana abierta durante 4000 milisegundos (4 segundos) antes de cerrarla nuevamente.

    Una vez que la ventana se haya cerrado, la ejecución de nuestro código puede continuar...

  15. Luego establecemos la bandera waiting_for_image en False para que estas etapas de procesamiento solo se realicen una vez (solo queremos capturar una única imagen). Esto activará el bucle while principal para que se detenga (ver abajo), causando que la ejecución general del node se detenga también.

  16. cv2.destroyAllWindows() garantiza que cualquier ventana emergente de imagen OpenCV que pueda seguir activa o en memoria sea destruida antes de que el método de clase salga (y el node se cierre).

  17. Este método de clase maneja el guardado de la imagen en un archivo usando herramientas de cv2 y pathlib.

  18. Aquí definimos una ubicación del sistema de archivos para guardar imágenes.

    Queremos que exista en una carpeta llamada "object_detection" en el directorio de inicio, así que podemos usar Path.home().joinpath(...) de Pathlib para definirla (una forma práctica de acceder al directorio de inicio del Usuario, sin necesidad de conocer el nombre del Usuario).

    Luego usamos el método Path.mkdir() de Pathlib para crear este directorio si no existe todavía.

  19. Aquí se construye una ruta completa de archivo para la imagen (usando el método Path.joinpath()), basada en:

    1. El base_image_path que definimos anteriormente
    2. Un nombre de imagen que se pasa a este método de clase a través del argumento img_name.

  20. Esto guarda la imagen en un archivo .jpg. Proporcionamos la full_image_path que se creó anteriormente, y también los datos reales de la imagen (self.cv_img) para que la función sepa qué imagen queremos guardar.

  21. Estamos imprimiendo un mensaje de registro en el terminal para informarnos de:

    1. Dónde se ha guardado la imagen
    2. Qué tan grande es la imagen (en términos de sus dimensiones en píxeles)
    3. Qué tan grande es el archivo de imagen (en bytes).

  22. Estamos usando spin_once() dentro de un bucle while aquí para poder vigilar el valor de la bandera wait_for_image, y dejar de hacer spin (es decir, salir del bucle while) una vez que se vuelve False.

Dependencias

Modifica tu archivo package.xml para acomodar las diversas dependencias del node object_detection.py:

package.xml
<exec_depend>opencv2</exec_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>sensor_msgs</exec_depend>