import time from turtle import right import numpy as np import cv2 from selflocalization import particle, camera import robot LANDMARKS = [6,8,9,7] LANDMARK_POSITIONS = { 6: [0, 0], 8: [300, 0], 9: [0, 400], 7: [300, 400] } POWER = 70 TURN_T = 9.6 # 1 degree DRIVE_T = 22 # 1 centimeter RIGHT_WHEEL_OFFSET = 4 CLOCKWISE_OFFSET = 0.96 FOCAL_LENGTH = 1691 CAMERA_MATRIX = np.array( [[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) SIGMA = 3 SIGMA_THETA = 0.3 NUM_PARTICLES = 1000 def look_around(noah, particles, cam, est_pose): for _ in range(24): time.sleep(0.2) # Fetch next frame for _ in range(10): 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] + 22.5, 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) # 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() for p in particles: p.setTheta(p.theta - np.deg2rad(15)) # 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 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() particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) # 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]) 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, particles): noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) start = time.time() while True: forward_dist = noah.read_front_ping_sensor() if forward_dist < 1000: noah.stop() break left_dist = noah.read_left_ping_sensor() if left_dist < 400: noah.stop() break right_dist = noah.read_right_ping_sensor() if right_dist < 400: noah.stop() break if time.time() - start > 5: noah.stop() break time.sleep(0.01) end = time.time() time_driven = end - start distance_driven = (time_driven*1000)/DRIVE_T for p in particles: x = np.cos(p.theta) * distance_driven y = np.sin(p.theta) * distance_driven p.x = p.x + x p.y = p.y + y particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) est_pose = particle.estimate_pose(particles) return time_driven, est_pose, particles def drunk_drive(noah): 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, particles): start = time.time() noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) while True: forward_dist = noah.read_front_ping_sensor() if forward_dist < 300: noah.stop() break end = time.time() time_driven = end - start distance_driven = (time_driven*1000)/DRIVE_T for p in particles: x = np.cos(p.theta) * distance_driven y = np.sin(p.theta) * distance_driven p.x = p.x + x p.y = p.y + y particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) est_pose = particle.estimate_pose(particles) return est_pose, particles 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 turn_90_degrees(noah, est_pose, particles): x_values = [i[0] for i in LANDMARK_POSITIONS.values()] y_values = [i[1] for i in LANDMARK_POSITIONS.values()] middle = np.array([max(x_values)/2, max(y_values)/2]) current_position = np.array([est_pose.x, est_pose.y]) relative_pos = middle - current_position angle = np.arctan(relative_pos[1]/relative_pos[0]) relative_angle = np.mod(angle - est_pose.theta, 2.0*np.pi) clockwise = relative_angle > 180 if clockwise: noah.go_diff(POWER, POWER, 1, 0) time.sleep((90 * TURN_T * CLOCKWISE_OFFSET)/1000) else: noah.go_diff(POWER, POWER, 0, 1) time.sleep((90 * TURN_T)/1000) noah.stop() for p in particles: if clockwise: p.setTheta(p.theta - np.deg2rad(90)) else: p.setTheta(p.theta + np.deg2rad(90)) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) _, est_pose, particles = drive_until_stopped(noah, particles) return est_pose, particles def main(noah): landmark_order = LANDMARKS + [ LANDMARKS[0] ] cam = camera.Camera(0, 'arlo', useCaptureThread=True) particles = initialize_particles(NUM_PARTICLES) # The estimate of the robots current pose est_pose = particle.estimate_pose(particles) for landmark in landmark_order: print(f"Going to landmark {landmark}") while True: particles, est_pose = look_around(noah, particles, cam, est_pose) print(est_pose) particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark) drive_time = time_to_landmark(est_pose, landmark) time_driven, est_pose, particles = drive_until_stopped(noah, particles) if not abs(time_driven - drive_time) < 0.5: # drunk_drive(noah) est_pose, particles = turn_90_degrees(noah, est_pose, particles) continue est_pose, particles = inch_closer(noah, particles) break if __name__ == "__main__": noah = robot.Robot() # Noah er vores robots navn try: main(noah) except KeyboardInterrupt: noah.stop()