198 lines
8.3 KiB
Python
198 lines
8.3 KiB
Python
#!/usr/bin/env python3
|
|
|
|
from planet import Planet, Direction
|
|
from enum import Enum
|
|
from time import sleep
|
|
|
|
"""
|
|
Class to hold the MQTT client
|
|
|
|
Documentation:
|
|
On start do the following (make part of __init__):
|
|
(1) subscribe to explorer/<N>
|
|
(2) publish to explorer/<N>: SYN ready
|
|
|
|
Next receive the planet name and starting coordinates
|
|
(1) receive from explorer/<N>: ACK <Name> <X>,<Y>
|
|
(2) subscribe to planet/<Name>
|
|
|
|
on_message:
|
|
Called when a message has been received on a topic that the client subscribes to and the message does not match an existing topic filter callback. Use message_callback_add() to define a callback that will be called for specific topic filters. on_message will serve as fallback when none matched.
|
|
client
|
|
the client instance for this callback
|
|
userdata
|
|
the private user data as set in Client() or user_data_set()
|
|
message
|
|
an instance of MQTTMessage. This is a class with members topic, payload, qos, retain.
|
|
|
|
|
|
mqtt message Documentation:
|
|
| Members:
|
|
|
|
|
| topic : String. topic that the message was published on.
|
|
| payload : String/bytes the message payload.
|
|
| qos : Integer. The message Quality of Service 0, 1 or 2.
|
|
| retain : Boolean. If true, the message is a retained message and not fresh.
|
|
| mid : Integer. The message id.
|
|
"""
|
|
|
|
|
|
def str2dir(string):
|
|
return {
|
|
'N': Direction.NORTH,
|
|
'E': Direction.EAST,
|
|
'S': Direction.SOUTH,
|
|
'W': Direction.WEST
|
|
}.get(string)
|
|
|
|
|
|
def dir2str(direction):
|
|
return {
|
|
Direction.NORTH: 'N',
|
|
Direction.EAST: 'E',
|
|
Direction.SOUTH: 'S',
|
|
Direction.WEST: 'W'
|
|
}.get(direction)
|
|
|
|
|
|
def str2tuple(string_a, string_b):
|
|
return (eval(string_a), eval(string_b))
|
|
|
|
|
|
def tupel2str(int_a, int_b):
|
|
return str(int_a) + ',' + str(int_b)
|
|
|
|
|
|
class Command(str, Enum):
|
|
SEND = "SYN " # SYN is short for synchronize
|
|
RECEIVE = "ACK " # ACK is short for acknowledge
|
|
READY = "ready"
|
|
PATH = "path "
|
|
TARGET = "target "
|
|
COMPLETE = "exploration completed!"
|
|
NOTICE = "notice"
|
|
|
|
|
|
class Mode(str, Enum):
|
|
EXPLORE = "explore"
|
|
GOTOSTATION = "gotostation"
|
|
TARGET = "target"
|
|
COMPLETE = "complete"
|
|
|
|
|
|
class Communication:
|
|
""" Initializes communication module, connect to server, subscribe, etc. """
|
|
def __init__(self, mqtt_client):
|
|
# THESE TWO VARIABLES MUST NOT BE CHANGED
|
|
self.msg_queue = []
|
|
self.planetname = None
|
|
self.planet = Planet()
|
|
self.client = mqtt_client
|
|
self.client.on_message = self.on_message
|
|
self.uid = '122'
|
|
passwd = '7KMuWPT2UE'
|
|
self.client.username_pw_set(self.uid, password=passwd)
|
|
try:
|
|
self.client.connect('robolab.inf.tu-dresden.de', port=8883)
|
|
except OSError:
|
|
print("ERROR: check your network connection.")
|
|
self.subscribetochannel("explorer/" + self.uid)
|
|
self.encode_message(Command.READY, None)
|
|
self.client.loop_start()
|
|
self._status = Mode.EXPLORE
|
|
self.navto = []
|
|
self.isfirstrun = True
|
|
self.target = None
|
|
|
|
def status(self):
|
|
return self._status
|
|
|
|
def update(self, odometry): # TODO: FINISH
|
|
if(self.isfirstrun):
|
|
self.isfirstrun = False
|
|
elif(odometry is not None): # use odometry to send data
|
|
self.encode_message(Command.PATH, ((self.planet.getcurnode(), self.planet.getcurdir()), ((self.planet.getcurnode()[0] + odometry[0][0], self.planet.getcurnode()[1] + odometry[0][1]), (odometry[1] - 180) % 360), odometry[2]))
|
|
elif(self._status is Mode.GOTOSTATION or self._status is Mode.TARGET):
|
|
self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]), (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2]))
|
|
# self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]), (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2]))
|
|
self.process_messages() # this should update curnode and curdir
|
|
|
|
def process_edges(self, edges):
|
|
edges = self.planet.checkedges(edges)
|
|
if(edges == [] and self._status is Mode.EXPLORE):
|
|
if(self.planet.unexedge == []):
|
|
self.encode_message(Command.COMPLETE, None)
|
|
self._status = Mode.COMPLETE
|
|
self.process_messages()
|
|
else:
|
|
self.navto = next(filter(lambda x: x is not None, map(lambda edge: self.planet.shortest_path(self.planet.getcurnode(), edge), self.planet.unexedge)))
|
|
self._status = Mode.GOTOSTATION
|
|
elif(edges != []):
|
|
self.navto = [(self.planet.getcurnode(), edges[0])]
|
|
|
|
def getdir(self):
|
|
return ((self.planet.getcurnode(), self.planet.getcurdir()))
|
|
|
|
# THIS FUNCTIONS SIGNATURE MUST NOT BE CHANGED
|
|
""" Handles the callback if any message arrived """
|
|
def on_message(self, client, data, message):
|
|
self.msg_queue.append(message)
|
|
|
|
def subscribetochannel(self, channelname):
|
|
self.client.subscribe(channelname, qos=1)
|
|
|
|
def encode_message(self, msgtype, msgdata):
|
|
if(msgtype == Command.READY):
|
|
self.msg_queue.append(("explorer/" + self.uid, Command.SEND + Command.READY))
|
|
if(msgtype == Command.TARGET):
|
|
self.msg_queue.append(("explorer/" + self.uid, Command.SEND + Command.TARGET + "reached!"))
|
|
if(msgtype == Command.COMPLETE):
|
|
self.msg_queue.append(("explorer/" + self.uid, Command.SEND + Command.COMPLETE))
|
|
if(msgtype == Command.PATH):
|
|
self.msg_queue.append(("planet/" + self.planetname, Command.SEND + Command.PATH + self.pathtostring(msgdata[0], msgdata[1], msgdata[2])))
|
|
|
|
def process_messages(self):
|
|
while(self.msg_queue != []):
|
|
for messages in self.msg_queue:
|
|
if(type(messages) == tuple):
|
|
print("Outgoing Message is:", messages)
|
|
self.client.publish(messages[0], payload=messages[1], qos=1)
|
|
elif(messages.payload.decode('utf-8').startswith(Command.RECEIVE)):
|
|
print("Incoming Message is:", messages.payload.decode('utf-8'))
|
|
self.comexec(messages.payload.decode('utf-8')[4:])
|
|
self.msg_queue.clear()
|
|
sleep(3)
|
|
|
|
def comexec(self, message):
|
|
if not any([message.startswith(instruction) for instruction in Command]): # is planet name and starting position
|
|
[self.planetname, startnode] = message.rsplit(' ')
|
|
self.subscribetochannel("planet/" + self.planetname)
|
|
self.planet.setcurnode(str2tuple(*startnode.rsplit(',')))
|
|
# Fake Message send and add entry to set start as blocked
|
|
self.encode_message(Command.PATH, [(self.planet.getcurnode(), Direction.SOUTH), (self.planet.getcurnode(), Direction.SOUTH), -1])
|
|
self.planet.add_path((self.planet.getcurnode(), Direction.SOUTH), (self.planet.getcurnode(), Direction.SOUTH), -1)
|
|
elif message.startswith(Command.PATH):
|
|
self.stringtopath(*message[5:].rsplit(' '))
|
|
elif message.startswith(Command.TARGET):
|
|
self.target = str2tuple(*message[7:].rsplit(','))
|
|
self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(',')))
|
|
if(self.navto is not None):
|
|
self._status = Mode.TARGET
|
|
elif message.startswith(Command.NOTICE):
|
|
print(message)
|
|
|
|
def stringtopath(self, start, target, block, weight):
|
|
snode = (str2tuple(*start[:-2].rsplit(',')), str2dir(start[-1:]))
|
|
tnode = (str2tuple(*target[:-2].rsplit(',')), str2dir(target[-1:]))
|
|
self.planet.add_path(snode, tnode, int(weight))
|
|
if(snode[0] == self.planet.getcurnode() or self.planet.getcurnode is None):
|
|
self.planet.setcurnode(tnode[0])
|
|
self.planet.setcurdir((tnode[1] - 180) % 360)
|
|
|
|
def pathtostring(self, start, target, blocked):
|
|
if(blocked == -1):
|
|
path = "blocked"
|
|
else:
|
|
path = "free"
|
|
return tupel2str(*start[0]) + ',' + dir2str(start[1]) + ' ' + tupel2str(*target[0]) + ',' + dir2str(target[1]) + ' ' + path
|