多ros转mqtt

思路

基于paho包开发，支持多个话题输入，但需对应输入话题类型和需要的ros queue长度

参考

Python paho-mqtt使用心得 - 大叔明 - 博客园

isaac_mission_client的ros转mqtt包

调试

launch-》ros to mqtt node-》mqttx工具

代码

node

# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
# Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# SPDX-License-Identifier: Apache-2.0

"""Implementation for RosToMqttNode."""
import json
import socket
import time

from isaac_ros_mqtt_bridge.MqttBridgeUtils import convert_dict_keys
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan

import paho.mqtt.client as mqtt
import rclpy
from rclpy.node import Node

from rosbridge_library.internal import message_conversion
from rosbridge_library.internal import ros_loader


class RosToMqttNode(Node):
    """
    Bridge node that converts ROS messages to MQTT messages.

    Bridge node that subscribes to a ROS topic, translates the received message to a JSON,
    and publishes it to a MQTT channel.
    """

    def __init__(self, name='multy_ros_to_mqtt_bridge_node'):
        """Construct the RosToMqttBridge."""
        super().__init__(name)
        self.declare_parameters(
            namespace='',
            parameters=[
                ('interface_name', 'uagv'),
                ('major_version', 'v2'),
                ('manufacturer', 'RobotCompany'),
                ('serial_number', 'carter01'),
                ('mqtt_client_name', 'RosToMqttBridge'),
                ('mqtt_host_name', '192.168.9.97'),
                ('mqtt_port', 1883),
                ('mqtt_transport', 'tcp'),
                ('mqtt_ws_path', ''),
                ('mqtt_keep_alive', 100),
                # ('ros_subscriber_type', 'vda5050_msgs/NodeState'),
                # ('ros_subscriber_type', 'sensor_msgs/msg/LaserScan'),
                # ('ros_subscriber_queue', 10),
                ('convert_snake_to_camel', True),
                ('reconnect_period', 5),
                ('retry_forever', False),
                ('num_retries', 10),

                ('ros_subscriber_types', ['sensor_msgs/msg/LaserScan','std_msgs/msg/String']),
                ('ros_sub_topics', ['/scan','/test_string']),
                ('ros_subscriber_queues', [1,10]),
            ]
        )

        self.mqtt_client = mqtt.Client(
            self.get_parameter('mqtt_client_name').value,
            transport=self.get_parameter('mqtt_transport').value)
        self.mqtt_client.username_pw_set(username="admin", password="Softw0rk")
        
        if self.get_parameter('mqtt_transport').value == 'websockets' and \
                self.get_parameter('mqtt_ws_path').value != '':
            self.mqtt_client.ws_set_options(path=self.get_parameter('mqtt_ws_path').value)

        self.interface_name = self.get_parameter('interface_name').value
        self.major_version = self.get_parameter('major_version').value
        self.manufacturer = self.get_parameter('manufacturer').value
        self.serial_number = self.get_parameter('serial_number').value
        self.mqtt_topic_prefix = \
            f'{self.interface_name}/{self.major_version}/{self.manufacturer}/{self.serial_number}'

        def on_mqtt_connect(client, userdata, flags, rc):
            if rc == 0:
                self.get_logger().info(f"MQTT connected successfully! rc:{rc}")
            else:   
                self.get_logger().error(f"Failed to connect to MQTT: result code {rc}")

        def on_mqtt_disconnect(client, userdata, rc):
            if rc != 0:
                self.mqtt_client.loop_start()

                self.get_logger().error(f'Disconnected with result code {str(rc)}')

        self.mqtt_client.on_connect = on_mqtt_connect
        self.mqtt_client.on_disconnect = on_mqtt_disconnect

        # print("test1")
        
        self.my_subscriptions = []
        self.callbacks = {}  # 存储动态生成的回调函数
        self.generate_callbacks(self.get_parameter('ros_sub_topics').value) #  生成回调函数
        for i in range(len(self.get_parameter('ros_subscriber_types').value)):
            # print("test55")
            temp_sub = self.create_subscription(
                ros_loader.get_message_class(
                    self.get_parameter('ros_subscriber_types').value[i]),
                self.get_parameter('ros_sub_topics').value[i], 
                self.callbacks[self.get_parameter('ros_sub_topics').value[i]],
                # self.callbacks[self.get_parameter('ros_sub_topics').value[i]],
                self.get_parameter('ros_subscriber_queues').value[i])
            self.my_subscriptions.append(temp_sub)
        # -------------------------------
        # print("test6")

        """
            self.subscription = self.create_subscription(
            # ros_loader.get_message_class(
            #     self.get_parameter('ros_subscriber_type').value),
            # 'ros_sub_topic', self.__ros_subscriber_callback,
            # String,
            # '/test_string', self.__ros_subscriber_callback,
            LaserScan,
            '/scan', self.__ros_subscriber_callback,
            self.get_parameter('ros_subscriber_queue').value)
        """
        
        max_retries = self.get_parameter('num_retries').value
        retries = 0
        connected = False
        retry_forever = self.get_parameter('retry_forever').value
        while retries < max_retries or retry_forever:
            try:
                self.get_logger().info(f'try....{retries}')

                self.mqtt_client.connect(
                    self.get_parameter('mqtt_host_name').value,
                    self.get_parameter('mqtt_port').value,
                    self.get_parameter('mqtt_keep_alive').value)
                connected = True
                self.get_logger().info(f'connected....')
                break
            except ConnectionRefusedError as e:
                self.get_logger().error(f'Connection Error: {e}. Please check the mqtt_host_name.')
                time.sleep(self.get_parameter('reconnect_period').value)
                retries += 1
            except socket.timeout as e:
                self.get_logger().error(f'Connection Error: {e}. Please check the mqtt_host_name'
                                        ' and make sure it is reachable.')
                time.sleep(self.get_parameter('reconnect_period').value)
                retries += 1
            except socket.gaierror as e:
                self.get_logger().error(f'Connection Error: {e}. Could not resolve mqtt_host_name')
                time.sleep(self.get_parameter('reconnect_period').value)
                retries += 1
        if connected:
            self.mqtt_client.loop_start()
            self.get_logger().error(f'loop_start')

        else:
            self.get_logger().error('Failed to connect to MQTT broker, ending retries.')
    def generate_callbacks(self, ros_sub_topics):
        # for value in ros_sub_topics:
        for i in range(len(ros_sub_topics)):
            # 定义动态回调函数
            def callback_function(msg, value=ros_sub_topics[i]):
                try:
                    extracted = message_conversion.extract_values(msg)
                    msg_class = ros_loader.get_message_class(self.get_parameter('ros_subscriber_types').value[i])
                    value = value.replace("/", "_")
                    class_name = str(msg_class).replace('\'', '')
                    # self.get_logger().info(f"Message class name : {class_name}")
                    self.get_logger().info(f"publish mqtt:{self.mqtt_topic_prefix}/{value}")
                    if self.get_parameter('convert_snake_to_camel').value:
                        if class_name == '<class vda5050_msgs.msg._agv_state.AGVState>':
                            # self.get_logger().info(f"get agv state class")
                            value = "agv_state"
                            self.mqtt_client.publish(
                                f'{self.mqtt_topic_prefix}/{value}',
                                json.dumps(convert_dict_keys(extracted, 'snake_to_dromedary')))
                        else:
                            self.mqtt_client.publish(
                                f'{self.mqtt_topic_prefix}/{value}',
                                json.dumps(extracted))

                except (message_conversion.FieldTypeMismatchException, 
                        json.decoder.JSONDecodeError) as e:
                    self.get_logger().info(repr(e))

            # 存储到实例的回调字典中
            self.callbacks[ros_sub_topics[i]] = callback_function

def main(args=None):
    """Execute the RosToMqttNode."""
    rclpy.init(args=args)
    node = RosToMqttNode('multy_ros_to_mqtt_bridge_node')
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

launch

略