Display working. Sensors starting to work.

Clusters are forming.

ch-13/full-version/computer/display_from_robot.py

@@ -14,17 +14,18 @@ class RobotDisplay:
         self.arena = {}
         self.display_closed = False
         self.fig, self.ax = plt.subplots()
-        self.pose_coords = []
-        self.pose_uv = []
+        self.poses = np.zeros([200, 3], dtype=np.float32)
 
     def handle_close(self, _):
         self.display_closed = True
 
     def handle_data(self, data):
         self.line += data.decode("utf-8")
+        # print(f"Received data: {data.decode('utf-8')}")
+        # print(f"Line is now: {self.line}")
         while "\n" in self.line:
             line, self.line = self.line.split("\n", 1)
-            # print(f"Received data: {line}")
+            print(f"Received line: {line}")
             try:
                 message = json.loads(line)
             except ValueError:
@@ -33,12 +34,8 @@ class RobotDisplay:
             if "arena" in message:
                 self.arena = message
             if "poses" in message:
-                # the robot poses are an array of [x, y, theta] arrays.
-                # matplotlib quiver plots wants an [x] ,[y] and [angle] arrays
-                poses = np.array(message["poses"])
-                self.pose_coords = poses[:,0:1].T
-                angle_rads = np.deg2rad(poses[:,2])
-                self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
+                print(message)
+                self.poses[message["offset"]: message["offset"] + len(message["poses"])] = message["poses"]
 
     def draw(self):
         self.ax.clear()
@@ -47,12 +44,7 @@ class RobotDisplay:
                 self.ax.plot(
                     [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
                 )
-            # for line in self.arena["target_zone"]:
-            #     self.ax.plot(
-            #         [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
-            #     )
-        if len(self.pose_coords) > 0:
-            self.ax.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue")
+        self.ax.scatter(self.poses[:,0], self.poses[:,1], color="blue")
 
     async def send_command(self, command):
         #+ "\n" - why does adding this (which sounds right) cause the ble stack (on the robot or computer? ) not to work any more?