diff --git a/rally2.py b/rally2.py index cb77a2b..a0a3577 100644 --- a/rally2.py +++ b/rally2.py @@ -43,39 +43,40 @@ def look_around(noah, particles, cam, est_pose): for _ in range(24): time.sleep(0.5) # Fetch next frame - colour = cam.get_next_frame() - landmark_values = [] + for _ in range(100): + colour = cam.get_next_frame() + landmark_values = [] - # Detect objects - objectIDs, dists, angles = cam.detect_aruco_objects(colour) - if not isinstance(objectIDs, type(None)): + # Detect objects + objectIDs, dists, angles = cam.detect_aruco_objects(colour) + if not isinstance(objectIDs, type(None)): - # List detected objects - for i in range(len(objectIDs)): - print( - "Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i]) - if objectIDs[i] in LANDMARKS: - landmark_values.append((objectIDs[i], dists[i], angles[i])) + # List detected objects + for i in range(len(objectIDs)): + print( + "Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i]) + if objectIDs[i] in LANDMARKS: + landmark_values.append((objectIDs[i], dists[i], angles[i])) - # Compute particle weights - for p in particles: - calc_weight(p, landmark_values) + # Compute particle weights + for p in particles: + calc_weight(p, landmark_values) - # Resampling - particles = particle.resample(particles) + # Resampling + particles = particle.resample(particles) - # Draw detected objects - cam.draw_aruco_objects(colour) + # Draw detected objects + cam.draw_aruco_objects(colour) - else: - # No observation - reset weights to uniform distribution - for p in particles: - p.setWeight(1.0/NUM_PARTICLES) + else: + # No observation - reset weights to uniform distribution + for p in particles: + p.setWeight(1.0/NUM_PARTICLES) - particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) - # The estimate of the robots current pose - est_pose = particle.estimate_pose(particles) - calc_weight(est_pose, landmark_values) + particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) + # The estimate of the robots current pose + est_pose = particle.estimate_pose(particles) + calc_weight(est_pose, landmark_values) noah.go_diff(POWER, POWER, 0, 1) time.sleep((15 * TURN_T)/1000) noah.stop()