From f2e3a7f56a4c8aaa8727e26f2db8345418ddf95b Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 10 Oct 2022 14:07:57 +0200
Subject: [PATCH] :sparkles:

---
 selflocalization/selflocalize.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index 0721080..7780709 100644
--- 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: