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()