11import os
22import sys
33from typing import Any , Callable , Generic , cast
4+ # from queue import queue
5+ from enum import Enum
46
57import paho .mqtt .client as MQTT
68import rclpy
1517from mqtt_ros_bridge .util import lookup_message
1618
1719
20+ class InteractionType (Enum ):
21+ ROS_PUBLISHER = 1
22+ ROS_SUBSCRIBER = 2
23+ ROS_CLIENT = 3
24+ ROS_SERVER = 4
25+
26+
27+ INTERACTION_TYPES : dict [str , InteractionType ] = {
28+ 'ros_publisher' : InteractionType .ROS_PUBLISHER ,
29+ 'ros_subscriber' : InteractionType .ROS_SUBSCRIBER ,
30+ 'ros_client' : InteractionType .ROS_CLIENT ,
31+ 'ros_server' : InteractionType .ROS_SERVER
32+ }
33+
34+
1835class TopicMsgInfo (Generic [MsgLikeT ]):
1936 """Metadata about a single topic."""
2037
21- def __init__ (self , topic : str , msg_object_path : str ,
22- publish_on_ros : bool , use_ros_serializer : bool = True ) -> None :
38+ def __init__ (self , topic : str , msg_object_path : str , interaction_type : InteractionType ,
39+ use_ros_serializer : bool = True ) -> None :
2340
2441 self .topic = topic
2542 self .msg_type : MsgLikeT = cast (MsgLikeT , lookup_message (msg_object_path ))
26- self .publish_on_ros = publish_on_ros
43+ self .interaction_type = interaction_type
2744 self .serializer = ROSDefaultSerializer if use_ros_serializer else JSONSerializer
2845
2946 def __str__ (self ) -> str :
30- return (f"Topic: { self .topic } , Message Type: { self .msg_type } , Publish on ROS: "
31- f"{ self .publish_on_ros } , Serializer: { self .serializer } " )
47+ return (f"Topic: { self .topic } , Message Type: { self .msg_type } , Type: "
48+ f"{ self .interaction_type } , Serializer: { self .serializer } " )
3249
3350
3451MQTT_PORT = 1883
3552MQTT_KEEPALIVE = 60
3653
3754PARAMETER_TOPIC = "topic"
3855PARAMETER_TYPE = "type"
39- PARAMETER_PUBLISH_ON_ROS = "publish_on_ros "
56+ PARAMETER_INTERACTION_TYPE = "interaction_type "
4057PARAMETER_USE_ROS_SERIALIZER = "use_ros_serializer"
4158
4259
@@ -57,7 +74,8 @@ def __init__(self, args: list[str]) -> None:
5774
5875 self .topics = self .topic_info_from_parameters (args [1 ])
5976
60- self .get_logger ().info (str (self .topics ))
77+ for topic , topic_info in self .topics .items ():
78+ print (f'{ topic } : { str (topic_info )} ' )
6179
6280 self .get_logger ().info ('Creating MQTT ROS bridge node' )
6381
@@ -69,7 +87,7 @@ def __init__(self, args: list[str]) -> None:
6987 self .ros_publishers : dict [str , Publisher ] = {}
7088
7189 for topic_info in self .topics .values ():
72- if topic_info .publish_on_ros :
90+ if topic_info .interaction_type == InteractionType . ROS_PUBLISHER :
7391 publisher = self .create_publisher (topic_info .msg_type , topic_info .topic , 10 )
7492 self .ros_publishers [topic_info .topic ] = publisher
7593 self .mqtt_client .subscribe (topic_info .topic )
@@ -102,12 +120,16 @@ def topic_info_from_parameters(self, config: str) -> dict[str, TopicMsgInfo]:
102120 else :
103121 ros_serialiser = False
104122
105- topic_infos [params [f"{ name } .{ PARAMETER_TOPIC } " ].value ] = (TopicMsgInfo (
123+ interaction_type_str = params [f"{ name } .{ PARAMETER_INTERACTION_TYPE } " ].value
124+ if interaction_type_str not in INTERACTION_TYPES .keys ():
125+ raise ValueError (f'Interaction types must be one of { INTERACTION_TYPES .keys ()} ' )
126+
127+ topic_infos [params [f"{ name } .{ PARAMETER_TOPIC } " ].value ] = TopicMsgInfo (
106128 params [f"{ name } .{ PARAMETER_TOPIC } " ].value ,
107129 params [f"{ name } .{ PARAMETER_TYPE } " ].value ,
108- params [ f" { name } . { PARAMETER_PUBLISH_ON_ROS } " ]. value ,
130+ INTERACTION_TYPES [ interaction_type_str ] ,
109131 ros_serialiser
110- ))
132+ )
111133
112134 return topic_infos
113135
0 commit comments