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

@@ -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):

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])

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_l.turnbyamount(20, 40)
-        self._wheel_r.turnbyamount(-40, 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)

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
 
     '''