diff --git a/ch-13/1-modelling-space/computer/display_from_robot.py b/ch-13/1-modelling-space/computer/display_from_robot.py index 8cfb920..569b29b 100644 --- a/ch-13/1-modelling-space/computer/display_from_robot.py +++ b/ch-13/1-modelling-space/computer/display_from_robot.py @@ -29,20 +29,21 @@ class RobotDisplay: self.arena = message def draw(self): - plt.gca().clear() + self.ax.clear() if self.arena: for line in self.arena["arena"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" ) for line in self.arena["target_zone"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" ) async def main(self): plt.ion() await self.ble_connection.connect() + self.fig, self.ax = plt.subplots() try: request = json.dumps({"command": "arena"}).encode() print(f"Sending request for arena: {request}") 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 233d3e9..d7034bf 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 @@ -39,22 +39,23 @@ class RobotDisplay: self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) def draw(self): - plt.gca().clear() + self.ax.clear() if self.arena: for line in self.arena["arena"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" ) for line in self.arena["target_zone"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" ) 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") + self.ax.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue") async def main(self): plt.ion() await self.ble_connection.connect() + self.fig, self.ax = plt.subplots() try: request = json.dumps({"command": "arena"}).encode() print(f"Sending request for arena: {request}") diff --git a/ch-13/2-displaying-a-pose/robot/code.py b/ch-13/2-displaying-a-pose/robot/code.py index a57020a..97ff8fb 100644 --- a/ch-13/2-displaying-a-pose/robot/code.py +++ b/ch-13/2-displaying-a-pose/robot/code.py @@ -13,6 +13,8 @@ class Simulation: for n in range(population_size): self.poses[n] = random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360) +def send_json(data): + robot.uart.write((json.dumps(data)+"\n").encode()) async def command_handler(simulation): while True: @@ -20,27 +22,20 @@ async def command_handler(simulation): print("Receiving data...") try: data = robot.uart.readline().decode() - except UnicodeError: - print("Invalid data") - continue - try: request = json.loads(data) - print(f"Received command: {request}") - except ValueError: - print("Invalid JSON") + except (UnicodeError, ValueError): + print("Invalid data") continue # {"command": "arena"} if request["command"] == "arena": - response = { + send_json({ "arena": arena.boundary_lines, "target_zone": arena.target_zone, - } - robot.uart.write((json.dumps(response)+"\n").encode()) + }) else: - response = { + send_json({ "poses": simulation.poses.tolist(), - } - robot.uart.write((json.dumps(response)+"\n").encode()) + }) await asyncio.sleep(0.1) simulation= Simulation() 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 9407a8e..e34e232 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 @@ -40,18 +40,18 @@ class RobotDisplay: self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) def draw(self): - plt.gca().clear() + self.ax.clear() if self.arena: for line in self.arena["arena"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" ) for line in self.arena["target_zone"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" ) 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") + self.ax.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() @@ -67,9 +67,10 @@ class RobotDisplay: async def main(self): plt.ion() await self.ble_connection.connect() + self.fig, self.ax = plt.subplots() try: await self.send_command("arena") - plt.gcf().canvas.mpl_connect("close_event", self.handle_close) + self.fig.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") diff --git a/ch-13/3-moving-poses-with-sensors/robot/code.py b/ch-13/3-moving-poses-with-sensors/robot/code.py index ff8a2ac..ab80fd9 100644 --- a/ch-13/3-moving-poses-with-sensors/robot/code.py +++ b/ch-13/3-moving-poses-with-sensors/robot/code.py @@ -30,6 +30,9 @@ class Simulation: finally: robot.stop() +def send_json(data): + robot.uart.write((json.dumps(data)+"\n").encode()) + async def command_handler(simulation): simulation_task = None while True: @@ -37,22 +40,16 @@ async def command_handler(simulation): print("Receiving data...") try: data = robot.uart.readline().decode() - except UnicodeError: - print("Invalid data") - continue - try: request = json.loads(data) - print(f"Received command: {request}") - except ValueError: - print("Invalid JSON") + except (UnicodeError, ValueError): + print("Invalid data") continue # {"command": "arena"} if request["command"] == "arena": - response = { + send_json({ "arena": arena.boundary_lines, "target_zone": arena.target_zone, - } - robot.uart.write((json.dumps(response)+"\n").encode()) + }) elif request["command"] == "start": print("Starting simulation") if simulation_task is None or simulation_task.done(): @@ -63,10 +60,9 @@ async def command_handler(simulation): simulation_task.cancel() simulation_task = None else: - response = { + send_json({ "poses": simulation.poses.tolist(), - } - robot.uart.write((json.dumps(response)+"\n").encode()) + }) await asyncio.sleep(0.1) simulation= Simulation() diff --git a/ch-13/4-moving-poses-with-sensors-turning/computer/display_from_robot.py b/ch-13/4-moving-poses-with-sensors-turning/computer/display_from_robot.py index 9407a8e..e34e232 100644 --- a/ch-13/4-moving-poses-with-sensors-turning/computer/display_from_robot.py +++ b/ch-13/4-moving-poses-with-sensors-turning/computer/display_from_robot.py @@ -40,18 +40,18 @@ class RobotDisplay: self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) def draw(self): - plt.gca().clear() + self.ax.clear() if self.arena: for line in self.arena["arena"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" ) for line in self.arena["target_zone"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" ) 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") + self.ax.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() @@ -67,9 +67,10 @@ class RobotDisplay: async def main(self): plt.ion() await self.ble_connection.connect() + self.fig, self.ax = plt.subplots() try: await self.send_command("arena") - plt.gcf().canvas.mpl_connect("close_event", self.handle_close) + self.fig.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") diff --git a/ch-13/4-moving-poses-with-sensors-turning/robot/code.py b/ch-13/4-moving-poses-with-sensors-turning/robot/code.py index 1a54581..89907ac 100644 --- a/ch-13/4-moving-poses-with-sensors-turning/robot/code.py +++ b/ch-13/4-moving-poses-with-sensors-turning/robot/code.py @@ -38,6 +38,9 @@ class Simulation: finally: robot.stop() +def send_json(data): + robot.uart.write((json.dumps(data)+"\n").encode()) + async def command_handler(simulation): simulation_task = None while True: @@ -51,11 +54,10 @@ async def command_handler(simulation): continue # {"command": "arena"} if request["command"] == "arena": - response = { + send_json({ "arena": arena.boundary_lines, "target_zone": arena.target_zone, - } - robot.uart.write((json.dumps(response)+"\n").encode()) + }) elif request["command"] == "start": print("Starting simulation") if simulation_task is None or simulation_task.done(): @@ -66,10 +68,18 @@ async def command_handler(simulation): simulation_task.cancel() simulation_task = None else: - response = { - "poses": simulation.poses.tolist(), - } - robot.uart.write((json.dumps(response)+"\n").encode()) + sys_status, gyro, accel, mag = robot.imu.calibration_status + if sys_status != 3: + send_json({"imu_calibration": { + "gyro": gyro, + "accel": accel, + "mag": mag, + "sys": sys_status, + }}) + else: + send_json({ + "poses": simulation.poses.tolist(), + }) await asyncio.sleep(0.1) simulation= Simulation() diff --git a/ch-13/5-eliminating-poses/computer/display_from_robot.py b/ch-13/5-eliminating-poses/computer/display_from_robot.py index 9407a8e..e34e232 100644 --- a/ch-13/5-eliminating-poses/computer/display_from_robot.py +++ b/ch-13/5-eliminating-poses/computer/display_from_robot.py @@ -40,18 +40,18 @@ class RobotDisplay: self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) def draw(self): - plt.gca().clear() + self.ax.clear() if self.arena: for line in self.arena["arena"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" ) for line in self.arena["target_zone"]: - plt.gca().plot( + self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" ) 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") + self.ax.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() @@ -67,9 +67,10 @@ class RobotDisplay: async def main(self): plt.ion() await self.ble_connection.connect() + self.fig, self.ax = plt.subplots() try: await self.send_command("arena") - plt.gcf().canvas.mpl_connect("close_event", self.handle_close) + self.fig.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") diff --git a/ch-13/5-eliminating-poses/robot/code.py b/ch-13/5-eliminating-poses/robot/code.py index 6312e45..c93e7d9 100644 --- a/ch-13/5-eliminating-poses/robot/code.py +++ b/ch-13/5-eliminating-poses/robot/code.py @@ -22,7 +22,7 @@ class Simulation: pose[1] += speed_in_mm * np.sin(np.radians(pose[2])) def eliminate_impossible_poses(self, left_distance, right_distance): - poses_to_keep = np.ones(len(self.poses), dtype=bool) + poses_to_keep = np.ones(len(self.poses), dtype=np.uint8) # todo: would reorganising these pose arrays let us better use ulab tools? for position, pose in enumerate(self.poses): # first those outside the arena @@ -31,9 +31,9 @@ class Simulation: left_distance_point = pose[0] + robot.distance_sensor_from_middle + left_distance * np.cos(np.radians(pose[2])), pose[1] + left_distance * np.sin(np.radians(pose[2])) right_distance_point = pose[0] + robot.distance_sensor_from_middle + right_distance * np.cos(np.radians(pose[2])), pose[1] + right_distance * np.sin(np.radians(pose[2])) keep = arena.point_is_inside_arena(left_distance_point) and arena.point_is_inside_arena(right_distance_point) - poses_to_keep[position] = keep + poses_to_keep[position] = int(keep) # apply the keep as a mask to the poses using ulab. - doesn't work in ulab. - self.poses = [item for item, keep in zip(self.poses, poses_to_keep) if keep] + self.poses = np.array([item for item, keep in zip(self.poses, poses_to_keep) if keep]) async def run(self): try: @@ -62,6 +62,9 @@ class Simulation: finally: robot.stop() +def send_json(data): + robot.uart.write((json.dumps(data)+"\n").encode()) + async def command_handler(simulation): simulation_task = None while True: @@ -75,11 +78,10 @@ async def command_handler(simulation): continue # {"command": "arena"} if request["command"] == "arena": - response = { + send_json({ "arena": arena.boundary_lines, "target_zone": arena.target_zone, - } - robot.uart.write((json.dumps(response)+"\n").encode()) + }) elif request["command"] == "start": print("Starting simulation") if simulation_task is None or simulation_task.done(): @@ -90,10 +92,18 @@ async def command_handler(simulation): simulation_task.cancel() simulation_task = None else: - response = { - "poses": simulation.poses.tolist(), - } - robot.uart.write((json.dumps(response)+"\n").encode()) + sys_status, gyro, accel, mag = robot.imu.calibration_status + if sys_status != 3: + send_json({"imu_calibration": { + "gyro": gyro, + "accel": accel, + "mag": mag, + "sys": sys_status, + }}) + else: + send_json({ + "poses": simulation.poses.tolist(), + }) await asyncio.sleep(0.1) simulation= Simulation()