✨

rally2.py

@@ -235,8 +235,8 @@ def main():
         print(f"Going to landmark {landmark}")
         while True:
             particles, est_pose = look_around(noah, particles, cam, est_pose)
-            particles, est_pose = turn_towards_landmark(
-                noah, particles, est_pose, landmark)
+            print(est_pose)
+            particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark)
             drive_time = time_to_landmark(est_pose, landmark)
             if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
                 drunk_drive(noah)

selflocalization/particle.py

@@ -37,6 +37,9 @@ class Particle(object):
     def copy(self):
         return Particle(self.x, self.y, self.theta, self.weight)
 
+    def __str__(self) -> str:
+        return f"({self.x},{self.y},{self.theta})"
+
 
 def estimate_pose(particles_list):
     """Estimate the pose from particles by computing the average position and orientation over all particles.