✨

selflocalization/selflocalize.py

@@ -2,13 +2,13 @@ import cv2
 import particle as particle
 import camera as camera
 import numpy as np
-import time
+from time import sleep
 from timeit import default_timer as timer
 import sys
 
 
 # Flags
-showGUI = False  # Whether or not to open GUI windows
+showGUI = True  # Whether or not to open GUI windows
 onRobot = True # Whether or not we are running on the Arlo robot
 
 
@@ -53,16 +53,16 @@ CBLACK = (0, 0, 0)
 
 # Landmarks.
 # The robot knows the position of 2 landmarks. Their coordinates are in the unit centimeters [cm].
-landmarkIDs = [4, 6]
+landmarkIDs = [1, 5]
 landmarks = {
-    4: (0.0, 0.0),  # Coordinates for landmark 1
-    6: (300.0, 0.0)  # Coordinates for landmark 2
+    1: (0.0, 0.0),  # Coordinates for landmark 1
+    5: (300.0, 0.0)  # Coordinates for landmark 2
 }
 landmark_colors = [CRED, CGREEN] # Colors used when drawing the landmarks
 
 
-SIGMA = 1
-SIGMA_THETA = 0.2
+SIGMA = 1.5
+SIGMA_THETA = 0.3
 
 
 def normal(x, mean, std):
@@ -72,7 +72,7 @@ def normal(x, mean, std):
 
 
 def jet(x):
-    """Colour map for drawing particles. This function determines the colour of 
+    """Colour map for drawing particles. This function determines the colour of
     a particle from its weight."""
     r = (x >= 3.0/8.0 and x < 5.0/8.0) * (4.0 * x - 3.0/2.0) + (x >= 5.0/8.0 and x < 7.0/8.0) + (x >= 7.0/8.0) * (-4.0 * x + 9.0/2.0)
     g = (x >= 1.0/8.0 and x < 3.0/8.0) * (4.0 * x - 1.0/2.0) + (x >= 3.0/8.0 and x < 5.0/8.0) + (x >= 5.0/8.0 and x < 7.0/8.0) * (-4.0 * x + 7.0/2.0)
@@ -104,7 +104,7 @@ def draw_world(est_pose, particles, world):
         y = ymax - (int(particle.getY() + offsetY))
         colour = jet(particle.getWeight() / max_weight)
         cv2.circle(world, (x,y), 2, colour, 2)
-        b = (int(particle.getX() + 15.0*np.cos(particle.getTheta()))+offsetX, 
+        b = (int(particle.getX() + 15.0*np.cos(particle.getTheta()))+offsetX,
                                      ymax - (int(particle.getY() + 15.0*np.sin(particle.getTheta()))+offsetY))
         cv2.line(world, (x,y), b, colour, 2)
 
@@ -116,7 +116,7 @@ def draw_world(est_pose, particles, world):
 
     # Draw estimated robot pose
     a = (int(est_pose.getX())+offsetX, ymax-(int(est_pose.getY())+offsetY))
-    b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offsetX, 
+    b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offsetX,
                                  ymax-(int(est_pose.getY() + 15.0*np.sin(est_pose.getTheta()))+offsetY))
     cv2.circle(world, a, 5, CMAGENTA, 2)
     cv2.line(world, a, b, CMAGENTA, 2)
@@ -126,7 +126,7 @@ def draw_world(est_pose, particles, world):
 def initialize_particles(num_particles):
     particles = []
     for i in range(num_particles):
-        # Random starting points. 
+        # Random starting points.
         p = particle.Particle(600.0*np.random.ranf() - 100.0, 600.0*np.random.ranf() - 250.0, np.mod(2.0*np.pi*np.random.ranf(), 2.0*np.pi), 1.0/num_particles)
         particles.append(p)
 
@@ -155,6 +155,28 @@ def calc_weight(particle, landmark_values):
 
     particle.setWeight(np.product(weights))
 
+def drive_to_middle(est_pose, arlo):
+    middle = np.array([150, 0])
+
+    position = np.array([est_pose.x, est_pose.y])
+    theta = est_pose.theta
+
+    relative_pos = middle - position
+    angle = np.arctan(relative_pos[1]/relative_pos[0])
+    turn_angle = np.mod(np.rad2deg(angle - theta), 360)
+    drive_distance = np.sqrt(position[0]**2 + position[1]**2)
+    if turn_angle < 180:
+        arlo.go_diff(POWER, POWER, 0, 1)
+        sleep((abs(turn_angle) * TURN_T)/1000)
+        arlo.stop()
+    else:
+        arlo.go_diff(POWER, POWER, 1, 0)
+        sleep((abs(180 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
+        arlo.stop()
+
+    arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+    sleep((drive_distance * DRIVE_T)/1000)
+    arlo.stop()
 
 # Main program #
 try:
@@ -170,7 +192,7 @@ try:
 
 
     # Initialize particles
-    num_particles = 1000
+    num_particles = 2000
     particles = initialize_particles(num_particles)
 
     est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
@@ -200,17 +222,6 @@ try:
         if action == ord('q'): # Quit
             break
 
-        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()
 
@@ -241,6 +252,7 @@ try:
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
         est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+        drive_to_middle(est_pose, arlo)
 
         if showGUI:
             # Draw map
@@ -251,11 +263,11 @@ try:
 
             # Show world
             cv2.imshow(WIN_World, world)
-    
-  
-finally: 
+
+
+finally:
     # Make sure to clean up even if an exception occurred
-    
+
     # Close all windows
     cv2.destroyAllWindows()