es.davy.ai

Preguntas y respuestas de programación confiables

¿Tienes una pregunta?

Si tienes alguna pregunta, puedes hacerla a continuación o ingresar lo que estás buscando.

Cómo enviar mensajes ros2 desde un servidor websocket a clientes conectados en tornado

29 Jul, 2023 Programación

Tengo un script de publicador ros2 que envía mensajes personalizados desde nodos ros2. Lo que necesito hacer es tener un suscriptor (que también es mi servidor de websocket) para escuchar el mensaje que envía el publicador, luego convertirlo a un diccionario y enviarlo como un json desde el servidor de websocket a un cliente de websocket conectado. Ya he revisado el repositorio de rosbridge pero no pude hacer que funcione. No tiene suficiente documentación y soy nuevo en ros.
Necesito algo como esto:

import rclpy
import sys
from rclpy.node import Node
import tornado.ioloop

import tornado.httpserver
import tornado.web
import threading

from custom.msg import CustomMsg

from .convert import message_to_ordereddict

wss = []

class wsHandler(tornado.websocket.WebSocketHandler):
    def open(self):
        print('Online')
        if self not in wss:
            wss.append(self)

    def on_close(self):
        print('Offline')
        if self in wss:
            wss.remove(self)

    def wsSend(message):
        for ws in wss:
            ws.write_message(message)

class MinimalSubscriber(Node):
    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(CustomMsg, 'topic', self.CustomMsg_callback, 10)
        self.subscription

    def CustomMsg_callback(self, msg):
        ws_message = message_to_ordereddict(msg)
        wsSend(ws_message)

if __name__ == "__main__":
    http_server = tornado.httpserver.HTTPServer(tornado.web.Application(wsHandler))
    http_server.listen(8888)
    main_loop = tornado.ioloop.IOLoop.instance()
    # Start main loop
    main_loop.start()

Entonces, la función de devolución de llamada en la clase MinimalSubscriber recibe el mensaje ros, lo convierte en un diccionario y lo envía al cliente de websocket. Estoy un poco confundido acerca de cómo hacer que estos dos hilos (ros y websocket) se comuniquen entre sí.

Tags:  , , , ,

Answer

  1. Avatar for davy.ai
    davy.ai

    Para hacer que el script del publicador de ROS2 y el servidor de websockets se comuniquen entre sí, puedes usar las bibliotecas de cliente de ROS2 en Python, que pueden publicar o suscribirse a temas en tu red de ROS2. Luego, puedes pasar mensajes entre el suscriptor de ROS2 y el servidor de websockets convirtiéndolos a formatos compatibles. Esto se puede hacer utilizando la función message_to_ordereddict() de ROS2 para convertir los mensajes de ROS2 en un diccionario de Python y luego utilizando JSON para enviar el diccionario al cliente de websockets.

    Aquí hay un ejemplo de cómo puedes modificar el código que proporcionaste para implementar esto:

    import rclpy
import sys
from rclpy.node import Node
import tornado.ioloop

import tornado.httpserver
import tornado.web
import threading
import json

from custom.msg import CustomMsg

from rclpy.qos import QoSProfile
from rclpy.executors import MultiThreadedExecutor


wss = []
class wsHandler(tornado.websocket.WebSocketHandler):
    def open(self):
        print('Conectado')
        if self not in wss:
            wss.append(self)

    def on_close(self):
        print('Desconectado')
        if self in wss:
            wss.remove(self)

def wsSend(message):
    for ws in wss:
        ws.write_message(json.dumps(message))


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(CustomMsg, 'topic', self.CustomMsg_callback, QoSProfile(depth = 10))
        self.executors = MultiThreadedExecutor(num_threads=2)
        self.executors.add_node(self)

    def CustomMsg_callback(self, msg):
        ws_message = message_to_ordereddict(msg)
        wsSend(ws_message)



if __name__ == "__main__":
    rclpy.init(args=sys.argv)
    minimal_subscriber = MinimalSubscriber()
    executor = MultiThreadedExecutor()
    executor.add_node(minimal_subscriber)

    http_server = tornado.httpserver.HTTPServer(tornado.web.Application([(r"/", wsHandler)]))
    http_server.listen(8888)
    main_loop = tornado.ioloop.IOLoop.instance()

    # Iniciar los hilos de ROS2 y el servidor de websockets
    executor_thread = threading.Thread(target=executor.spin)
    executor_thread.start()
    websocket_thread = threading.Thread(target=main_loop.start)
    websocket_thread.start()

    # Unir los hilos al hilo principal
    executor_thread.join()
    websocket_thread.join()

    # Apagar ROS2
    minimal_subscriber.destroy_node()
    rclpy.shutdown()

    En el código anterior, hemos modificado la función wsSend() para convertir el diccionario de mensajes a JSON antes de enviarlo al cliente de websockets utilizando la función write_message(). También hemos agregado un QoSProfile al suscriptor de ROS2, que especifica el número máximo de mensajes que se pueden almacenar en búfer antes de que se descarten los mensajes antiguos.

    En la función main(), hemos inicializado ROS2 utilizando rclpy.init(), creado una instancia de la clase MinimalSubscriber y agregado el suscriptor a un MultiThreadedExecutor. También hemos creado un servidor de websockets utilizando la biblioteca Tornado y lo hemos iniciado en un hilo separado. Finalmente, hemos iniciado tanto los hilos de ROS2 como los del servidor de websockets y los hemos unido al hilo principal.

    Este código debería permitir que tu servidor de websockets escuche los mensajes de ROS2 y los envíe como JSON a los clientes conectados. Ten en cuenta que es posible que debas modificar el tema y los tipos de mensajes de ROS2 para que coincidan con los de tu propio código.

Comments are closed.