66 lines
2.4 KiB
Python
66 lines
2.4 KiB
Python
#!/usr/bin/env python3
|
|
import uuid
|
|
import paho.mqtt.client as mqtt
|
|
from planet import Direction, Planet
|
|
from communication import Mode, Communication
|
|
from move import Move
|
|
from time import sleep
|
|
|
|
# DO NOT EDIT
|
|
client = None
|
|
|
|
|
|
def run():
|
|
# DO NOT EDIT
|
|
global client
|
|
# client_id has to be unique among ALL users
|
|
client = mqtt.Client(client_id=str(uuid.uuid4()),
|
|
clean_session=False,
|
|
protocol=mqtt.MQTTv31)
|
|
|
|
communication = Communication(client)
|
|
move = Move()
|
|
|
|
while(communication.planetname is None):
|
|
communication.process_messages()
|
|
|
|
# run first time not in loop because odometry data is not relevant
|
|
communication.update(None) # TODO: Register in map
|
|
# correct odometry right here otherwise list of edges will be incorrect
|
|
move.setcurdir(communication.getdir())
|
|
communication.process(move.getstationedges())
|
|
move.turnto(communication.navto[0][1]) # TODO: add control mechanism to communication class
|
|
sleep(4)
|
|
communication.planet.setcurdir(communication.navto[0][1])
|
|
move.setcurdir(communication.getdir())
|
|
move.reset()
|
|
print("current dir:", move._odometry.angle_get())
|
|
print("Navigation Data:", communication.navto)
|
|
move.traversetonextstation(False)
|
|
|
|
while(communication.status() is not Mode.COMPLETE):
|
|
while(communication.status() is Mode.GOTOSTATION or communication.status() is Mode.TARGET): # should be triggered if current station has no unexplored edges
|
|
for instructions in communication.navto:
|
|
for instructions in communication.navto:
|
|
communication.update(None, None)
|
|
move.turnto(instructions[1])
|
|
move.traversetonextstation(True)
|
|
|
|
communication.update(move.getstats()) # TODO: Register in map
|
|
# correct odometry right here otherwise list of edges will be incorrect
|
|
move.setcurdir(communication.getdir())
|
|
communication.process(move.getstationedges())
|
|
move.turnto(communication.navto[0][1]) # TODO: add control mechanism to communication class
|
|
sleep(4)
|
|
communication.planet.setcurdir(communication.navto[0][1])
|
|
move.setcurdir(communication.getdir())
|
|
move.reset()
|
|
print("current dir:", move._odometry.angle_get())
|
|
print("Navigation Data:", communication.navto)
|
|
move.traversetonextstation(False)
|
|
|
|
|
|
# DO NOT EDIT
|
|
if __name__ == '__main__':
|
|
run()
|