✨

selflocalization/selflocalize.py

@@ -215,17 +215,12 @@ try:
     else:
         cam = camera.Camera(0, 'macbookpro', useCaptureThread = True)
 
-    arlo.go_diff(40, 40, 0, 1)
 
-    for i in range(100):
-        # Move the robot according to user input (only for testing)
-        print(i)
-        action = cv2.waitKey(10)
-        if action == ord('q'): # Quit
-            break
+    while True:
 
         # Fetch next frame
-        colour = cam.get_next_frame()
+        # colour = cam.get_next_frame()
+        colour = arlo.take_photo()
 
         # Detect objects
         objectIDs, dists, angles = cam.detect_aruco_objects(colour)
@@ -254,7 +249,8 @@ try:
                 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
+        est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
+        print(certainty)
 
         if showGUI:
             # Draw map
@@ -266,8 +262,7 @@ try:
             # Show world
             cv2.imshow(WIN_World, world)
 
-    arlo.stop()
-    drive_towards_middle(est_pose, arlo)
+    # drive_towards_middle(est_pose, arlo)
 
 
 finally: