:sparkles

drive_to_aruco.py

@@ -52,6 +52,7 @@ def main():
 
     angle = np.rad2deg(np.arctan(position[0]/position[2]))
     drive_distance = np.sqrt(position[0]**2 + position[2]**2)
+    print(drive_distance)
     if angle < 0:
         arlo.go_diff(POWER, POWER, 0, 1)
         sleep((abs(angle) * TURN_T)/1000)
@@ -61,9 +62,9 @@ def main():
         sleep((abs(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()
+    # arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+    # sleep((drive_distance * DRIVE_T)/1000)
+    # arlo.stop()
 
 
 if __name__ == "__main__":

rally.py

@@ -5,16 +5,16 @@ import cv2
 
 import robot
 
-LANDMARKS = [7,11,10,6] # SET ARUCO NUMBERS
+LANDMARKS = [1,2,3,4] # SET ARUCO NUMBERS
 
 POWER = 70
 
-TURN_T = 7.9 # 1 degree
+TURN_T = 8.3 # 1 degree
 DRIVE_T = 22 # 1 centimeter
 
-RIGHT_WHEEL_OFFSET = 4
+RIGHT_WHEEL_OFFSET = 5
 
-CLOCKWISE_OFFSET = 0.96
+CLOCKWISE_OFFSET = 0.8
 
 FOCAL_LENGTH = 1691
 
@@ -38,21 +38,49 @@ def find_aruco(image):
 
     return {ids[i][0]: box[0] for i, box in enumerate(corners)}
 
-def careful_forward(drive_time, arlo, careful_range = 600):
+
+def drunk_drive(drive_time, arlo, careful_range = 600):
     start = time.time()
+
     end = start + drive_time
     turning = None
-    hit_something = False
     while time.time() < end:
         forward_dist = arlo.read_front_ping_sensor()
         right_dist = arlo.read_right_ping_sensor()
         left_dist = arlo.read_left_ping_sensor()
-        if forward_dist > careful_range and all(x > 400 for x in [right_dist, left_dist]):
+        if forward_dist > careful_range and all(x > 250 for x in [right_dist, left_dist]):
+            turning = None
+            arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+        else:
+            if turning == "R" or (forward_dist > careful_range and right_dist > 250 and turning is None):
+                arlo.go_diff(POWER, POWER, 1, 0)
+                turning = "R"
+            else:
+                arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 0, 1)
+                turning = "L"
+
+        time.sleep(0.01)
+
+def careful_forward(drive_time, arlo, careful_range = 600):
+    start = time.time()
+    if drive_time > 0.8:
+        drive_time = 0.8
+        hit_something = True
+    else:
+        hit_something = False
+
+    end = start + drive_time
+    turning = None
+    while time.time() < end:
+        forward_dist = arlo.read_front_ping_sensor()
+        right_dist = arlo.read_right_ping_sensor()
+        left_dist = arlo.read_left_ping_sensor()
+        if forward_dist > careful_range and all(x > 250 for x in [right_dist, left_dist]):
             turning = None
             arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
         else:
             hit_something = True
-            if turning == "R" or (forward_dist > careful_range and right_dist > 400 and turning is None):
+            if turning == "R" or (forward_dist > careful_range and right_dist > 250 and turning is None):
                 arlo.go_diff(POWER, POWER, 1, 0)
                 turning = "R"
             else:
@@ -87,17 +115,15 @@ def main():
                 if landmark in arucos:
                     break
 
-                careful_forward(3, noah)
+                drunk_drive(3, noah)
 
 
             position = cv2.aruco.estimatePoseSingleMarkers(
                 np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
             )[1][0][0]
 
-            angle = np.rad2deg(np.arctan(position[0]/position[2]))
-            drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 30
-            if drive_distance < 30:
-                break
+            angle = np.rad2deg(np.arctan(position[0]/position[2])) - 5
+            drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 100
 
             if angle < 0:
                 noah.go_diff(POWER, POWER, 0, 1)
@@ -109,7 +135,20 @@ def main():
                 noah.stop()
 
             if not careful_forward((drive_distance * DRIVE_T)/1000, noah, 400):
-                break
+                arucos = find_aruco(noah.take_photo())
+                if landmark in arucos:
+                    position = cv2.aruco.estimatePoseSingleMarkers(
+                        np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
+                    )[1][0][0]
+                    drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 25
+                    noah.go_diff(POWER, POWER, 1, 1)
+                    time.sleep((drive_distance * DRIVE_T)/1000)
+                    noah.stop()
+
+                    break
+
 
 if __name__ == "__main__":
+    start = time.time()
     main()
+    print(f"Drove for {time.time() - start} seconds")

rally2.py

@@ -7,18 +7,18 @@ from selflocalization import particle, camera
 
 import robot
 
-LANDMARKS = [7, 11, 10, 6]
+LANDMARKS = [6,8,9,7]
 
 LANDMARK_POSITIONS = {
-    7: [0, 0],
-    11: [200, 0],
-    10: [0, 300],
-    6: [200, 300]
+    6: [0, 0],
+    8: [300, 0],
+    9: [0, 400],
+    7: [300, 400]
 }
 
 POWER = 70
 
-TURN_T = 9.1  # 1 degree
+TURN_T = 9.6  # 1 degree
 DRIVE_T = 22  # 1 centimeter
 
 RIGHT_WHEEL_OFFSET = 4
@@ -41,45 +41,47 @@ NUM_PARTICLES = 1000
 
 def look_around(noah, particles, cam, est_pose):
     for _ in range(24):
+        time.sleep(0.2)
         # Fetch next frame
-        colour = cam.get_next_frame()
-        landmark_values = []
+        for _ in range(10):
+            colour = cam.get_next_frame()
+            landmark_values = []
 
-        # Detect objects
-        objectIDs, dists, angles = cam.detect_aruco_objects(colour)
-        if not isinstance(objectIDs, type(None)):
+            # 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]))
+                # 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] + 22.5, angles[i]))
 
-            # Compute particle weights
-            for p in particles:
-                calc_weight(p, landmark_values)
+                # Compute particle weights
+                for p in particles:
+                    calc_weight(p, landmark_values)
 
-            # Resampling
-            particles = particle.resample(particles)
+                # Resampling
+                particles = particle.resample(particles)
 
-            # Draw detected objects
-            cam.draw_aruco_objects(colour)
+                # 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)
+            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)
-        # The estimate of the robots current pose
-        est_pose = particle.estimate_pose(particles)
-        calc_weight(est_pose, landmark_values)
+            particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+            # The estimate of the robots current pose
+            est_pose = particle.estimate_pose(particles)
+            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))
+            p.setTheta(p.theta - np.deg2rad(15))
 
     # The estimate of the robots current pose
     est_pose = particle.estimate_pose(particles)
