ros与mqtt相互转换

vda5050

VDA5050协议介绍和详细翻译-CSDN博客

ros与mqtt相互转换

如何转换的，通过某个中转包，获取ros的消息然后以需要的格式转换为mqtt

需要的参数

ros相关

parameters=[
('ros_subscriber_type', 'vda5050_msgs/NodeState'),
('ros_subscriber_queue', 1),
]

mqtt相关

parameters=[
('interface_name', 'uagv'),
('major_version', 'v2'),
('manufacturer', 'RobotCompany'),
('serial_number', 'carter01'),
('mqtt_client_name', 'RosToMqttBridge'),
('mqtt_host_name', 'localhost'),
('mqtt_port', 1883),
('mqtt_transport', 'tcp'),
('mqtt_ws_path', ''),
('mqtt_keep_alive', 60),
('convert_snake_to_camel', True),
('reconnect_period', 5),
('retry_forever', False),
('num_retries', 10)
]

ros需要的内容

订阅了需要转换为mqtt的ros订阅者

self.subscription = self.create_subscription(
            ros_loader.get_message_class(
                self.get_parameter('ros_subscriber_type').value),
            'ros_sub_topic', self.__ros_subscriber_callback,
            self.get_parameter('ros_subscriber_queue').value)

mqtt需要的内容

mqtt客户端

self.mqtt_client = mqtt.Client(
            self.get_parameter('mqtt_client_name').value,
            transport=self.get_parameter('mqtt_transport').value)

mqtt配置项

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}'

mqtt状态检查

基于次数、或bool值，结合try-except来实现对mqtt_host_name、mqtt_port、mqtt_keep_alive的检查

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.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
                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()
        else:
            self.get_logger().error('Failed to connect to MQTT broker, ending retries.')

ros话题转换mqtt

需要使用的包

from rosbridge_library.internal import message_conversion
import json

使用message_conversion提取话题数据，利用json将提取的数据转换并发布到mqtt客户端中。

 def __ros_subscriber_callback(self, msg):
        try:
            extracted = message_conversion.extract_values(msg)
            if self.get_parameter('convert_snake_to_camel').value:
                self.mqtt_client.publish(
                    f'{self.mqtt_topic_prefix}/state',
                    json.dumps(convert_dict_keys(extracted, 'snake_to_dromedary')))
            else:
                self.mqtt_client.publish(f'{self.mqtt_topic_prefix}/state', json.dumps(extracted))
        except (message_conversion.FieldTypeMismatchException,
                json.decoder.JSONDecodeError) as e:
            self.get_logger().info(repr(e))

mqtt转换ros

使用包

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

用ros_loader.get_message_instance创建ros消息

接收mqtt的msg，由json加载为ros消息键值，再用message_conversion将ros消息键值对转换为ros_msg并利用发布者发布。

def on_mqtt_message(client, userdata, msg):
            try:
                publisher = None
                self.get_logger().info(f'From {msg.topic}: {str(msg.payload)}')
                if msg.topic.endswith('order'):
                    ros_msg = ros_loader.get_message_instance(
                        self.get_parameter('ros_publisher_type').value)
                    publisher = self.publisher
                if msg.topic.endswith('instantActions'):
                    ros_msg = ros_loader.get_message_instance('vda5050_msgs/InstantActions')
                    publisher = self.instant_actions_publisher

                if self.get_parameter('convert_camel_to_snake').value:
                    message_dict = json.loads(str(msg.payload, 'utf-8'))
                    converted_message_dict = convert_dict_keys(
                        message_dict, 'camel_to_snake')
                    message_conversion.populate_instance(
                        converted_message_dict, ros_msg)
                else:
                    message_conversion.populate_instance(
                        json.loads(str(msg.payload, 'utf-8')), ros_msg)
                if publisher:
                    publisher.publish(ros_msg)
            except (message_conversion.FieldTypeMismatchException,
                    json.decoder.JSONDecodeError) as e:
                self.get_logger().info(repr(e))
                error_msg = String()
                error_msg.data = repr(e)
                self.error_publisher.publish(error_msg)