Displaying a pose written up.

ch-13/2-displaying-a-pose/robot/arena.py

@@ -1,5 +1,8 @@
-"""Represent the lines and target zone of the arena"""
-
+"""Represent the lines of the arena"""
+try:
+  from ulab import numpy as np
+except ImportError:
+  import numpy as np
 
 boundary_lines = [
     [(0,0), (0, 1500)],
@@ -10,13 +13,65 @@ boundary_lines = [
     [(1000, 0), (0, 0)],
 ]
 
-
-target_zone = [
-  [(1100, 900), (1100, 1100)],
-  [(1100, 1100), (1250, 1100)],
-  [(1250, 1100), (1250, 900)],
-  [(1250, 900), (1100, 900)],
-]
-
 width = 1500
 height = 1500
+
+
+grid_cell_size = 50
+overscan = 10 # 10 each way
+
+
+def get_distance_to_segment(x, y, segment):
+    """Return the distance from the point to the segment.
+    Segment -> ((x1, y1), (x2, y2))
+    All segments are horizontal or vertical.
+    """
+    x1, y1 = segment[0]
+    x2, y2 = segment[1]
+    # if the segment is horizontal, the point will be closest to the y value of the segment
+    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
+        return abs(y - y1)
+    # if the segment is vertical, the point will be closest to the x value of the segment
+    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
+        return abs(x - x1)
+    # the point will be closest to one of the end points
+    return np.sqrt(
+        min(
+            (x - x1) ** 2 + (y - y1) ** 2, 
+            (x - x2) ** 2 + (y - y2) ** 2
+        )
+    )
+
+
+def get_distance_likelihood(x, y):
+    """Return the distance from the point to the nearest segment as a decay function."""
+    min_distance = None
+    for segment in boundary_lines:
+        distance = get_distance_to_segment(x, y, segment)
+        if min_distance is None or distance < min_distance:
+            min_distance = distance
+    return 1.0 / (1 + min_distance/250) ** 2
+
+
+grid_cell_size = 50
+overscan = 10 # 10 each way
+
+# beam endpoint model
+def make_distance_grid():
+    """Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
+    make a grid of the distance to the nearest boundary line.
+    """
+    grid = np.zeros((
+            width // grid_cell_size + 2 * overscan, 
+            height // grid_cell_size + 2 * overscan
+        ), dtype=np.float)
+    for x in range(grid.shape[0]):
+        column_x = x * grid_cell_size - (overscan * grid_cell_size)
+        for y in range(grid.shape[1]):
+            row_y = y * grid_cell_size - (overscan * grid_cell_size)
+            grid[x, y] = get_distance_likelihood(
+                column_x, row_y
+            )
+    return grid
+
+distance_grid = make_distance_grid()

ch-13/2-displaying-a-pose/robot/code.py

@@ -6,37 +6,51 @@ from ulab import numpy as np
 import arena
 import robot
 
-class Simulation:
-  def __init__(self):
-    population_size = 10
-    self.poses = np.empty((population_size, 3), dtype=np.float)
-    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())
+    robot.uart.write((json.dumps(data) + "\n").encode())
+
+def read_json():
+    try:
+      data = robot.uart.readline()
+      decoded = data.decode()
+      return json.loads(decoded)
+    except (UnicodeError, ValueError):
+      print("Invalid data")
+      return None
+
+
+class Simulation:
+    def __init__(self):
+      self.population_size = 20
+      self.poses = np.array(
+          [(
+              int(random.uniform(0, arena.width)),
+              int(random.uniform(0, arena.height)),
+              int(random.uniform(0, 360))) for _ in range(self.population_size)],
+          dtype=np.int16,
+      )
+
+def send_poses(samples):
+    send_json({
+        "poses": samples[:,:2].tolist(),
+    })
 
 async def command_handler(simulation):
-  while True:
-    if robot.uart.in_waiting:
-      print("Receiving data...")
-      try:
-        data = robot.uart.readline().decode()
-        request = json.loads(data)
-      except (UnicodeError, ValueError):
-        print("Invalid data")
-        continue
-      # {"command": "arena"}
-      if request["command"] == "arena":
-         send_json({
-            "arena": arena.boundary_lines,
-            "target_zone": arena.target_zone,
-         })
-    else:
-      send_json({
-        "poses": simulation.poses.tolist(),
-      })
-    await asyncio.sleep(0.1)
+    print("Starting handler")
+    while True:
+        if robot.uart.in_waiting:
+            request = read_json()
+            if not request:
+                continue
+            print("Received: ", request)
+            if request["command"] == "arena":
+                send_json({
+                    "arena": arena.boundary_lines,
+                })
+                send_poses(simulation.poses)
 
-simulation= Simulation()
+        await asyncio.sleep(0.1)
+
+
+simulation = Simulation()
 asyncio.run(command_handler(simulation))