✨

rally2.py

@@ -43,39 +43,40 @@ def look_around(noah, particles, cam, est_pose):
     for _ in range(24):
         time.sleep(0.5)
         # Fetch next frame
-        colour = cam.get_next_frame()
+        for _ in range(100):
-        landmark_values = []
+            colour = cam.get_next_frame()
+            landmark_values = []
 
-        # Detect objects
+            # Detect objects
-        objectIDs, dists, angles = cam.detect_aruco_objects(colour)
+            objectIDs, dists, angles = cam.detect_aruco_objects(colour)
-        if not isinstance(objectIDs, type(None)):
+            if not isinstance(objectIDs, type(None)):
 
-            # List detected objects
+                # List detected objects
-            for i in range(len(objectIDs)):
+                for i in range(len(objectIDs)):
-                print(
+                    print(
-                    "Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i])
+                        "Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i])
-                if objectIDs[i] in LANDMARKS:
+                    if objectIDs[i] in LANDMARKS:
-                    landmark_values.append((objectIDs[i], dists[i], angles[i]))
+                        landmark_values.append((objectIDs[i], dists[i], angles[i]))
 
-            # Compute particle weights
+                # Compute particle weights
-            for p in particles:
+                for p in particles:
-                calc_weight(p, landmark_values)
+                    calc_weight(p, landmark_values)
 
-            # Resampling
+                # Resampling
-            particles = particle.resample(particles)
+                particles = particle.resample(particles)
 
-            # Draw detected objects
+                # Draw detected objects
-            cam.draw_aruco_objects(colour)
+                cam.draw_aruco_objects(colour)
 
-        else:
+            else:
-            # No observation - reset weights to uniform distribution
+                # No observation - reset weights to uniform distribution
-            for p in particles:
+                for p in particles:
-                p.setWeight(1.0/NUM_PARTICLES)
+                    p.setWeight(1.0/NUM_PARTICLES)
 
-        particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+            particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
-        # The estimate of the robots current pose
+            # The estimate of the robots current pose
-        est_pose = particle.estimate_pose(particles)
+            est_pose = particle.estimate_pose(particles)
-        calc_weight(est_pose, landmark_values)
+            calc_weight(est_pose, landmark_values)
         noah.go_diff(POWER, POWER, 0, 1)
         time.sleep((15 * TURN_T)/1000)
         noah.stop()