From be73bc3dc3666ac0316a548ba63aac8b1327a940 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Sun, 13 Nov 2022 19:11:04 +0000 Subject: [PATCH] Improve serial handling - make similar to other examples. --- .../computer/display_from_robot.py | 45 +++++++++---------- .../computer/display_from_robot.py | 21 ++++----- 2 files changed, 29 insertions(+), 37 deletions(-) diff --git a/ch-13/2-displaying-a-pose/computer/display_from_robot.py b/ch-13/2-displaying-a-pose/computer/display_from_robot.py index 955bb30..e46f70c 100644 --- a/ch-13/2-displaying-a-pose/computer/display_from_robot.py +++ b/ch-13/2-displaying-a-pose/computer/display_from_robot.py @@ -20,26 +20,24 @@ class RobotDisplay: def handle_data(self, data): self.line += data.decode("utf-8") - if not self.line.endswith("\n"): - return - print(f"Received data: {self.line}") - try: - message = json.loads(self.line) - except ValueError: - print("Error parsing JSON") - return - self.line = "" - 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 array of [x,y] arrays, and a separate array of angles - poses = np.array(message["poses"]).T - self.pose_coords = poses[:2] - angle_rads = np.deg2rad(poses[2]) - self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) - - + while "\n" in self.line: + line, self.line = self.line.split("\n", 1) + print(f"Received data: ```{line}```") + try: + message = json.loads(line) + except ValueError: + print("Error parsing JSON") + return + 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"]).T + self.pose_coords = poses[:2] + angle_rads = np.deg2rad(poses[2]) + self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) + def draw(self): plt.gca().clear() if self.arena: @@ -53,13 +51,11 @@ class RobotDisplay: ) if len(self.pose_coords) > 0: plt.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue") - plt.draw() async def main(self): plt.ion() - + await self.ble_connection.connect() try: - await self.ble_connection.connect() request = json.dumps({"command": "arena"}).encode() print(f"Sending request for arena: {request}") await self.ble_connection.send_uart_data(request) @@ -67,10 +63,9 @@ class RobotDisplay: while not self.display_closed: self.draw() + plt.draw() plt.pause(0.05) await asyncio.sleep(0.01) - - plt.show() finally: await self.ble_connection.close() diff --git a/ch-13/3-moving-poses-with-sensors/computer/display_from_robot.py b/ch-13/3-moving-poses-with-sensors/computer/display_from_robot.py index a22702d..452e7cd 100644 --- a/ch-13/3-moving-poses-with-sensors/computer/display_from_robot.py +++ b/ch-13/3-moving-poses-with-sensors/computer/display_from_robot.py @@ -13,8 +13,8 @@ class RobotDisplay: self.line = "" self.arena = {} self.display_closed = False - self.pose_coords = np.array([(0,), (0,)]) - self.pose_uv = np.array([(1,), (1,)]) + self.pose_coords = [] + self.pose_uv = [] def handle_close(self, _): self.display_closed = True @@ -29,30 +29,29 @@ class RobotDisplay: except ValueError: print("Error parsing JSON") return - 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 array of [x,y] arrays, and a separate array of angles + # matplotlib quiver plots wants an [x] ,[y] and [angle] arrays poses = np.array(message["poses"]).T self.pose_coords = poses[:2] angle_rads = np.deg2rad(poses[2]) self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) def draw(self): - self.ax.clear() + plt.gca().clear() if self.arena: for line in self.arena["arena"]: - self.ax.plot( + plt.gca().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( + plt.gca().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") + plt.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue") async def send_command(self, command): request = json.dumps({"command": command}).encode() @@ -69,9 +68,8 @@ class RobotDisplay: plt.ion() await self.ble_connection.connect() try: - # await self.send_command("arena") - self.fig, self.ax = plt.subplots() - self.fig.canvas.mpl_connect("close_event", self.handle_close) + await self.send_command("arena") + self.gcf().canvas.mpl_connect("close_event", self.handle_close) start_button = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Start") start_button.on_clicked(self.start) stop_button = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Stop") @@ -81,7 +79,6 @@ class RobotDisplay: self.draw() plt.pause(0.05) await asyncio.sleep(0.01) - plt.draw() finally: await self.ble_connection.close()