diff --git a/vda5050_connector/vda5050_connector_py/mqtt_bridge.py b/vda5050_connector/vda5050_connector_py/mqtt_bridge.py index 21eebf1..949780c 100755 --- a/vda5050_connector/vda5050_connector_py/mqtt_bridge.py +++ b/vda5050_connector/vda5050_connector_py/mqtt_bridge.py @@ -49,7 +49,7 @@ from vda5050_connector_py.utils import convert_ros_message_to_json from vda5050_connector_py.utils import get_vda5050_ts -from vda5050_connector_py.vda5050_controller import DEFAULT_PROTOCOL_VERSION +from vda5050_connector_py.vda5050_controller import DEFAULT_PROTOCOL_VERSION, SUPPORTED_PROTOCOL_VERSIONS # ROS msgs / srvs / actions from vda5050_msgs.msg import Action as VDAAction @@ -194,6 +194,32 @@ def generate_vda_instant_action_msg(instant_action): return vda_instant_action +def generate_vda5050_topic_alias(vda_version): + """ + Create an alias for the current vda5050 version. The aliases are needed to + create the mqtt topics. + + Args: + ---- + vda_version (string): VDA5050 version with format x.x.x. + + Raises: + ------ + ValueError if the alias is not within the supported values. + + Returns + ------- + The alias of the version. For example, for the version '2.0.0', the alias is + 'v2' + """ + if vda_version in SUPPORTED_PROTOCOL_VERSIONS: + return f"v{vda_version[0]}" + else: + raise ValueError( + f"Invalid protocol major version. Supported versions are: {SUPPORTED_PROTOCOL_VERSIONS}," + f"but got {vda_version}" + ) + class MQTTBridge(Node): """Translates VDA5050 MQTT messages from and to ROS2.""" @@ -209,6 +235,9 @@ def __init__(self): mqtt_username = read_str_parameter(self, "mqtt_username", "") mqtt_password = read_str_parameter(self, "mqtt_password", "") + self.vda5050_version = read_str_parameter(self, "vda5050_protocol_version", "2.0.0") + self.vda5050_version_alias = generate_vda5050_topic_alias(self.vda5050_version) + self._manufacturer_name = read_str_parameter( self, "manufacturer_name", "robots" ) @@ -238,6 +267,7 @@ def __init__(self): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="connection", + major_version=self.vda5050_version_alias, ) # NOTE: will payload cannot be set dynamically or updated @@ -247,7 +277,7 @@ def __init__(self): will_payload = convert_ros_message_to_json( VDAConnection( header_id=0, - version=DEFAULT_PROTOCOL_VERSION, + version=self.vda5050_version, timestamp="1970-01-01T12:00:00.00Z", manufacturer=self._manufacturer_name, serial_number=self._serial_number, @@ -278,6 +308,7 @@ def on_connect_mqtt(self, client, userdata, flags, rc): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="order", + major_version=self.vda5050_version_alias, ) ) self.mqtt_client.subscribe( @@ -285,12 +316,13 @@ def on_connect_mqtt(self, client, userdata, flags, rc): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="instantActions", + major_version=self.vda5050_version_alias ) ) self._publish_connection( msg=VDAConnection( header_id=0, - version=DEFAULT_PROTOCOL_VERSION, + version=self.vda5050_version, timestamp=get_vda5050_ts(), manufacturer=self._manufacturer_name, serial_number=self._serial_number, @@ -405,7 +437,7 @@ def on_shutdown(self): offline_message = VDAConnection( header_id=0, - version=DEFAULT_PROTOCOL_VERSION, + version=self.vda5050_version, timestamp=get_vda5050_ts(), manufacturer=self._manufacturer_name, serial_number=self._serial_number, @@ -424,6 +456,7 @@ def on_shutdown(self): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="order", + major_version=self.vda5050_version_alias ) ) self.mqtt_client.unsubscribe( @@ -431,6 +464,7 @@ def on_shutdown(self): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="instantActions", + major_version=self.vda5050_version_alias ) ) @@ -463,6 +497,7 @@ def _publish_state(self, msg: VDAOrderState): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="state", + major_version=self.vda5050_version_alias ) self._publish_to_topic(msg, topic) @@ -485,6 +520,7 @@ def _publish_connection(self, msg: VDAConnection): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="connection", + major_version=self.vda5050_version_alias ) self._publish_to_topic(msg, topic) @@ -501,5 +537,6 @@ def _publish_visualization(self, msg: VDAVisualization): manufacturer=self._manufacturer_name, serial_number=self._serial_number, topic="visualization", + major_version=self.vda5050_version_alias ) self._publish_to_topic(msg, topic) diff --git a/vda5050_connector/vda5050_connector_py/utils.py b/vda5050_connector/vda5050_connector_py/utils.py index e1611e6..430643c 100644 --- a/vda5050_connector/vda5050_connector_py/utils.py +++ b/vda5050_connector/vda5050_connector_py/utils.py @@ -221,7 +221,7 @@ def convert_ros_message_to_json(msg): def get_vda5050_mqtt_topic( - manufacturer, serial_number, topic, interface_name="uagv", major_version="v1" + manufacturer, serial_number, topic, major_version, interface_name="uagv", ): """ Return suggested VDA5050 MQTT topics. diff --git a/vda5050_connector/vda5050_connector_py/vda5050_controller.py b/vda5050_connector/vda5050_connector_py/vda5050_controller.py index 16424b4..b1ec729 100755 --- a/vda5050_connector/vda5050_connector_py/vda5050_controller.py +++ b/vda5050_connector/vda5050_connector_py/vda5050_controller.py @@ -95,6 +95,7 @@ DEFAULT_SERIAL_NUMBER = "robot_1" DEFAULT_PROTOCOL_VERSION = "2.0.0" DEFAULT_STARTING_NODE_ID = "" +SUPPORTED_PROTOCOL_VERSIONS = ["1.1.0", "2.0.0"] DEFAULT_GET_STATE_SVC_NAME = "adapter/get_state" DEFAULT_SUPPORTED_ACTIONS_SVC_NAME = "adapter/supported_actions"