From aa2bc483f01362aed862db643e35041da316cd08 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Mon, 30 Jan 2023 14:02:34 +0000 Subject: [PATCH] Extra debug revealed a problem with the sensor calculations. --- .../display_with_sensor_debug.py | 105 ++++++++++++++++++ ch-13/4.3-monte-carlo_perf/robot/code.py | 18 ++- 2 files changed, 119 insertions(+), 4 deletions(-) create mode 100644 ch-13/4.3-monte-carlo_perf/display_with_sensor_debug.py diff --git a/ch-13/4.3-monte-carlo_perf/display_with_sensor_debug.py b/ch-13/4.3-monte-carlo_perf/display_with_sensor_debug.py new file mode 100644 index 0000000..16f087a --- /dev/null +++ b/ch-13/4.3-monte-carlo_perf/display_with_sensor_debug.py @@ -0,0 +1,105 @@ +import asyncio +import json +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.widgets import Button + +from robot_ble_connection import BleConnection + + +class RobotDisplay: + def __init__(self): + self.ble_connection = BleConnection(self.handle_data) + self.buffer = "" + self.arena = {} + self.closed = False + self.fig, self.axes = plt.subplots() + self.poses = None + self.distance_observation = None + + def handle_close(self, _): + self.closed = True + + def handle_data(self, data): + self.buffer += data.decode() + # print(f"Received raw data: {data}") + while "\n" in self.buffer: + line, self.buffer = self.buffer.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: + self.poses = np.array(message["poses"], dtype=np.int16) + if "distance_observation" in message: + self.distance_observation = message["distance_observation"] + + def draw(self): + self.axes.clear() + if self.arena: + for line in self.arena["arena"]: + self.axes.plot( + [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" + ) + if self.poses is not None: + self.axes.scatter(self.poses[:,0], self.poses[:,1], color="blue") + if self.distance_observation: + # draw a red arrow from the distance observation pose (in its direction) + heading = np.radians(self.distance_observation["pose"][2]) + self.axes.arrow( + self.distance_observation["pose"][0], + self.distance_observation["pose"][1], + 100 * np.cos(heading), + 100 * np.sin(heading), + color="red", + ) + # draw yellow dot at the left distance sensor location + self.axes.scatter( + self.distance_observation["left_sensor"][0], + self.distance_observation["left_sensor"][1], + color="yellow", + ) + # draw yellow dot at the right distance sensor location + self.axes.scatter( + self.distance_observation["right_sensor"][0], + self.distance_observation["right_sensor"][1], + color="yellow", + ) + # write the weight of the distance observation as text below the arrow + self.axes.text( + self.distance_observation["pose"][0], + self.distance_observation["pose"][1], + f"{self.distance_observation['weight']:.2E}", + ) + + async def send_command(self, command): + request = (json.dumps({"command": command}) ).encode() + print(f"Sending request: {request}") + await self.ble_connection.send_uart_data(request) + + def start(self, _): + self.button_task = asyncio.create_task(self.send_command("start")) + + async def main(self): + plt.ion() + await self.ble_connection.connect() + try: + await self.send_command("arena") + 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) + while not self.closed: + self.draw() + plt.draw() + plt.pause(0.05) + await asyncio.sleep(0.01) + finally: + await self.ble_connection.close() + + +robot_display = RobotDisplay() +asyncio.run(robot_display.main()) diff --git a/ch-13/4.3-monte-carlo_perf/robot/code.py b/ch-13/4.3-monte-carlo_perf/robot/code.py index 5edf358..e87153d 100644 --- a/ch-13/4.3-monte-carlo_perf/robot/code.py +++ b/ch-13/4.3-monte-carlo_perf/robot/code.py @@ -172,13 +172,13 @@ class Simulation: def observe_distance_sensors(self, weights): self.pc_observe_distance_sensors.start() # Sensor triangle left - opposite = self.distance_sensors.left + robot.dist_forward_mm - adjacent = robot.dist_side_mm + adjacent = self.distance_sensors.left + robot.dist_forward_mm + opposite = robot.dist_side_mm left_angle = np.atan(opposite / adjacent) left_hypotenuse = np.sqrt(opposite**2 + adjacent**2) # Sensor triangle right - opposite = self.distance_sensors.right + robot.dist_forward_mm - adjacent = robot.dist_side_mm + adjacent = self.distance_sensors.right + robot.dist_forward_mm + opposite = robot.dist_side_mm right_angle = np.atan(opposite / adjacent) right_hypotenuse = np.sqrt(opposite**2 + adjacent**2) @@ -201,6 +201,16 @@ class Simulation: sensor_weight = arena.get_distance_likelihood_at(left_sensor[index,0], left_sensor[index,1]) sensor_weight += arena.get_distance_likelihood_at(right_sensor[index,0], right_sensor[index,1]) weights[index] *= sensor_weight + send_json({"distance_observation": + { + "weight": weights[0], + "pose": self.poses[0].tolist(), + "left_sensor": left_sensor[0].tolist(), + "right_sensor": right_sensor[0].tolist(), + "left_distance": self.distance_sensors.left, + "right_distance": self.distance_sensors.right + } + }) self.pc_observe_distance_sensors.stop() return weights