From f547e8e0c36fb33e4d55395c4aaee0226400bce3 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Mon, 10 Oct 2022 13:47:38 +0200 Subject: [PATCH] :sparkles: --- selflocalization/selflocalize.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index e062654..b687e68 100644 --- a/selflocalization/selflocalize.py +++ b/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