NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-26 14:30:13 +02:00
parent 0e46b0dbe2
commit ef5f297a47
2 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
rally2.py
View File

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

3
selflocalization/particle.py
View File

@ -37,6 +37,9 @@ class Particle(object):
def copy(self): def copy(self):
return Particle(self.x, self.y, self.theta, self.weight) 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): def estimate_pose(particles_list):
"""Estimate the pose from particles by computing the average position and orientation over all particles. """Estimate the pose from particles by computing the average position and orientation over all particles.