Beam endpoint works.

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

@@ -0,0 +1,27 @@
+import arena
+from matplotlib import pyplot as plt
+import numpy as np
+
+def draw_arena_boundaries(arena):
+    for line in arena:
+        plt.plot([line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red")
+
+def draw_distance_grid(ax):
+  overscan_size = arena.overscan * arena.grid_cell_size
+
+  ax.imshow(
+    arena.distance_grid.T, 
+    extent = [-overscan_size, arena.width + overscan_size, -overscan_size, arena.height + overscan_size], 
+    origin="lower",
+    cmap="gray",
+    norm="log",
+  )
+
+fig, ax = plt.subplots()
+draw_arena_boundaries(arena.boundary_lines)
+print("Value at 0, 1500 is", arena.get_distance_grid_at_point(0, 1500))
+# print("Value at 1000, 500 is", arena.get_distance_grid_at_point(1000, 500))
+# print("Value at 500, 1000 is", arena.get_distance_grid_at_point(500, 1000))
+# print("Value at 550, 1000 is", arena.get_distance_grid_at_point(550, 1000))
+draw_distance_grid(ax)
+plt.show()

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

@@ -0,0 +1,24 @@
+from unittest import TestCase
+import math
+import arena
+
+class TestArena(TestCase):
+  def test_get_point_to_distance_segment_1(self):
+      segment = ((0, 0), (0, 1500))
+      for x in range(0, 1500):
+        for y in (0, 500, 1000):
+          self.assertEqual(arena.get_point_distance_to_segment(x, y, segment), x)
+
+  def test_get_point_to_distance_segment_2(self):
+      segment = ((0, 0), (1500, 0))
+      for y in range(0, 1500):
+        for x in (0, 500, 1000):
+          self.assertEqual(arena.get_point_distance_to_segment(x, y, segment), y)
+
+  def test_get_point_distance_to_nearest_segment(self):
+      segments = [
+        [(0, 1500), (1500, 1500)],
+      ]
+      for y in range(1500):
+        for x in (0, 500, 1000):
+          self.assertEqual(arena.get_point_distance_to_nearest_segment(segments, x, y), 1500 - y)

ch-13/full-version/robot/arena.py

@@ -1,5 +1,8 @@
 """Represent the lines and target zone of the arena"""
-import math
+try:
+  from ulab import numpy as np
+except ImportError:
+  import numpy as np
 
 boundary_lines = [
     [(0,0), (0, 1500)],
@@ -29,60 +32,55 @@ def point_is_inside_arena(x, y):
     return False
   return True
 
-## intention - we can use a distance squared function to avoid the square root, and just square the distance sensor readings too.
+def get_point_distance_to_segment(x, y, segment):
-def get_ray_distance_to_segment_squared(ray_x, ray_y, ray_tan, ray_heading, segment):
+  """Return the distance squared from the point to the segment.
-  """Return the distance squared from the ray origin to the intersection point along the given ray heading.
-  The segments are boundary lines, which will be horizontal or vertical, and have known lengths.
-  The ray can have any heading, and will be infinite in length.
-  Ray -> (x, y, heading)
-  ray_tan -> tangent of the heading (optimization)
   Segment -> ((x1, y1), (x2, y2))
+  All segments are horizontal or vertical.
   """
   segment_x1, segment_y1 = segment[0]
   segment_x2, segment_y2 = segment[1]
-  # if the segment is horizontal, the ray will intersect it at a known y value
+  # if the segment is horizontal, the point will be closest to the y value of the segment
-  if segment_y1 == segment_y2:
+  if segment_y1 == segment_y2 and x >= min(segment_x1, segment_x2) and x <= max(segment_x1, segment_x2):
-    # if the ray is horizontal, it will never intersect the segment
+      return abs(y - segment_y1)
-    if ray_heading == 0:
+  # if the segment is vertical, the point will be closest to the x value of the segment
-      return None
+  if segment_x1 == segment_x2 and y >= min(segment_y1, segment_y2) and y <= max(segment_y1, segment_y2):
-    # calculate the x value of the intersection point
+      return abs(x - segment_x1)
-    intersection_x = ray_x + (segment_y1 - ray_y) / ray_tan
+  # the point will be closest to one of the end points
-    # is the intersection point on the segment?
+  return np.sqrt(min((x - segment_x1) ** 2 + (y - segment_y1) ** 2, (x - segment_x2) ** 2 + (y - segment_y2) ** 2))
-    if intersection_x > max(segment_x1, segment_x2) or intersection_x < min(segment_x1, segment_x2):
-      return None
-    # calculate the distance from the ray origin to the intersection point
-    return (intersection_x - ray_x) ** 2 + (segment_y1 - ray_y) ** 2
-  # if the segment is vertical, the ray will intersect it at a known x value
-  if segment_x1 == segment_x2:
-    # if the ray is vertical, it will never intersect the segment
-    if ray_heading == math.pi / 2:
-      return None
-    # calculate the y value of the intersection point 
-    intersection_y = ray_y + (segment_x1 - ray_x) * ray_tan
-    # is the intersection point on the segment?
-    if intersection_y > max(segment_y1, segment_y2) or intersection_y < min(segment_y1, segment_y2):
-      return None
-    # calculate the distance from the ray origin to the intersection point
-    return (intersection_y - ray_y) ** 2 + (segment_x1 - ray_x) ** 2 
-  else:
-    raise Exception("Segment is not horizontal or vertical")
 
-def get_ray_distance_squared_to_nearest_boundary_segment(ray):
+
-  """Return the distance from the ray origin to the intersection point along the given ray heading.
+def get_point_decay_from_nearest_segment(segments, x, y):
-  The segments are boundary lines, which will be horizontal or vertical, and have known lengths.
+  """Return the distance from the point to the nearest segment as a decay function."""
-  The ray can have any heading, and will be infinite in length.
+  max_decay = None
-  Ray -> (x, y, heading)
+  for segment in segments:
+    decay = 1.0 / max(1, get_point_distance_to_segment(x, y, segment))
+    if max_decay is None or decay > max_decay:
+      max_decay = decay
+  return max_decay
+
+
+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.
   """
-  # find the distance to each segment
+  grid = np.zeros((width // grid_cell_size + 2 * overscan, height // grid_cell_size + 2 * overscan), dtype=np.float)
-  distances = []
+  for x in range(grid.shape[0]):
-  ray_x, ray_y, ray_heading = ray
+    column_x = x * grid_cell_size - (overscan * grid_cell_size)
-  ray_tan = math.tan(ray_heading)
+    for y in range(grid.shape[1]):
-  for segment in boundary_lines:
+      value = get_point_decay_from_nearest_segment(boundary_lines, column_x, y * grid_cell_size - (overscan * grid_cell_size))
-    distance_squared = get_ray_distance_to_segment_squared(ray_x, ray_y, ray_tan, ray_heading, segment)
+      grid[x, y] = value
-    if distance_squared is not None:
+  return grid
-      distances.append(distance_squared)
+
-  # return the minimum distance
+distance_grid = make_distance_grid()
-  if distances:
+
-    return min(distances)
+def get_distance_grid_at_point(x, y):
-  else:
+  """Return the distance grid value at the given point."""
-    return None
+  grid_x = int(x // grid_cell_size + overscan)
+  grid_y = int(y // grid_cell_size + overscan)
+  if grid_x < 0 or grid_x >= distance_grid.shape[0] or grid_y < 0 or grid_y >= distance_grid.shape[1]:
+    return 0
+  return distance_grid[grid_x, grid_y]

ch-13/full-version/robot/code.py

@@ -4,19 +4,10 @@ import random
 from ulab import numpy as np
 import arena
 import robot
-import math
-import time
-
-# initial sample set - uniform
-# then apply sensor model
-# then resample
-# then apply motion model
-# and repeat
 
 class VaryingWallAvoid:
     def __init__(self):
         self.speed = 0.6
-        self.last_call = time.monotonic()
 
     def speed_from_distance(self, distance):
         limited_error = min(distance, 300) * self.speed
@@ -26,23 +17,19 @@ class VaryingWallAvoid:
         return motor_speed
 
     def update(self, left_distance, right_distance):
-        # Currently being called every 1.6 seconds - that is far too long.
-        print("Since last call:", time.monotonic() - self.last_call)
         left = self.speed_from_distance(left_distance)
         right = self.speed_from_distance(right_distance)
-        # print("left speed:", left, "right speed:", right)
         robot.set_left(left)
         robot.set_right(right)
-        self.last_call = time.monotonic()
 
-triangular_proportion = math.sqrt(6) / 2
+triangular_proportion = np.sqrt(6) / 2
 def get_triangular_sample(mean, standard_deviation):
     base = triangular_proportion * (random.uniform(-standard_deviation, standard_deviation) + random.uniform(-standard_deviation, standard_deviation))
     return mean + base
 
 class Simulation:
     def __init__(self):
-        self.population_size = 50
+        self.population_size = 100
         self.left_distance = 100
         self.right_distance = 100
         self.imu_mix = 0.3 * 0.5
@@ -61,7 +48,6 @@ class Simulation:
         self.collision_avoider = VaryingWallAvoid()
 
     async def apply_sensor_model(self):
-        # Timing is about 0.65s
         # Based on vl53l1x sensor readings, create weight for each pose.
         # vl53l1x standard dev is +/- 5 mm. Each distance is a mean reading
         # we will first determine sensor positions based on poses
@@ -69,69 +55,49 @@ class Simulation:
         # then check this projected position against occupancy grid
         # and weight accordingly
 
-        # distance sensor positions projected forward. x, y, heading, reading
+        # distance sensor positions projected forward. x, y
-        fn_start = time.monotonic()
+        distance_sensor_left = np.zeros(
-        print("Starting apply sensor model")
+            (self.poses.shape[0], 2), dtype=np.float)
-        distance_sensor_left_rays = np.zeros(
+        distance_sensor_right = np.zeros(
-            (self.poses.shape[0], 3), dtype=np.float)
+            (self.poses.shape[0], 2), dtype=np.float)
-        distance_sensor_right_rays = np.zeros(
-            (self.poses.shape[0], 3), dtype=np.float)
         # sensors - they are facing forward, either side of the robot. Project them out to the sides
         # based on each poses heading and turn sensors into rays,
         # left sensor
         poses_left_90 = np.radians(self.poses[:, 2] + 90)
-        # print("poses_left_90_shape:",poses_left_90.shape, "distance_sensor_positions_shape:",distance_sensor_positions.shape, "poses_shape:",self.poses.shape)
+        distance_sensor_left[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle
-        distance_sensor_left_rays[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle
+        distance_sensor_left[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle
-        distance_sensor_left_rays[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle
+        # now project forward by distance sensor range
-        distance_sensor_left_rays[:, 2] = np.radians(self.poses[:, 2])
+        distance_sensor_left[:, 0] += np.cos(self.poses[:, 2]) * self.left_distance
+        distance_sensor_left[:, 1] += np.sin(self.poses[:, 2]) * self.left_distance
         # right sensor
         poses_right_90 = np.radians(self.poses[:, 2] - 90)
-        distance_sensor_right_rays[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle
+        distance_sensor_right[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle
-        distance_sensor_right_rays[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle
+        distance_sensor_right[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle
-        distance_sensor_right_rays[:, 2] = np.radians(self.poses[:, 2])
+        # now project forward by distance sensor range
-        # for each sensor position, find the distance to the nearest obstacle
+        distance_sensor_right[:, 0] += np.cos(self.poses[:, 2]) * self.left_distance
-        distance_sensor_standard_dev = 5
+        distance_sensor_right[:, 1] += np.sin(self.poses[:, 2]) * self.left_distance
-        dl_squared = self.left_distance ** 2
+
-        dr_squared = self.right_distance ** 2
         await asyncio.sleep(0)
-        print("Time to calculate sensor positions:", time.monotonic() - fn_start)
-        fn_start = time.monotonic()
         # weighted poses a numpy array of weights for each pose
         weights = np.empty(self.poses.shape[0], dtype=np.float)
-        # 0.6 seconds in this loop!
         for index in range(self.poses.shape[0]):
             # remove any that are outside the arena
-            if not arena.point_is_inside_arena(self.poses[index,0], self.poses[index,1]) or \
+            if not arena.point_is_inside_arena(self.poses[index,0], self.poses[index,1]):
-                    not arena.point_is_inside_arena(distance_sensor_left_rays[index,0], distance_sensor_left_rays[index,1]) or \
-                    not arena.point_is_inside_arena(distance_sensor_right_rays[index,0], distance_sensor_right_rays[index,1]):
                 weights[index] = 0
                 continue
             # difference between this distance and the distance sensed is the error
-            # add noise to this error
-            # left sensor
-            noise = get_triangular_sample(0, distance_sensor_standard_dev)
-            left_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(distance_sensor_left_rays[index])
-            left_error = abs(left_actual - dl_squared + noise)
-            # right sensor
-            noise = get_triangular_sample(0, distance_sensor_standard_dev)
-            right_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(distance_sensor_right_rays[index])
-            right_error = abs(right_actual - dr_squared + noise)
             # weight is the inverse of the error
-            weights[index] = 1 / (left_error + right_error)
+            weights[index] = arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1])
-        print("Time to calculate pose weights", time.monotonic() - fn_start)
+            weights[index] += arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1])
         await asyncio.sleep(0)
         #normalise the weights
-        # print("Weights sum before normalising:", np.sum(weights))
         weights = weights / np.sum(weights)
-        # print("Weights sum:", np.sum(weights))
         return weights
 
     def resample(self, weights):
-        # Fast - 0.01 to 0.035 seconds
         # based on the weights, resample the poses
         # weights is a numpy array of weights
         # resample is a numpy array of indices into the poses array
-        # fn_start = time.monotonic()
         samples = []
         # use low variance resampling
         start = random.uniform(0, 1 / self.population_size)
@@ -145,7 +111,6 @@ class Simulation:
             samples.append(source_index)
         # set poses to the resampled poses
         self.poses = np.array([self.poses[n] for n in samples])
-        # print("resample time", time.monotonic() - fn_start)
 
     def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta):
         # convert odometry to motion
@@ -175,16 +140,11 @@ class Simulation:
 
     async def motion_model(self):
         """move forward, apply the motion model"""
-        # fn_start = time.monotonic()
-        # Reading sensors - 0.001 to 0.002 seconds.
         starting_heading = robot.imu.euler[0]
         encoder_left = robot.left_encoder.read()
         encoder_right = robot.right_encoder.read()
-        # print("Reading sensors time", time.monotonic() - fn_start)
 
         await asyncio.sleep(0.01)
-        # fn_start = time.monotonic()
-        # record sensor changes - 0.001 to 0.002 seconds
         rot1, trans, rot2 = self.convert_odometry_to_motion(
             robot.left_encoder.read() - encoder_left, 
             robot.right_encoder.read() - encoder_right)
@@ -199,10 +159,7 @@ class Simulation:
             rot1 = rot1 * self.encoder_mix + heading_change * self.imu_mix
             rot2 = rot2 * self.encoder_mix + heading_change * self.imu_mix
         else:
-          print("Failed to get heading")
+            print("Failed to get heading")
-        # print("Got headings time", time.monotonic() - fn_start)
-        # fn_start = time.monotonic()
-        # move poses 0.07 - 0.08 seconds
         rot1_model = np.array([get_triangular_sample(rot1, self.rotation_standard_dev) for _ in range(self.poses.shape[0])])
         trans_model = np.array([get_triangular_sample(trans, self.speed_standard_dev) for _ in range(self.poses.shape[0])])
         rot2_model = np.array([get_triangular_sample(rot2, self.rotation_standard_dev) for _ in range(self.poses.shape[0])])
@@ -213,7 +170,6 @@ class Simulation:
         self.poses[:,2] += rot2_model
         self.poses[:,2] = np.vectorize(lambda n: float(n % 360))(self.poses[:,2])
         self.poses = np.array(self.poses, dtype=np.int16)
-        # print("Move poses times", time.monotonic() - fn_start)
 
     async def distance_sensor_updater(self):
         robot.left_distance.distance_mode = 2
@@ -223,32 +179,22 @@ class Simulation:
         robot.left_distance.start_ranging()
         robot.right_distance.start_ranging()
         while True:
-            # About 0.02 seconds
-            # loop_start = time.monotonic()
             if robot.left_distance.data_ready and robot.left_distance.distance:
                 self.left_distance = robot.left_distance.distance * 10  # convert to mm
                 robot.left_distance.clear_interrupt()
             if robot.right_distance.data_ready and robot.right_distance.distance:
                 self.right_distance = robot.right_distance.distance * 10
                 robot.right_distance.clear_interrupt()
-            print("left_distance:", self.left_distance, "right_distance:", self.right_distance)
-            # move forward - with collision avoidance 0.03 to 0.04 seconds
             self.collision_avoider.update(self.left_distance, self.right_distance)
-
-            # print("distance_sensor_updater_used_time: ", time.monotonic() - loop_start)
             await asyncio.sleep(0.01)
 
     async def run(self):
         asyncio.create_task(self.distance_sensor_updater())
         try:
             while True:
-                # print("Applying sensor model")
                 weights = await self.apply_sensor_model()
-                # print("Sensor model complete.\nResampling")
                 self.resample(weights)
-                # print("Resampling complete.\nMoving robot")
                 await self.motion_model()
-                # print("Robot move complete")
         finally:
             robot.stop()
 
@@ -277,8 +223,6 @@ def read_command():
 async def updater(simulation):
     print("starting updater")
     while True:
-        loop_start = time.monotonic()
-        # Imu calibration and send - 0.0625 seconds
         sys_status, gyro, accel, mag = robot.imu.calibration_status
         if sys_status < 3:
             send_json(
@@ -291,18 +235,11 @@ async def updater(simulation):
                     }
                 }
             )
-        print("Sent imu calibration in", time.monotonic() - loop_start)
-        # The big time delay is in sending the poses.
-        print("Sending poses", simulation.poses.shape[0])
         for n in range(0, simulation.poses.shape[0], 10):
-            loop_start = time.monotonic()
-            # each pose group is 0.2 seconds.
-            # print("Sending poses from ", n, "to", n+10, "of", simulation.poses.shape[0], "poses")
             send_json({
                 "poses": simulation.poses[n:n+10].tolist(),
                 "offset": n,
             })
-            print("Sent poses in", time.monotonic() - loop_start)
             await asyncio.sleep(0.01)
         await asyncio.sleep(0.5)
 
@@ -311,7 +248,6 @@ async def command_handler(simulation):
     print("Starting handler")
     update_task = None
     simulation_task = None
-    # simulation_task = asyncio.create_task(simulation.run())
     while True:
         if robot.uart.in_waiting:
             print("Receiving data...")

ch-13/full-version/robot/test_arena.py

@@ -1,30 +0,0 @@
-from unittest import TestCase
-import math
-import arena
-
-class TestArena(TestCase):
-  def test_get_ray_distance_to_segment_squared_is_not_none(self):
-    """Use an example ray, test we get a distance squared (not none)"""
-    ray = (253.415, 85.2855, 0.479889)
-    segment = arena.boundary_lines[4]
-    distance_squared = arena.get_ray_distance_to_segment_squared(ray, segment)
-    self.assertIsNotNone(distance_squared)
-
-  def test_get_distance_squared_for_vertical_ray(self):
-    """Make a vertical ray, say at y=1000, x=500, heading=pi/2, and test we get the correct distance squared"""
-    ray = (500, 1000, math.pi / 2)
-    segment = arena.boundary_lines[1]
-    distance_squared = arena.get_ray_distance_to_segment_squared(ray, segment)
-    self.assertEqual(distance_squared, 500 ** 2)
-
-  def test_get_distance_squared_for_vertical_with_nearest_segment(self):
-    """Make a vertical ray, say at y=1000, x=500, heading=pi/2, and test we get the correct distance squared"""
-    ray = (500, 1000, math.pi / 2)
-    distance_squared = arena.get_ray_distance_squared_to_nearest_boundary_segment(ray)
-    self.assertEqual(distance_squared, 500 ** 2)
-
-  def test_get_distance_squared_for_horizontal_ray(self):
-    """Make a horizontal ray, say at y=500, x=1000, heading=0, and test we get the correct distance squared"""
-    ray = (500, 250, 0)
-    distance_squared = arena.get_ray_distance_squared_to_nearest_boundary_segment(ray)
-    self.assertEqual(distance_squared, 500 ** 2)