robolab/src/communication.py

#!/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