Semana 15. Módulo HAL (Láser, Cámara y Darknet ROS)

La semana pasada implementé en la API de HAL las funciones setV y setW en ROS 2. En realidad es una interfaz que conecta con el nodo publicador de la velocidad que usa el topic /cmd_vel. Consta de un Thread que está continuamente actualizando las publicaciones sobre dicho topic.

La dificultad de esta semana era implementar las funciones de HAL que internamente usan nodos suscriptores. En ROS, un nodo que contiene un suscriptor necesita llamar a spin() o spin_once() para actualizar las funciones de retrollamada (callback). ROS 2 incorpora una nueva funcionalidad en la ROS Client Library, los Ejecutores de Thread (ThreadedExecutor) que permiten al programador añadir nodos, como si se tratara de un set, y poder hacer spin de todos ellos concurrentemente.

De modo que adapté los Suscriptores del Láser, Cámara y Darknet Ros a ROS 2 por separado, siendo cada Suscriptor una clase que hereda de rclpy.Node, y usé en el fichero hal.py un MultiThreadedExecutor de la siguiente manera:

class HAL:
    def __init__(self):
        rclpy.init()

        self.motors = PublisherMotors("cmd_vel", 4, 0.3)
        self.laser = ListenerLaser("scan")
        self.camera = ListenerCamera("/depth_camera/image_raw")
        self.bounding_boxes = ListenerBoundingBoxes("/darknet_ros/bounding_boxes")
        
        self.listener_executor = MultiThreadedExecutor(num_threads=4)
        self.listener_executor.add_node(self.laser)
        self.listener_executor.add_node(self.camera)
        self.listener_executor.add_node(self.bounding_boxes)
        
        self.thread = ThreadHAL(self.listener_executor)
    
    def start_thread(self):
    	self.thread.start()
      
  ...
  ...

class ThreadHAL(threading.Thread):
    def __init__(self, executor):
        super(ThreadHAL, self).__init__()
        self.executor = executor

    def run(self):
    	try:
    	    self.executor.spin()
    	finally:
    	    self.executor.shutdown()

Así, los suscriptores funcionaron correctamente y de este modo, conseguí implementar las siguientes funciones de la API:

• HAL.getImage()
• HAL.getLaserData()
• HAL.getBoundingBoxes()

También redimensioné el tamaño de la ventana donde se visualizará los fotogramas de la cámara y probé el módulo GUI. En la siguiente imagen, muestro el resultado: