From 616d902ee26c3cd6e9e9cf0788459522fd771496 Mon Sep 17 00:00:00 2001 From: Sebastian Giovanni Murgia Kristensen Date: Wed, 26 Oct 2022 13:37:40 +0200 Subject: [PATCH] :smirk_cat: --- rally2.py | 74 ++++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 51 insertions(+), 23 deletions(-) diff --git a/rally2.py b/rally2.py index 8c3bae0..32a7ad4 100644 --- a/rally2.py +++ b/rally2.py @@ -1,4 +1,5 @@ import time +from turtle import right import numpy as np import cv2 @@ -6,19 +7,19 @@ from selflocalization import particle, camera import robot -LANDMARKS = [7,11,10,6] +LANDMARKS = [7, 11, 10, 6] LANDMARK_POSITIONS = { - 7: [0,0], - 11: [300,0], - 10: [0,400], - 6: [300,400] + 7: [0, 0], + 11: [300, 0], + 10: [0, 400], + 6: [300, 400] } POWER = 70 -TURN_T = 7.9 # 1 degree -DRIVE_T = 22 # 1 centimeter +TURN_T = 7.9 # 1 degree +DRIVE_T = 22 # 1 centimeter RIGHT_WHEEL_OFFSET = 4 @@ -27,16 +28,17 @@ CLOCKWISE_OFFSET = 0.96 FOCAL_LENGTH = 1691 CAMERA_MATRIX = np.array( - [[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360],[0,0,1]], + [[FOCAL_LENGTH, 0, 512], [0, FOCAL_LENGTH, 360], [0, 0, 1]], dtype=np.float32 ) -DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32) +DIST_COEF = np.array([0, 0, 0, 0, 0], dtype=np.float32) SIGMA = 3 SIGMA_THETA = 0.3 NUM_PARTICLES = 1000 + def look_around(noah, particles, cam, est_pose): for _ in range(24): # Fetch next frame @@ -49,7 +51,8 @@ def look_around(noah, particles, cam, est_pose): # List detected objects for i in range(len(objectIDs)): - print("Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i]) + 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])) @@ -69,7 +72,8 @@ def look_around(noah, particles, cam, est_pose): 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 + # 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) @@ -77,9 +81,11 @@ def look_around(noah, particles, cam, est_pose): for p in particles: p.setTheta(p.theta + np.deg2rad(15)) - est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + # The estimate of the robots current pose + est_pose = particle.estimate_pose(particles) 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 @@ -99,10 +105,11 @@ def turn_towards_landmark(noah, particles, est_pose, landmark): 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 + # The estimate of the robots current pose + est_pose = particle.estimate_pose(particles) return particles, est_pose + def time_to_landmark(est_pose, landmark): """Kør indenfor 1 meter""" current_position = np.array([est_pose.x, est_pose.y]) @@ -112,19 +119,29 @@ def time_to_landmark(est_pose, landmark): 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() break + left_dist = noah.read_left_sensor() + if left_dist < 400: + noah.stop() + break + right_dist = noah.read_right_sensor() + if right_dist < 400: + noah.stop() + break + time.sleep(0.01) end = time.time() return end - start + def drunk_drive(noah): start = time.time() end = start + 2 @@ -146,6 +163,7 @@ def drunk_drive(noah): time.sleep(0.01) + def inch_closer(noah): noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) while True: @@ -154,21 +172,26 @@ 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) + 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_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)) @@ -178,6 +201,7 @@ def normal(x, mean, std): (np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std) ) + def calc_weight(particle, landmark_values): if landmark_values == []: return @@ -186,12 +210,14 @@ def calc_weight(particle, 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_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] @@ -199,15 +225,17 @@ def main(): particles = initialize_particles(NUM_PARTICLES) - est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + # The estimate of the robots current pose + est_pose = particle.estimate_pose(particles) - noah = robot.Robot() # Noah er vores robots navn - cam = camera.Camera(0, 'arlo', useCaptureThread = True) + noah = robot.Robot() # Noah er vores robots navn + cam = camera.Camera(0, 'arlo', useCaptureThread=True) for landmark in landmark_order: while True: particles, est_pose = look_around(noah, particles, cam, est_pose) - particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark) + 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) @@ -217,4 +245,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main()