@@ -95,7 +97,7 @@ def turn_towards_landmark(noah, particles, est_pose, landmark):
     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))
+        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)
@@ -105,6 +107,7 @@ def turn_towards_landmark(noah, particles, est_pose, landmark):
         time.sleep((abs(360 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
         noah.stop()
 
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
     # The estimate of the robots current pose
     est_pose = particle.estimate_pose(particles)
     return particles, est_pose
@@ -120,7 +123,7 @@ def time_to_landmark(est_pose, landmark):
     return (DRIVE_T * drive_distance)/1000
 
 
-def drive_until_stopped(noah):
+def drive_until_stopped(noah, particles):
     noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
     start = time.time()
     while True:
@@ -136,10 +139,24 @@ def drive_until_stopped(noah):
         if right_dist < 400:
             noah.stop()
             break
+        if time.time() - start > 5:
+            noah.stop()
+            break
         time.sleep(0.01)
 
     end = time.time()
-    return end - start
+    time_driven = end - start
+    distance_driven = (time_driven*1000)/DRIVE_T
+    for p in particles:
+        x = np.cos(p.theta) * distance_driven
+        y = np.sin(p.theta) * distance_driven
+        p.x = p.x + x
+        p.y = p.y + y
+
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+    est_pose = particle.estimate_pose(particles)
+
+    return time_driven, est_pose, particles
 
 
 def drunk_drive(noah):
@@ -164,7 +181,8 @@ def drunk_drive(noah):
         time.sleep(0.01)
 
 
-def inch_closer(noah):
+def inch_closer(noah, particles):
+    start = time.time()
     noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
     while True:
         forward_dist = noah.read_front_ping_sensor()
@@ -172,6 +190,19 @@ def inch_closer(noah):
             noah.stop()
             break
 
+    end = time.time()
+    time_driven = end - start
+    distance_driven = (time_driven*1000)/DRIVE_T
+    for p in particles:
+        x = np.cos(p.theta) * distance_driven
+        y = np.sin(p.theta) * distance_driven
+        p.x = p.x + x
+        p.y = p.y + y
+
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+    est_pose = particle.estimate_pose(particles)
+    return est_pose, particles
+
 
 def initialize_particles(num_particles):
     particles = []
@@ -217,33 +248,67 @@ def calc_weight(particle, landmark_values):
 
     particle.setWeight(np.product(weights))
 
+def turn_90_degrees(noah, est_pose, particles):
+    x_values = [i[0] for i in LANDMARK_POSITIONS.values()]
+    y_values = [i[1] for i in LANDMARK_POSITIONS.values()]
+    middle = np.array([max(x_values)/2, max(y_values)/2])
 
-def main():
+    current_position = np.array([est_pose.x, est_pose.y])
+    relative_pos = middle - current_position
+    angle = np.arctan(relative_pos[1]/relative_pos[0])
+    relative_angle = np.mod(angle - est_pose.theta, 2.0*np.pi)
+
+    clockwise = relative_angle > 180
+
+    if clockwise:
+        noah.go_diff(POWER, POWER, 1, 0)
+        time.sleep((90 * TURN_T * CLOCKWISE_OFFSET)/1000)
+    else:
+        noah.go_diff(POWER, POWER, 0, 1)
+        time.sleep((90 * TURN_T)/1000)
+
+    noah.stop()
+
+    for p in particles:
+        if clockwise:
+            p.setTheta(p.theta - np.deg2rad(90))
+        else:
+            p.setTheta(p.theta + np.deg2rad(90))
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+    _, est_pose, particles = drive_until_stopped(noah, particles)
+    return est_pose, particles
+
+
+def main(noah):
     landmark_order = LANDMARKS + [
         LANDMARKS[0]
     ]
-
+    cam = camera.Camera(0, 'arlo', useCaptureThread=True)
     particles = initialize_particles(NUM_PARTICLES)
 
     # The estimate of the robots current pose
     est_pose = particle.estimate_pose(particles)
 
-    noah = robot.Robot()  # Noah er vores robots navn
-    cam = camera.Camera(0, 'arlo', useCaptureThread=True)
-
     for landmark in landmark_order:
         print(f"Going to landmark {landmark}")
         while True:
             particles, est_pose = look_around(noah, particles, cam, est_pose)
-            particles, est_pose = turn_towards_landmark(
-                noah, particles, est_pose, landmark)
+            print(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)
+            time_driven, est_pose, particles = drive_until_stopped(noah, particles)
+            if not abs(time_driven - drive_time) < 0.5:
+                # drunk_drive(noah)
+                est_pose, particles = turn_90_degrees(noah, est_pose, particles)
                 continue
-            inch_closer(noah)
+
+            est_pose, particles = inch_closer(noah, particles)
             break
 
 
 if __name__ == "__main__":
-    main()
+    noah = robot.Robot()  # Noah er vores robots navn
+    try:
+        main(noah)
+    except KeyboardInterrupt:
+        noah.stop()

robot/__init__.py

@@ -1,2 +1 @@
 from .robot import Robot
-from .arlo import Arlo

selflocalization/camera.py

@@ -114,13 +114,14 @@ class Camera(object):
 
         # Set camera calibration info
         if robottype == 'arlo':
-            self.imageSize = (1280, 720)
+            focal_length = 1050
+            self.imageSize = (1024, 720)
             #self.intrinsic_matrix = np.asarray([ 7.1305391967046853e+02, 0., 3.1172820723774367e+02, 0.,
             #       7.0564929862291285e+02, 2.5634470978315028e+02, 0., 0., 1. ], dtype = np.float64)
             #self.intrinsic_matrix = np.asarray([ 6.0727040957659040e+02, 0., 3.0757300398967601e+02, 0.,
             #       6.0768864690145904e+02, 2.8935674612358201e+02, 0., 0., 1. ], dtype = np.float64)
-            self.intrinsic_matrix = np.asarray([500, 0., self.imageSize[0] / 2.0, 0.,
-                   500, self.imageSize[1] / 2.0, 0., 0., 1.], dtype = np.float64)
+            self.intrinsic_matrix = np.asarray([focal_length, 0., self.imageSize[0] / 2.0, 0.,
+                   focal_length, self.imageSize[1] / 2.0, 0., 0., 1.], dtype = np.float64)
             self.intrinsic_matrix.shape = (3, 3)
             #self.distortion_coeffs = np.asarray([ 1.1911006165076067e-01, -1.0003366233413549e+00,
             #       1.9287903277399834e-02, -2.3728201444308114e-03, -2.8137265581326476e-01 ], dtype = np.float64)

selflocalization/particle.py

@@ -37,6 +37,9 @@ class Particle(object):
     def copy(self):
         return Particle(self.x, self.y, self.theta, self.weight)
 
+    def __str__(self) -> str:
+        return f"({self.x},{self.y},{self.theta})"
+
 
 def estimate_pose(particles_list):
     """Estimate the pose from particles by computing the average position and orientation over all particles.