Delegate Robot Model¶
The delegate robot model is a disembodied supervisor robot which manages
communication between the local simulated radio network and a remote MQTT
server. The delegate assumes it is a singleton; no more than one delegate robot
should be added to a world.
It uses the supervisor functions to identify proxy robot bodies in the world, and then uses them to display the position and name of remote robots as reported via received MQTT messages.
This model is demonstrated in the network-party.wbt world.
delegate.proto¶
The robot model has been encapsulated in a .proto file for easy reuse.
1#VRML_SIM R2022a utf8
2# Disembodied supervisor robot which can communicate between the
3# local radio network and a remote MQTT server. It manages
4# the position of local proxy robots representing remote participants.
5# documentation url: https://courses.ideate.cmu.edu/16-375
6# license: No copyright, 2020-2022 Garth Zeglin. This file is explicitly placed in the public domain.
7PROTO delegate [
8 field SFVec3f translation 0 0 0
9 field SFRotation rotation 0 1 0 0
10 field SFString controller "disembod"
11 field SFString name "delegate"
12 field SFString customData "Wobbly"
13]
14{
15 Robot {
16 # connect properties to user-visible data fields
17 translation IS translation
18 rotation IS rotation
19 controller IS controller
20 name IS name
21 customData IS customData
22
23 # enable supervisor mode so this robot can directly query simulation state
24 # and configure proxy robots
25 supervisor TRUE
26
27 children [
28 # add a default radio receiver and transmitter
29 Receiver {
30 }
31 Emitter {
32 }
33 ]
34 }
35}
Sample Control Code¶
1# disembod.py
2#
3# Sample Webots controller file for a disembodied
4# supervisor robot which communicates between the
5# local radio network and a remote MQTT server.
6
7# No copyright, 2020-2021, Garth Zeglin. This file is
8# explicitly placed in the public domain.
9
10print("loading disembod.py...")
11
12################################################################
13# standard Python libraries
14import math, getpass
15
16# Import the Paho MQTT client library.
17# documentation: https://www.eclipse.org/paho/clients/python/docs/
18import paho.mqtt.client as mqtt
19
20# Import the Webots simulator API.
21from controller import Supervisor
22
23################################################################
24
25HOST = "mqtt.ideate.cmu.edu" # IDeATe MQTT server hostname
26PORT = 8885 # port of server instance for 16-375
27USER = "students" # Specific login for this server.
28PASSWORD = "<password>" # Specific password for this login.
29
30# Define the time step in milliseconds between controller updates.
31EVENT_LOOP_DT = 200
32
33################################################################
34class Disembod(Supervisor):
35
36 def __init__(self):
37 super(Disembod, self).__init__()
38
39 self.robot_name = self.getName()
40 print("%s: controller connected." % (self.robot_name))
41
42 # Connect to the radio emitter and receiver. The radio runs at the same
43 # rate as the event loop as it is the main function of this robot.
44 self.receiver = self.getDevice('receiver')
45 self.emitter = self.getDevice('emitter')
46 self.receiver.enable(EVENT_LOOP_DT)
47
48 # The custom data field is used to specify the name of the individual robot
49 # whose data is broadcast over MQTT.
50 self.public_robot = self.getCustomData().replace(" ","_")
51
52 # Create a subscription to receive all party messages. This will
53 # include our own transmissions, but these will be filtered after receipt.
54 self.subscription = 'party/#' # message topics to receive
55
56 # Create a default broadcast topic based on the current username.
57 # Please customize this as needed.
58 self.username = getpass.getuser()
59 self.send_topic = 'party/' + self.username # message topic to send
60
61 # Initialize the MQTT client system
62 self.mqtt_client = mqtt.Client()
63 self.mqtt_client.on_connect = self.on_connect
64 self.mqtt_client.on_message = self.on_message
65 self.mqtt_client.tls_set()
66 self.mqtt_client.username_pw_set(USER, PASSWORD)
67
68 # Initialize the list of available proxies. Each entry is a tuple with
69 # several field objects used to query and set its state.
70 self.proxies = []
71
72 # Initialize the mapping which allocates proxies to incoming robot names.
73 self.proxy_for_name = {}
74
75 # Locate proxy robots in the scene tree to use for displaying received data.
76 self.find_proxies()
77 print("%s: found %d proxies." % (self.robot_name, len(self.proxies)))
78
79 return
80
81 #================================================================
82 def find_proxies(self):
83 """Find the proxy robot bodies in the scene tree to use for displaying the
84 position of remote robots. This assumes the proxies were created using
85 the proto file and have a base type of proxy rather than Robot, i.e.,
86 were not expanded into base nodes."""
87
88 # fetch the root node of the scene tree, a Group object containing all visible nodes
89 root = self.getRoot()
90 top_level = root.getField('children')
91 num_items = top_level.getCount()
92
93 # walk the list of top-level scene tree nodes to look for proxy objects
94 for i in range(num_items):
95 node = top_level.getMFNode(i)
96 basetype = node.getBaseTypeName()
97 typename = node.getTypeName()
98 # print("Found base type", basetype)
99 if basetype == 'Robot' and typename == 'proxy':
100 name_field = node.getField('name')
101 pos_field = node.getField('translation')
102 rot_field = node.getField('rotation')
103 self.proxies.append((node, name_field, pos_field, rot_field))
104 name = name_field.getSFString()
105 position = pos_field.getSFVec3f()
106 # print("Found proxy %s at %s" % (name, position))
107
108 def get_proxy(self, name):
109 """Return a proxy body for the given robot name. Either returns an existing
110 proxy, allocates an available one, or returns None if no more are available."""
111 proxy = self.proxy_for_name.get(name)
112 if proxy is None and len(self.proxies) > 0:
113 proxy = self.proxies[-1]
114 del(self.proxies[-1])
115 name_field = proxy[1]
116 name_field.setSFString(name)
117 self.proxy_for_name[name] = proxy
118 print("%s: allocated proxy for %s" % (self.robot_name, name))
119 return proxy
120
121 def set_proxy_pose(self, proxy, x, y, z, heading):
122 # extract the fields from the proxy list, see find_proxies for format
123 pos_field = proxy[2]
124 rot_field = proxy[3]
125
126 # set the X, Y location
127 pos_field.setSFVec3f([x, y, z])
128
129 # convert the compass heading to an angle in radians w.r.t. the reference pose and apply it to the Z rotation
130 radians = (90 - heading) * (math.pi/180.0)
131 rot_field.setSFRotation([0, 0, 1, radians])
132 return
133
134 #================================================================
135 # Declare some MQTT callbacks.
136 def on_connect(self, client, userdata, flags, rc):
137 # Subscribing in on_connect() means that if we lose the connection and reconnect then subscriptions will be renewed.
138 global mqtt_client
139 print("%s: MQTT connected to server with with flags: %s, result code: %s" % (self.robot_name, flags, rc))
140 print("%s: MQTT subscribing to %s" % (self.robot_name, self.subscription))
141 self.mqtt_client.subscribe(self.subscription)
142
143 def on_message(self, client, userdata, msg):
144 # filter out our own transmissions
145 if msg.topic != self.send_topic:
146 # print("MQTT message received: {%s} %s" % (msg.topic, msg.payload))
147
148 # the expected topic has the form party/name, this will extract the name
149 topic_path = msg.topic.split('/')
150 if len(topic_path) == 2:
151 remote_name = topic_path[1]
152 proxy = self.get_proxy(remote_name)
153 if proxy is None:
154 printf("%s: no proxy body available for %s" % (self.robot_name, remote_name))
155 else:
156 # if a proxy body is available, parse the message text
157 tokens = msg.payload.split()
158 if len(tokens) != 4:
159 print("%s received malformed message on %s: %s" % (self.robot_name, msg.topic, msg.payload))
160 else:
161 try:
162 # apply the received data to the proxy body
163 x, y, z, heading = [float(x) for x in tokens]
164 self.set_proxy_pose(proxy, x, y, z, heading)
165
166 # and also rebroadcast on the local radio network
167 # emitter requires a bytestring, not a Python Unicode string
168 data = remote_name.encode() + b" " + msg.payload
169 self.emitter.send(data)
170
171 except ValueError:
172 print("%s received malformed message on %s: %s" % (self.robot_name, msg.topic, msg.payload))
173
174 def connect(self):
175 # Connect to the MQTT network
176 print("%s: initiating MQTT connection to %s:%d" % (self.robot_name, HOST, PORT))
177 self.mqtt_client.connect(host=HOST, port=PORT)
178
179 #================================================================
180 def poll_receiver(self):
181 """Process all available radio messages, forwarding select messages to the MQTT
182 network based on the first message token."""
183 while self.receiver.getQueueLength() > 0:
184 packet = self.receiver.getData()
185 tokens = packet.split()
186 if len(tokens) < 2:
187 print("%s malformed packet: %s" % (self.robot_name, packet))
188 else:
189 name = tokens[0].decode() # convert bytestring to Unicode
190 if name == self.public_robot:
191 # retransmit the remaining message string unchanged over MQTT; this
192 # will retain some flexibility against message format changes
193 data = b" ".join(tokens[1:])
194 self.mqtt_client.publish(self.send_topic, data)
195
196 # done with packet processing, advance to the next packet
197 self.receiver.nextPacket()
198 # no more data
199 return
200
201 #================================================================
202 def run(self):
203
204 # Run loop to execute a periodic script until the simulation quits.
205 # If the controller returns -1, the simulator is quitting.
206 while self.step(EVENT_LOOP_DT) != -1:
207
208 # Poll the simulated radio receiver.
209 self.poll_receiver()
210
211 # Poll the MQTT network system.
212 self.mqtt_client.loop(timeout=0.0)
213
214 # Read simulator clock time.
215 t = self.getTime()
216
217 # Test: move one proxy
218
219
220 # Shut down the networking cleanly. At this point
221 # print() output will no longer show in the Webots
222 # console, so this is silent.
223 self.mqtt_client.disconnect()
224
225################################################################
226
227controller = Disembod()
228controller.connect()
229controller.run()