From ef5f297a47baebdc9ff44f5269e7fc3d95099a16 Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Wed, 26 Oct 2022 14:30:13 +0200
Subject: [PATCH] :sparkles:

---
 rally2.py                    | 4 ++--
 selflocalization/particle.py | 3 +++
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/rally2.py b/rally2.py
index b516150..e83c929 100644
--- a/rally2.py
+++ b/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)
diff --git a/selflocalization/particle.py b/selflocalization/particle.py
index bef0a49..0b304ec 100644
--- a/selflocalization/particle.py
+++ b/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.