Bugfixing monte carlo

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

@@ -13,6 +13,8 @@ boundary_lines = [
 width = 1500
 height = 1500
 
+# need to state clearly the orientation of the heading
+# if coordinates 0, 0 is bottom left, then heading 0 is right, with heading increasing anticlockwise.
 
 def point_is_inside_arena(x, y):
   """Return True if the point is inside the arena.
@@ -46,7 +48,7 @@ def get_ray_distance_to_segment_squared(ray, segment):
     # calculate the x value of the intersection point
     intersection_x = ray_x + (segment_y1 - ray_y) / math.tan(ray_heading)
     # is the intersection point on the segment?
-    if intersection_x < segment_x1 or intersection_x > segment_x2:
+    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
@@ -58,10 +60,10 @@ def get_ray_distance_to_segment_squared(ray, segment):
     # calculate the y value of the intersection point 
     intersection_y = ray_y + (segment_x1 - ray_x) * math.tan(ray_heading)
     # is the intersection point on the segment?
-    if intersection_y < segment_y1 or intersection_y > segment_y2:
+    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 (segment_x1 - ray_x) ** 2 + (intersection_y - ray_y) ** 2
+    return (intersection_y - ray_y) ** 2 + (segment_x1 - ray_x) ** 2 
   else:
     raise Exception("Segment is not horizontal or vertical")
 

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

@@ -18,7 +18,6 @@ class Simulation:
         self.population_size = 200
         self.left_distance = 100
         self.right_distance = 100
-        self.occupancy_grid = arena.get_binary_occupancy_grid()
         self.time_step = 0.1
         # Poses - each an array of [x, y, heading]
         self.poses = np.array(
@@ -33,7 +32,7 @@ class Simulation:
         self.turn_pid = pid_controller.PIDController(0.1, 0.01, 0.01)
         self.distance_aim = 100
 
-    def apply_sensor_model(self, distance_left, distance_right):
+    def apply_sensor_model(self):
         # 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
@@ -42,12 +41,13 @@ class Simulation:
         # and weight accordingly
 
         # distance sensor positions projected forward. left_x, left_y, right_x, right_y
-        distance_sensor_positions = np.array(
-            (self.poses.shape[0], 6), dtype=np.float)
+        distance_sensor_positions = np.zeros(
+            (self.poses.shape[0], 4), dtype=np.float)
         # sensors - they are facing forward, either side of the robot. Project them out to the sides
         # based on each poses heading
         # 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_positions[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle
         distance_sensor_positions[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle
         # right sensor
@@ -56,8 +56,8 @@ class Simulation:
         distance_sensor_positions[:, 3] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle
         # for each sensor position, find the distance to the nearest obstacle
         distance_sensor_standard_dev = 5
-        dl_squared = distance_left ** 2
-        dr_squared = distance_right ** 2
+        dl_squared = self.left_distance ** 2
+        dr_squared = self.right_distance ** 2
 
         # weighted poses a numpy array of weights for each pose
         weights = np.empty(self.poses.shape[0], dtype=np.float)
@@ -69,15 +69,21 @@ class Simulation:
                 weights[index] = 0
                 continue
             # left sensor
-            left_ray = sensor_position[0], sensor_position[1], self.poses[index, 2]
+            left_ray = sensor_position[0], sensor_position[1], np.radians(self.poses[index, 2])
             noise = get_gaussian_sample(0, distance_sensor_standard_dev)
-            left_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(left_ray) + noise
-            left_error = abs(left_actual - dl_squared) # error
+            left_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(left_ray)
+            if left_actual is None:
+                print("left_actual is None. Ray was ", left_ray)
+                left_actual = 100
+            left_error = abs(left_actual - dl_squared + noise) # error
             # right sensor
-            right_ray = sensor_position[2], sensor_position[3], self.poses[index, 2]
+            right_ray = sensor_position[2], sensor_position[3], np.radians(self.poses[index, 2])
             noise = get_gaussian_sample(0, distance_sensor_standard_dev)
-            right_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(right_ray) + noise
-            right_error = abs(right_actual - dr_squared) #error
+            right_actual = arena.get_ray_distance_squared_to_nearest_boundary_segment(right_ray)
+            if right_actual is None:
+                print("right_actual is None. Ray was ", right_ray)
+                right_actual = 100
+            right_error = abs(right_actual - dr_squared + noise) #error
             # weight is the inverse of the error
             weights[index] = 1 / (left_error + right_error)
 

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

@@ -0,0 +1,30 @@
+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)