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