👑

rally2.py

@@ -2,6 +2,8 @@ import time
 import numpy as np
 import cv2
 
+from selflocalization import particle, camera
+
 import robot
 
 LANDMARKS = [7,11,10,6]
@@ -30,20 +32,91 @@ CAMERA_MATRIX = np.array(
 )
 DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32)
 
-def look_around():
-    pass
+SIGMA = 3
+SIGMA_THETA = 0.3
 
-def turn_towards_landmark(est_pose, landmark):
-    pass
+NUM_PARTICLES = 1000
+
+def look_around(noah, particles, cam, est_pose):
+    for _ in range(24):
+        # Fetch next frame
+        colour = cam.get_next_frame()
+        landmark_values = []
+
+        # Detect objects
+        objectIDs, dists, angles = cam.detect_aruco_objects(colour)
+        if not isinstance(objectIDs, type(None)):
+
+            # List detected objects
+            for i in range(len(objectIDs)):
+                print("Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i])
+                if objectIDs[i] in LANDMARKS:
+                    landmark_values.append((objectIDs[i], dists[i], angles[i]))
+
+            # Compute particle weights
+            for p in particles:
+                calc_weight(p, landmark_values)
+
+            # Resampling
+            particles = particle.resample(particles)
+
+            # Draw detected objects
+            cam.draw_aruco_objects(colour)
+
+        else:
+            # No observation - reset weights to uniform distribution
+            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
+        calc_weight(est_pose, landmark_values)
+        noah.go_diff(POWER, POWER, 0, 1)
+        time.sleep((15 * TURN_T)/1000)
+        noah.stop()
+        for p in particles:
+            p.setTheta(p.theta + np.deg2rad(15))
+
+    est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+    return particles, est_pose
+
+def turn_towards_landmark(noah, particles, est_pose, landmark):
+    current_position = np.array([est_pose.x, est_pose.y])
+    current_theta = est_pose.theta
+    landmark_position = np.array(LANDMARK_POSITIONS[landmark])
+
+    relative_pos = landmark_position - current_position
+    angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0]))
+    turn_angle = np.mod(angle - (np.rad2deg(current_theta) - 180), 360)
+    for p in particles:
+        p.setTheta(p.theta + np.deg2rad(turn_angle))
+    if turn_angle < 180:
+        noah.go_diff(POWER, POWER, 0, 1)
+        time.sleep((abs(turn_angle) * TURN_T)/1000)
+        noah.stop()
+    else:
+        noah.go_diff(POWER, POWER, 1, 0)
+        time.sleep((abs(360 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
+        noah.stop()
+
+
+    est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+    return particles, est_pose
 
 def time_to_landmark(est_pose, landmark):
     """Kør indenfor 1 meter"""
-    pass
+    current_position = np.array([est_pose.x, est_pose.y])
+    landmark_position = np.array(LANDMARK_POSITIONS[landmark])
+
+    relative_pos = landmark_position - current_position
+    drive_distance = np.sqrt(relative_pos[0]**2 + relative_pos[1]**2) - 100
+    return (DRIVE_T * drive_distance)/1000
 
 def drive_until_stopped(noah):
     noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
     start = time.time()
     while True:
+        # TODO blind vinkel
         forward_dist = noah.read_front_sensor()
         if forward_dist < 1000:
             noah.stop()
@@ -53,7 +126,25 @@ def drive_until_stopped(noah):
     return end - start
 
 def drunk_drive(noah):
-    pass
+    start = time.time()
+    end = start + 2
+    turning = None
+    while time.time() < end:
+        forward_dist = noah.read_front_ping_sensor()
+        right_dist = noah.read_right_ping_sensor()
+        left_dist = noah.read_left_ping_sensor()
+        if forward_dist > 600 and all(x > 400 for x in [right_dist, left_dist]):
+            turning = None
+            noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+        else:
+            if turning == "R" or (forward_dist > 600 and right_dist > 400 and turning is None):
+                noah.go_diff(POWER, POWER, 1, 0)
+                turning = "R"
+            else:
+                noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 0, 1)
+                turning = "L"
+
+        time.sleep(0.01)
 
 def inch_closer(noah):
     noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
@@ -63,17 +154,60 @@ def inch_closer(noah):
             noah.stop()
             break
 
+def initialize_particles(num_particles):
+    particles = []
+    for _ in range(num_particles):
+        # 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)
+
+    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 normal(x, mean, std):
+    return (
+        (np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std)
+    )
+
+def calc_weight(particle, landmark_values):
+    if landmark_values == []:
+        return
+    weights = []
+    for values in landmark_values:
+        dist_to_landmark = dist(particle, LANDMARK_POSITIONS[values[0]])
+        dist_weight = normal(values[1], dist_to_landmark, SIGMA)
+
+        angle_to_landmark = calc_angle(particle, LANDMARK_POSITIONS[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))
+
 def main():
     landmark_order = LANDMARKS + [
         LANDMARKS[0]
     ]
 
+    particles = initialize_particles(NUM_PARTICLES)
+
+    est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+
     noah = robot.Robot() # Noah er vores robots navn
+    cam = camera.Camera(0, 'arlo', useCaptureThread = True)
 
     for landmark in landmark_order:
         while True:
-            est_pose = look_around()
-            est_pose = turn_towards_landmark(est_pose, landmark)
+            particles, est_pose = look_around(noah, particles, cam, est_pose)
+            particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark)
             drive_time = time_to_landmark(est_pose, landmark)
             if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
                 drunk_drive(noah)