robolab/src/communication.py

160 lines
5.5 KiB
Python
Raw Normal View History

#!/usr/bin/env python3
from planet import Planet, Direction
from enum import Enum
"""
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"
TARGET2 = "target2"
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
def status(self):
return self._status
def update(self):
pass
# 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):
for messages in self.msg_queue:
if(type(messages) == tuple):
self.client.publish(messages[0], payload=messages[1], qos=1)
elif(messages.payload.decode('utf-8').startswith(Command.RECEIVE)):
self.comexec(messages.payload.decode('utf-8')[4:])
self.msg_queue.clear()
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(',')))
elif message.startswith(Command.PATH):
self.stringtopath(*message[5:].rsplit(' '))
elif message.startswith(Command.TARGET):
self._status = Mode.TARGET
self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(',')))
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))
def pathtostring(self, start, target, blocked):
if(blocked):
path = "blocked"
else:
path = "free"
return tupel2str(*start[0]) + ',' + dir2str(start[1]) + ' ' + tupel2str(*target[0]) + ',' + dir2str(target[1]) + ' ' + path