derped/robolab
1
0
Fork 0
Code Pull requests Releases Activity

Split up communication update method to get better odometry handling. Implemented rudimentary blocked path handling. Some more work ok main loop and communication/map handling.

Browse source
This commit is contained in:
d3rped 2018-03-26 07:05:55 +02:00
parent c479954960
commit aa6b049ce5
4 changed files with 50 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
src/communication.py
View file

@ -105,19 +105,21 @@ class Communication:
def status(self):
return self._status
def update(self, odometry, edges): # TODO: FINISH
def update(self, odometry): # TODO: FINISH
if(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]))
if(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.navto.pop(0)
self.process_messages() # this should update curnode and curdir
else:
self.process_messages() # this should update curnode and curdir
def process(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
@ -139,9 +141,9 @@ class Communication:
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!")
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)
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])))
@ -155,15 +157,16 @@ class Communication:
print("Incoming Message is:", messages.payload.decode('utf-8'))
self.comexec(messages.payload.decode('utf-8')[4:])
self.msg_queue.clear()
sleep(2)
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 to set start as blocked
# 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):
@ -179,6 +182,7 @@ class Communication:
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):

10
src/main.py
View file

@ -25,7 +25,10 @@ def run():
communication.process_messages()
# run first time not in loop because odometry data is not relevant
communication.update(None, move.getstationedges()) # TODO: Register in map
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])
@ -43,7 +46,10 @@ def run():
move.turnto(instructions[1])
move.traversetonextstation(True)
communication.update(move.getstats(), move.getstationedges()) # TODO: Register in map
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])

27
src/move.py
View file

@ -70,8 +70,9 @@ class Move:
self._sensor.refresh()
self._wheel_l.stop()
self._wheel_r.stop()
self._wheel_l.turnbyamount(40, 40)
self._wheel_r.turnbyamount(-40, 40)
self._wheel_l.turnbyamount(20, 40)
self._wheel_r.turnbyamount(-20, 40)
sleep(1)
def setcurcir(self, curdir):
self.odometry.angle_set(curdir[1])
@ -132,7 +133,7 @@ class Move:
self._sensor.refresh()
self._sensor.checkbumper()
self._sensor.getcolor()
if (not (self._sensor.lastcolor is None)):
if(self._sensor.lastcolor is not None):
break
self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
@ -140,6 +141,26 @@ class Move:
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
self._wheel_l.stop()
self._wheel_r.stop()
if(self._sensor._bumper_r):
self._wheel_l.turnbyamount(-200, 120)
self._wheel_r.turnbyamount(-200, 120)
sleep(1)
self._wheel_l.turnbyamount(320, 120)
self._wheel_r.turnbyamount(-320, 120)
sleep(2)
self._aligntoedge()
self._wheel_l.run()
self._wheel_r.run()
while(True):
self._sensor.refresh()
self._sensor.getcolor()
if(self._sensor.lastcolor is not None):
break
self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
if(not isknownstation):
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
self._wheel_l.turnbyamount(200, 90)
self._wheel_r.turnbyamount(200, 90)
sleep(2)

13
src/planet.py
View file

@ -81,19 +81,18 @@ class Planet:
def getcurdir(self):
return self._curdir
def addtounexedge(self, node_edge_list):
self.unexedge = self.unexedge + node_edge_list
def exploreedge(self, node_edge):
self.unexedge.remove(node_edge)
def checkedges(self, testnode):
if(self._curnode not in self._planetmap):
self._planetmap[self._curnode] = {}
for edges in testnode:
if(edges in self._planetmap[self._curnode]):
testnode.remove(edges)
# maybe do some explored node stuff here?
if(testnode != [] and self._curnode not in self.unexedge):
self.unexedge.append(self._curnode)
if(testnode == [] and self._curnode in self.unexedge):
self.unexedge.remove(self._curnode)
print("MAP:", self._planetmap[self._curnode])
print("NODES:", testnode)
return testnode
'''