From aa6b049ce56bb93c0d48bdc12a971b2a00086312 Mon Sep 17 00:00:00 2001 From: d3rped Date: Mon, 26 Mar 2018 07:05:55 +0200 Subject: [PATCH] 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. --- src/communication.py | 20 ++++++++++++-------- src/main.py | 10 ++++++++-- src/move.py | 27 ++++++++++++++++++++++++--- src/planet.py | 13 ++++++------- 4 files changed, 50 insertions(+), 20 deletions(-) diff --git a/src/communication.py b/src/communication.py index bf6c3df..6dcdb5f 100644 --- a/src/communication.py +++ b/src/communication.py @@ -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 + 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): diff --git a/src/main.py b/src/main.py index 606829a..14d8236 100644 --- a/src/main.py +++ b/src/main.py @@ -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]) diff --git a/src/move.py b/src/move.py index 5506b63..f444aec 100644 --- a/src/move.py +++ b/src/move.py @@ -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) diff --git a/src/planet.py b/src/planet.py index aee3e46..04f6f33 100644 --- a/src/planet.py +++ b/src/planet.py @@ -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 '''