✨

2022-10-10 14:07:57 +02:00
parent b0e4bcf85c
commit f2e3a7f56a
1 changed files with 4 additions and 2 deletions
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@ -249,7 +249,9 @@ try:

        particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
        est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
-        print(certainty)
+        print("Certainty: ", certainty)
+        if certainty < 1:
+            break

        if showGUI:
            # Draw map
@ -261,7 +263,7 @@ try:
            # Show world
            cv2.imshow(WIN_World, world)

-    # drive_towards_middle(est_pose, arlo)
+    drive_towards_middle(est_pose, arlo)


 finally: