✨

selflocalization/selflocalize.py

@@ -199,7 +199,7 @@ try:
     num_particles = 2000
     particles = initialize_particles(num_particles)
 
-    est_pose, _ = particle.estimate_pose(particles) # The estimate of the robots current pose
+    est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
 
     # Driving parameters
     velocity = 0.0 # cm/sec
@@ -252,21 +252,20 @@ try:
                 p.setWeight(1.0/num_particles)
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
-        est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
+        est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
         calc_weight(est_pose, landmark_values)
         est_weight = est_pose.weight * 10**10
         print("est_weight:", est_weight)
-        print("certainty:", certainty)
         if est_weight > 1:
             draw_world(est_pose, particles, world)
             cv2.imwrite("test.png", world)
             break
-        else:
-            arlo.go_diff(POWER, POWER, 0, 1)
-            sleep((15 * TURN_T)/1000)
-            arlo.stop()
-            for p in particles:
-                p.setTheta(p.theta + np.deg2rad(15))
+        # else:
+        #     arlo.go_diff(POWER, POWER, 0, 1)
+        #     sleep((15 * TURN_T)/1000)
+        #     arlo.stop()
+        #     for p in particles:
+        #         p.setTheta(p.theta + np.deg2rad(15))
 
         if showGUI:
             # Draw map