✨

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)
@@ -162,44 +199,38 @@ try:
         action = cv2.waitKey(10)
         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
-            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.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
+            arlo.go_diff(POWER, POWER, 0, 1)
+        elif action == ord('e'): # Right
+            arlo.go_diff(POWER, POWER, 1, 0)
 
 
         # Fetch next frame
         colour = cam.get_next_frame()
-        
+
         # 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,13 +239,13 @@ 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:
             # Draw map
             draw_world(est_pose, particles, world)
-    
+
             # Show frame
             cv2.imshow(WIN_RF1, colour)