✨

selflocalization/selflocalize.py

@@ -155,7 +155,7 @@ def calc_weight(particle, landmark_values):
 
     particle.setWeight(np.product(weights))
 
-def drive_to_middle(est_pose, arlo):
+def drive_towards_middle(est_pose, arlo):
     middle = np.array([150, 0])
 
     position = np.array([est_pose.x, est_pose.y])
@@ -175,7 +175,7 @@ def drive_to_middle(est_pose, arlo):
         arlo.stop()
 
     arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
-    sleep((drive_distance * DRIVE_T)/1000)
+    sleep(0.5)
     arlo.stop()
 
 # Main program #
@@ -215,8 +215,9 @@ try:
     else:
         cam = camera.Camera(0, 'macbookpro', useCaptureThread = True)
 
-    while True:
+    arlo.go_diff(POWER, POWER, 0, 1)
 
+    for _ in range(100):
         # Move the robot according to user input (only for testing)
         action = cv2.waitKey(10)
         if action == ord('q'): # Quit
@@ -245,6 +246,7 @@ try:
 
             # Draw detected objects
             cam.draw_aruco_objects(colour)
+
         else:
             # No observation - reset weights to uniform distribution
             for p in particles:
@@ -252,7 +254,6 @@ 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
@@ -264,6 +265,9 @@ try:
             # Show world
             cv2.imshow(WIN_World, world)
 
+    arlo.stop()
+    drive_towards_middle(est_pose, arlo)
+
 
 finally:
     # Make sure to clean up even if an exception occurred