✨

selflocalization/camera.py

@@ -3,7 +3,7 @@ import numpy as np
 import time
 import sys
 import threading
-import selflocalization.framebuffer as framebuffer
+import framebuffer as framebuffer
 from pkg_resources import parse_version
 
 

selflocalization/particle.py

@@ -1,6 +1,6 @@
+import random
 import numpy as np
-import selflocalization.random_numbers as rn
-
+import random_numbers as rn
 
 class Particle(object):
     """Data structure for storing particle information (state and weight)"""
@@ -34,6 +34,9 @@ class Particle(object):
     def setWeight(self, val):
         self.weight = val
 
+    def copy(self):
+        return Particle(self.x, self.y, self.theta, self.weight)
+
 
 def estimate_pose(particles_list):
     """Estimate the pose from particles by computing the average position and orientation over all particles. 
@@ -64,7 +67,9 @@ def estimate_pose(particles_list):
      
 def move_particle(particle, delta_x, delta_y, delta_theta):
     """Move the particle by (delta_x, delta_y, delta_theta)"""
-    print("particle.py: move_particle not implemented. You should do this.") 
+    particle.setX(particle.getX() + delta_x)
+    particle.setY(particle.getY() + delta_y)
+    particle.setTheta(particle.getTheta() + delta_theta)
 
 
 def add_uncertainty(particles_list, sigma, sigma_theta):
@@ -83,3 +88,11 @@ def add_uncertainty_von_mises(particles_list, sigma, theta_kappa):
         particle.x += rn.randn(0.0, sigma)
         particle.y += rn.randn(0.0, sigma)
         particle.theta = np.mod(rn.rand_von_mises(particle.theta, theta_kappa), 2.0 * np.pi) - np.pi
+
+def resample(particle_list):
+    weights = [p.weight for p in particle_list]
+    if sum(weights) == 0:
+        weights = [1/len(particle_list) for _ in particle_list]
+    new_particles = random.choices(particle_list, weights, k=len(particle_list))
+    particle_list = [p.copy() for p in new_particles]
+    return particle_list

selflocalization/selflocalize.py

@@ -1,6 +1,6 @@
 import cv2
-import selflocalization.particle as particle
-import selflocalization.camera as camera
+import particle as particle
+import camera as camera
 import numpy as np
 import time
 from timeit import default_timer as timer
@@ -8,7 +8,7 @@ import sys
 
 
 # Flags
-showGUI = True  # Whether or not to open GUI windows
+showGUI = False  # Whether or not to open GUI windows
 onRobot = True # Whether or not we are running on the Arlo robot
 
 
@@ -20,8 +20,7 @@ def isRunningOnArlo():
 
 
 if isRunningOnArlo():
-    # XXX: You need to change this path to point to where your robot.py file is located
-    sys.path.append("../../../../Arlo/python")
+    sys.path.append("..")
 
 
 try:
@@ -32,6 +31,14 @@ except ImportError:
     onRobot = False
 
 
+POWER = 70
+
+TURN_T = 7.9 # 1 degree
+DRIVE_T = 22 # 1 centimeter
+
+RIGHT_WHEEL_OFFSET = 4
+
+CLOCKWISE_OFFSET = 0.96
 
 
 # Some color constants in BGR format
@@ -46,16 +53,23 @@ CBLACK = (0, 0, 0)
 
 # Landmarks.
 # The robot knows the position of 2 landmarks. Their coordinates are in the unit centimeters [cm].
-landmarkIDs = [1, 2]
+landmarkIDs = [4, 6]
 landmarks = {
-    1: (0.0, 0.0),  # Coordinates for landmark 1
-    2: (300.0, 0.0)  # Coordinates for landmark 2
+    4: (0.0, 0.0),  # Coordinates for landmark 1
+    6: (300.0, 0.0)  # Coordinates for landmark 2
 }
 landmark_colors = [CRED, CGREEN] # Colors used when drawing the landmarks
 
 
+SIGMA = 1
+SIGMA_THETA = 0.2
 
 
+def normal(x, mean, std):
+    return (
+        (np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std)
+    )
+
 
 def jet(x):
     """Colour map for drawing particles. This function determines the colour of 
@@ -118,6 +132,29 @@ def initialize_particles(num_particles):
 
     return particles
 
+def dist(particle, landmark):
+    return np.sqrt((landmark[0]-particle.x)**2+(landmark[1]-particle.y)**2)
+
+def calc_angle(particle, landmark, dist):
+    e_theta = np.array([np.cos(particle.theta), np.sin(particle.theta)]).T
+    e_landmark = (np.array([landmark[0]-particle.x, landmark[1]-particle.y]).T)/dist
+    e_hat_theta = np.array([-np.sin(particle.theta), np.cos(particle.theta)]).T
+    return np.sign(np.dot(e_landmark, e_hat_theta)) * np.arccos(np.dot(e_landmark, e_theta))
+
+def calc_weight(particle, landmark_values):
+    if landmark_values == []:
+        return
+    weights = []
+    for values in landmark_values:
+        dist_to_landmark = dist(particle, landmarks[values[0]])
+        dist_weight = normal(values[1], dist_to_landmark, SIGMA)
+
+        angle_to_landmark = calc_angle(particle, landmarks[values[0]], dist_to_landmark)
+        angle_weight = normal(values[2], angle_to_landmark, SIGMA_THETA)
+        weights.append(dist_weight * angle_weight)
+
+    particle.setWeight(np.product(weights))
+
 
 # Main program #
 try:
@@ -142,7 +179,7 @@ try:
     velocity = 0.0 # cm/sec
     angular_velocity = 0.0 # radians/sec
 
-    # Initialize the robot (XXX: You do this)
+    arlo = robot.Robot()
 
     # Allocate space for world map
     world = np.zeros((500,500,3), dtype=np.uint8)
@@ -163,25 +200,15 @@ try:
         if action == ord('q'): # Quit
             break
 
-        if not isRunningOnArlo():
-            if action == ord('w'): # Forward
-                velocity += 4.0
-            elif action == ord('x'): # Backwards
-                velocity -= 4.0
-            elif action == ord('s'): # Stop
-                velocity = 0.0
-                angular_velocity = 0.0
+        arlo.stop()
+        if action == ord(','): # Forward
+            arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+        elif action == ord('o'): # Backwards
+            arlo.go_diff(POWER, POWER, 0, 0)
         elif action == ord('a'): # Left
-                angular_velocity += 0.2
-            elif action == ord('d'): # Right
-                angular_velocity -= 0.2
-
-
-
-        
-        # Use motor controls to update particles
-        # XXX: Make the robot drive
-        # XXX: You do this
+            arlo.go_diff(POWER, POWER, 0, 1)
+        elif action == ord('e'): # Right
+            arlo.go_diff(POWER, POWER, 1, 0)
 
 
         # Fetch next frame
@@ -190,16 +217,20 @@ try:
         # Detect objects
         objectIDs, dists, angles = cam.detect_aruco_objects(colour)
         if not isinstance(objectIDs, type(None)):
+            landmark_values = []
+
             # List detected objects
             for i in range(len(objectIDs)):
                 print("Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i])
-                # XXX: Do something for each detected object - remember, the same ID may appear several times
+                if objectIDs[i] in landmarkIDs:
+                    landmark_values.append((objectIDs[i], dists[i], angles[i]))
 
             # Compute particle weights
-            # XXX: You do this
+            for p in particles:
+                calc_weight(p, landmark_values)
 
             # Resampling
-            # XXX: You do this
+            particles = particle.resample(particles)
 
             # Draw detected objects
             cam.draw_aruco_objects(colour)
@@ -208,7 +239,7 @@ try:
             for p in particles:
                 p.setWeight(1.0/num_particles)
 
-    
+        particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
         est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
 
         if showGUI: