✨

2022-10-10 14:37:09 +02:00
parent b3aaf40487
commit a0eeeba787
1 changed files with 9 additions and 1 deletions
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@ -254,8 +254,16 @@ try:
        particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
        est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
        calc_weight(est_pose, landmark_values)
-        if est_pose.weight > 10**-23:
+        est_weight = est_pose.weight * 10**23
+        print(est_weight)
+        if est_weight > 1:
            break
+        else:
+            arlo.go_diff(POWER, POWER, 0, 1)
+            sleep((10 * TURN_T)/1000)
+            arlo.stop()
+            for p in particles:
+                p.setTheta(p.theta + np.deg2rad(10))

        if showGUI:
            # Draw map