From 1ccafa4982590eaa794ad24792aa7da602507183 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Tue, 16 Apr 2024 12:19:06 -0400 Subject: [PATCH] util works --- config/pub.yaml | 5 +++++ config/sub.yaml | 0 launch/demo_pub.py | 38 ++++++++++++++++++++++++++++++++++ launch/demo_sub.py | 26 +++++++++++++++++++++++ mqtt_ros_bridge/bridge_node.py | 19 ++++++++++------- mqtt_ros_bridge/serializer.py | 7 +++---- mqtt_ros_bridge/util.py | 11 ++++++++++ setup.py | 5 ++++- 8 files changed, 98 insertions(+), 13 deletions(-) create mode 100644 config/pub.yaml create mode 100644 config/sub.yaml create mode 100644 launch/demo_pub.py create mode 100644 launch/demo_sub.py create mode 100644 mqtt_ros_bridge/util.py diff --git a/config/pub.yaml b/config/pub.yaml new file mode 100644 index 0000000..fb2d38b --- /dev/null +++ b/config/pub.yaml @@ -0,0 +1,5 @@ +demo_bridge_pub: + ros__parameters: + pub1: + topic: "hi" + diff --git a/config/sub.yaml b/config/sub.yaml new file mode 100644 index 0000000..e69de29 diff --git a/launch/demo_pub.py b/launch/demo_pub.py new file mode 100644 index 0000000..98241aa --- /dev/null +++ b/launch/demo_pub.py @@ -0,0 +1,38 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch.actions import SetEnvironmentVariable +from launch.launch_description import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """ + Generate LaunchDescription for MQTT ROS bridge. + + Returns + ------- + LaunchDescription + Launches bridge_node. + + """ + + config = os.path.join( + get_package_share_directory('mqtt_ros_bridge'), + 'config', + 'pub.yaml' + ) + + run_bridge_node = Node( + package='mqtt_ros_bridge', + executable='bridge_node', + name='demo_bridge_pub', + emulate_tty=True, + output='screen', + parameters=[config] + ) + + return LaunchDescription([ + SetEnvironmentVariable("ROS_DOMAIN_ID", "2"), + run_bridge_node + ]) diff --git a/launch/demo_sub.py b/launch/demo_sub.py new file mode 100644 index 0000000..4b61be3 --- /dev/null +++ b/launch/demo_sub.py @@ -0,0 +1,26 @@ +from launch.actions import SetEnvironmentVariable +from launch.launch_description import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """ + Generate LaunchDescription for MQTT ROS bridge. + + Returns + ------- + LaunchDescription + Launches bridge_node. + + """ + run_bridge_node = Node( + package='mqtt_ros_bridge', + executable='bridge_node', + emulate_tty=True, + output='screen' + ) + + return LaunchDescription([ + SetEnvironmentVariable("ROS_DOMAIN_ID", "1"), + run_bridge_node + ]) diff --git a/mqtt_ros_bridge/bridge_node.py b/mqtt_ros_bridge/bridge_node.py index 7270861..e4ea604 100644 --- a/mqtt_ros_bridge/bridge_node.py +++ b/mqtt_ros_bridge/bridge_node.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from typing import Any, Callable, Generic, Type +from typing import Any, Callable, Generic import paho.mqtt.client as MQTT import rclpy @@ -7,11 +7,11 @@ from rclpy._rclpy_pybind11 import RMWError from rclpy.node import Node from rclpy.publisher import Publisher -from rclpy.subscription import Subscription from std_msgs.msg import String from mqtt_ros_bridge.msg_typing import MsgLikeT from mqtt_ros_bridge.serializer import ROSDefaultSerializer, Serializer +from mqtt_ros_bridge.util import lookup_object @dataclass @@ -19,14 +19,15 @@ class TopicInfo(Generic[MsgLikeT]): """Metadata about a single topic.""" name: str - msg_type: Type[MsgLikeT] + msg_type: type[MsgLikeT] # Should Serializer also be generic across MsgLikeT? serializer: type[Serializer] publish_on_ros: bool TOPICS: dict[str, TopicInfo] = { - '/turtle1/cmd_vel': TopicInfo('/turtle1/cmd_vel', String, ROSDefaultSerializer, False), + '/turtle1/cmd_vel': TopicInfo('/turtle1/cmd_vel', lookup_object("std_msgs.msg:String"), + ROSDefaultSerializer, False), # 'sub_topic': TopicInfo('sub_topic', String, ROSDefaultSerializer, False) } @@ -40,6 +41,9 @@ class BridgeNode(Node): def __init__(self) -> None: super().__init__('mqtt_bridge_node') + # TODO get from parameters + DEBUG = True + self.get_logger().info('Creating MQTT ROS bridge node') self.mqtt_client = MQTT.Client() @@ -48,18 +52,17 @@ def __init__(self) -> None: self.mqtt_client.loop_start() self.ros_publishers: dict[str, Publisher] = {} - self.ros_subscriptions: list[Subscription] = [] for topic_info in TOPICS.values(): + print(topic_info.msg_type) if topic_info.publish_on_ros: publisher = self.create_publisher(topic_info.msg_type, topic_info.name, 10) self.ros_publishers[topic_info.name] = publisher self.mqtt_client.subscribe(topic_info.name) else: callback = self.make_ros_callback(topic_info) - subscription = self.create_subscription( - topic_info.msg_type, topic_info.name, callback, 10) - self.ros_subscriptions.append(subscription) + # TODO proper QOS? + self.create_subscription(topic_info.msg_type, topic_info.name, callback, 10) self.mqtt_client.on_message = self.mqtt_callback diff --git a/mqtt_ros_bridge/serializer.py b/mqtt_ros_bridge/serializer.py index 8ea107c..0800da0 100644 --- a/mqtt_ros_bridge/serializer.py +++ b/mqtt_ros_bridge/serializer.py @@ -1,5 +1,4 @@ from abc import ABC, abstractmethod -from typing import Type from rclpy.serialization import deserialize_message, serialize_message @@ -30,7 +29,7 @@ def serialize(message: MsgLike) -> bytes: @staticmethod @abstractmethod - def deserialize(serialized_message: bytes, message_type: Type[MsgLikeT]) -> MsgLikeT: + def deserialize(serialized_message: bytes, message_type: type[MsgLikeT]) -> MsgLikeT: """ Deserialize the provided bytes into a ROS message of the provided type. @@ -57,7 +56,7 @@ def serialize(message: MsgLike) -> bytes: return serialize_message(message) @staticmethod - def deserialize(serialized_message: bytes, message_type: Type[MsgLikeT]) -> MsgLikeT: + def deserialize(serialized_message: bytes, message_type: type[MsgLikeT]) -> MsgLikeT: return deserialize_message(serialized_message, message_type) @@ -69,5 +68,5 @@ def serialize(message: MsgLike) -> bytes: return json_serialize(message) @staticmethod - def deserialize(serialized_message: bytes, message_type: Type[MsgLikeT]) -> MsgLikeT: + def deserialize(serialized_message: bytes, message_type: type[MsgLikeT]) -> MsgLikeT: return json_deserialize(serialized_message, message_type) diff --git a/mqtt_ros_bridge/util.py b/mqtt_ros_bridge/util.py new file mode 100644 index 0000000..893171b --- /dev/null +++ b/mqtt_ros_bridge/util.py @@ -0,0 +1,11 @@ +from importlib import import_module + +from mqtt_ros_bridge.msg_typing import MsgLike + + +def lookup_object(object_path: str, package: str = 'mqtt_ros_bridge') -> type[MsgLike]: + """ lookup object from a some.module:object_name specification. """ + module_name, obj_name = object_path.split(":") + module = import_module(module_name, package) + obj = getattr(module, obj_name) + return obj diff --git a/setup.py b/setup.py index f01878a..7c3f120 100644 --- a/setup.py +++ b/setup.py @@ -15,7 +15,10 @@ ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. (os.path.join('share', PACKAGE_NAME, 'launch'), - glob('launch/*launch.[pxy][yma]*')) + glob('launch/*launch.[pxy][yma]*')), + # Include all config files. + (os.path.join('share', PACKAGE_NAME, 'config'), + glob('config/*')) ], install_requires=['setuptools'], zip_safe=True,