From 8c2a835e01eebb8b415136c4c5a6eb3fa773f9de Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Mon, 10 Oct 2022 13:39:05 +0200 Subject: [PATCH] :sparkles: --- selflocalization/selflocalize.py | 66 +++++++++++++++++++------------- 1 file changed, 39 insertions(+), 27 deletions(-) diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index 334b655..bd2fd23 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -2,13 +2,13 @@ import cv2 import particle as particle import camera as camera import numpy as np -import time +from time import sleep from timeit import default_timer as timer import sys # Flags -showGUI = False # Whether or not to open GUI windows +showGUI = True # Whether or not to open GUI windows onRobot = True # Whether or not we are running on the Arlo robot @@ -53,16 +53,16 @@ CBLACK = (0, 0, 0) # Landmarks. # The robot knows the position of 2 landmarks. Their coordinates are in the unit centimeters [cm]. -landmarkIDs = [4, 6] +landmarkIDs = [1, 5] landmarks = { - 4: (0.0, 0.0), # Coordinates for landmark 1 - 6: (300.0, 0.0) # Coordinates for landmark 2 + 1: (0.0, 0.0), # Coordinates for landmark 1 + 5: (300.0, 0.0) # Coordinates for landmark 2 } landmark_colors = [CRED, CGREEN] # Colors used when drawing the landmarks -SIGMA = 1 -SIGMA_THETA = 0.2 +SIGMA = 1.5 +SIGMA_THETA = 0.3 def normal(x, mean, std): @@ -72,7 +72,7 @@ def normal(x, mean, std): def jet(x): - """Colour map for drawing particles. This function determines the colour of + """Colour map for drawing particles. This function determines the colour of a particle from its weight.""" r = (x >= 3.0/8.0 and x < 5.0/8.0) * (4.0 * x - 3.0/2.0) + (x >= 5.0/8.0 and x < 7.0/8.0) + (x >= 7.0/8.0) * (-4.0 * x + 9.0/2.0) g = (x >= 1.0/8.0 and x < 3.0/8.0) * (4.0 * x - 1.0/2.0) + (x >= 3.0/8.0 and x < 5.0/8.0) + (x >= 5.0/8.0 and x < 7.0/8.0) * (-4.0 * x + 7.0/2.0) @@ -104,7 +104,7 @@ def draw_world(est_pose, particles, world): y = ymax - (int(particle.getY() + offsetY)) colour = jet(particle.getWeight() / max_weight) cv2.circle(world, (x,y), 2, colour, 2) - b = (int(particle.getX() + 15.0*np.cos(particle.getTheta()))+offsetX, + b = (int(particle.getX() + 15.0*np.cos(particle.getTheta()))+offsetX, ymax - (int(particle.getY() + 15.0*np.sin(particle.getTheta()))+offsetY)) cv2.line(world, (x,y), b, colour, 2) @@ -116,7 +116,7 @@ def draw_world(est_pose, particles, world): # Draw estimated robot pose a = (int(est_pose.getX())+offsetX, ymax-(int(est_pose.getY())+offsetY)) - b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offsetX, + b = (int(est_pose.getX() + 15.0*np.cos(est_pose.getTheta()))+offsetX, ymax-(int(est_pose.getY() + 15.0*np.sin(est_pose.getTheta()))+offsetY)) cv2.circle(world, a, 5, CMAGENTA, 2) cv2.line(world, a, b, CMAGENTA, 2) @@ -126,7 +126,7 @@ def draw_world(est_pose, particles, world): def initialize_particles(num_particles): particles = [] for i in range(num_particles): - # Random starting points. + # 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) @@ -155,6 +155,28 @@ def calc_weight(particle, landmark_values): particle.setWeight(np.product(weights)) +def drive_to_middle(est_pose, arlo): + middle = np.array([150, 0]) + + position = np.array([est_pose.x, est_pose.y]) + theta = est_pose.theta + + relative_pos = middle - position + angle = np.arctan(relative_pos[1]/relative_pos[0]) + turn_angle = np.mod(np.rad2deg(angle - theta), 360) + drive_distance = np.sqrt(position[0]**2 + position[1]**2) + if turn_angle < 180: + arlo.go_diff(POWER, POWER, 0, 1) + sleep((abs(turn_angle) * TURN_T)/1000) + arlo.stop() + else: + arlo.go_diff(POWER, POWER, 1, 0) + sleep((abs(180 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000) + arlo.stop() + + arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) + sleep((drive_distance * DRIVE_T)/1000) + arlo.stop() # Main program # try: @@ -170,7 +192,7 @@ try: # Initialize particles - num_particles = 1000 + num_particles = 2000 particles = initialize_particles(num_particles) est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose @@ -200,17 +222,6 @@ try: if action == ord('q'): # Quit break - arlo.stop() - if action == ord(','): # Forward - arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) - elif action == ord('o'): # Backwards - arlo.go_diff(POWER, POWER, 0, 0) - elif action == ord('a'): # Left - arlo.go_diff(POWER, POWER, 0, 1) - elif action == ord('e'): # Right - arlo.go_diff(POWER, POWER, 1, 0) - - # Fetch next frame colour = cam.get_next_frame() @@ -241,6 +252,7 @@ try: particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose + drive_to_middle(est_pose, arlo) if showGUI: # Draw map @@ -251,11 +263,11 @@ try: # Show world cv2.imshow(WIN_World, world) - - -finally: + + +finally: # Make sure to clean up even if an exception occurred - + # Close all windows cv2.destroyAllWindows()