From c78c596db2131ca4fb1399c627f4e6b9297ddd20 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Wed, 26 Oct 2022 13:09:52 +0200 Subject: [PATCH] :crown: --- rally2.py | 150 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 142 insertions(+), 8 deletions(-) diff --git a/rally2.py b/rally2.py index 930927b..8c3bae0 100644 --- a/rally2.py +++ b/rally2.py @@ -2,6 +2,8 @@ import time import numpy as np import cv2 +from selflocalization import particle, camera + import robot LANDMARKS = [7,11,10,6] @@ -30,20 +32,91 @@ CAMERA_MATRIX = np.array( ) DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32) -def look_around(): - pass +SIGMA = 3 +SIGMA_THETA = 0.3 -def turn_towards_landmark(est_pose, landmark): - pass +NUM_PARTICLES = 1000 + +def look_around(noah, particles, cam, est_pose): + for _ in range(24): + # Fetch next frame + colour = cam.get_next_frame() + landmark_values = [] + + # 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])) + + # Compute particle weights + for p in particles: + calc_weight(p, landmark_values) + + # Resampling + particles = particle.resample(particles) + + # 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) + + particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) + est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + calc_weight(est_pose, landmark_values) + noah.go_diff(POWER, POWER, 0, 1) + time.sleep((15 * TURN_T)/1000) + noah.stop() + for p in particles: + p.setTheta(p.theta + np.deg2rad(15)) + + est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + return particles, est_pose + +def turn_towards_landmark(noah, particles, est_pose, landmark): + current_position = np.array([est_pose.x, est_pose.y]) + current_theta = est_pose.theta + landmark_position = np.array(LANDMARK_POSITIONS[landmark]) + + relative_pos = landmark_position - current_position + angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0])) + turn_angle = np.mod(angle - (np.rad2deg(current_theta) - 180), 360) + for p in particles: + p.setTheta(p.theta + np.deg2rad(turn_angle)) + if turn_angle < 180: + noah.go_diff(POWER, POWER, 0, 1) + time.sleep((abs(turn_angle) * TURN_T)/1000) + noah.stop() + else: + noah.go_diff(POWER, POWER, 1, 0) + time.sleep((abs(360 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000) + noah.stop() + + + est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + return particles, est_pose def time_to_landmark(est_pose, landmark): """Kør indenfor 1 meter""" - pass + current_position = np.array([est_pose.x, est_pose.y]) + landmark_position = np.array(LANDMARK_POSITIONS[landmark]) + + relative_pos = landmark_position - current_position + drive_distance = np.sqrt(relative_pos[0]**2 + relative_pos[1]**2) - 100 + return (DRIVE_T * drive_distance)/1000 def drive_until_stopped(noah): noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) start = time.time() while True: + # TODO blind vinkel forward_dist = noah.read_front_sensor() if forward_dist < 1000: noah.stop() @@ -53,7 +126,25 @@ def drive_until_stopped(noah): return end - start def drunk_drive(noah): - pass + start = time.time() + end = start + 2 + turning = None + while time.time() < end: + forward_dist = noah.read_front_ping_sensor() + right_dist = noah.read_right_ping_sensor() + left_dist = noah.read_left_ping_sensor() + if forward_dist > 600 and all(x > 400 for x in [right_dist, left_dist]): + turning = None + noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) + else: + if turning == "R" or (forward_dist > 600 and right_dist > 400 and turning is None): + noah.go_diff(POWER, POWER, 1, 0) + turning = "R" + else: + noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 0, 1) + turning = "L" + + time.sleep(0.01) def inch_closer(noah): noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) @@ -63,17 +154,60 @@ def inch_closer(noah): noah.stop() break +def initialize_particles(num_particles): + particles = [] + for _ in range(num_particles): + # Random starting points. + p = particle.Particle(600.0*np.random.ranf() - 100.0, 600.0*np.random.ranf() - 250.0, np.mod(2.0*np.pi*np.random.ranf(), 2.0*np.pi), 1.0/num_particles) + particles.append(p) + + return particles + +def dist(particle, landmark): + return np.sqrt((landmark[0]-particle.x)**2+(landmark[1]-particle.y)**2) + +def calc_angle(particle, landmark, dist): + e_theta = np.array([np.cos(particle.theta), np.sin(particle.theta)]).T + e_landmark = (np.array([landmark[0]-particle.x, landmark[1]-particle.y]).T)/dist + e_hat_theta = np.array([-np.sin(particle.theta), np.cos(particle.theta)]).T + return np.sign(np.dot(e_landmark, e_hat_theta)) * np.arccos(np.dot(e_landmark, e_theta)) + + +def normal(x, mean, std): + return ( + (np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std) + ) + +def calc_weight(particle, landmark_values): + if landmark_values == []: + return + weights = [] + for values in landmark_values: + dist_to_landmark = dist(particle, LANDMARK_POSITIONS[values[0]]) + dist_weight = normal(values[1], dist_to_landmark, SIGMA) + + angle_to_landmark = calc_angle(particle, LANDMARK_POSITIONS[values[0]], dist_to_landmark) + angle_weight = normal(values[2], angle_to_landmark, SIGMA_THETA) + weights.append(dist_weight * angle_weight) + + particle.setWeight(np.product(weights)) + def main(): landmark_order = LANDMARKS + [ LANDMARKS[0] ] + particles = initialize_particles(NUM_PARTICLES) + + est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + noah = robot.Robot() # Noah er vores robots navn + cam = camera.Camera(0, 'arlo', useCaptureThread = True) for landmark in landmark_order: while True: - est_pose = look_around() - est_pose = turn_towards_landmark(est_pose, landmark) + particles, est_pose = look_around(noah, particles, cam, 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)