From 640db167fe563dbe6e4dfe0c5d2fb50c584ed5e3 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Wed, 5 Oct 2022 11:40:03 +0200 Subject: [PATCH] :sparkles: --- selflocalization/camera.py | 2 +- selflocalization/particle.py | 19 +++++- selflocalization/selflocalize.py | 99 +++++++++++++++++++++----------- 3 files changed, 82 insertions(+), 38 deletions(-) diff --git a/selflocalization/camera.py b/selflocalization/camera.py index a0a4fb4..e6bc367 100644 --- a/selflocalization/camera.py +++ b/selflocalization/camera.py @@ -3,7 +3,7 @@ import numpy as np import time import sys import threading -import selflocalization.framebuffer as framebuffer +import framebuffer as framebuffer from pkg_resources import parse_version diff --git a/selflocalization/particle.py b/selflocalization/particle.py index 3421c4c..5e7430c 100644 --- a/selflocalization/particle.py +++ b/selflocalization/particle.py @@ -1,6 +1,6 @@ +import random import numpy as np -import selflocalization.random_numbers as rn - +import random_numbers as rn class Particle(object): """Data structure for storing particle information (state and weight)""" @@ -34,6 +34,9 @@ class Particle(object): def setWeight(self, val): self.weight = val + def copy(self): + return Particle(self.x, self.y, self.theta, self.weight) + def estimate_pose(particles_list): """Estimate the pose from particles by computing the average position and orientation over all particles. @@ -64,7 +67,9 @@ def estimate_pose(particles_list): def move_particle(particle, delta_x, delta_y, delta_theta): """Move the particle by (delta_x, delta_y, delta_theta)""" - print("particle.py: move_particle not implemented. You should do this.") + particle.setX(particle.getX() + delta_x) + particle.setY(particle.getY() + delta_y) + particle.setTheta(particle.getTheta() + delta_theta) def add_uncertainty(particles_list, sigma, sigma_theta): @@ -83,3 +88,11 @@ def add_uncertainty_von_mises(particles_list, sigma, theta_kappa): particle.x += rn.randn(0.0, sigma) particle.y += rn.randn(0.0, sigma) particle.theta = np.mod(rn.rand_von_mises(particle.theta, theta_kappa), 2.0 * np.pi) - np.pi + +def resample(particle_list): + weights = [p.weight for p in particle_list] + if sum(weights) == 0: + weights = [1/len(particle_list) for _ in particle_list] + new_particles = random.choices(particle_list, weights, k=len(particle_list)) + particle_list = [p.copy() for p in new_particles] + return particle_list diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index df22f43..334b655 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -1,6 +1,6 @@ import cv2 -import selflocalization.particle as particle -import selflocalization.camera as camera +import particle as particle +import camera as camera import numpy as np import time from timeit import default_timer as timer @@ -8,7 +8,7 @@ import sys # Flags -showGUI = True # Whether or not to open GUI windows +showGUI = False # Whether or not to open GUI windows onRobot = True # Whether or not we are running on the Arlo robot @@ -20,8 +20,7 @@ def isRunningOnArlo(): if isRunningOnArlo(): - # XXX: You need to change this path to point to where your robot.py file is located - sys.path.append("../../../../Arlo/python") + sys.path.append("..") try: @@ -32,6 +31,14 @@ except ImportError: onRobot = False +POWER = 70 + +TURN_T = 7.9 # 1 degree +DRIVE_T = 22 # 1 centimeter + +RIGHT_WHEEL_OFFSET = 4 + +CLOCKWISE_OFFSET = 0.96 # Some color constants in BGR format @@ -46,16 +53,23 @@ CBLACK = (0, 0, 0) # Landmarks. # The robot knows the position of 2 landmarks. Their coordinates are in the unit centimeters [cm]. -landmarkIDs = [1, 2] +landmarkIDs = [4, 6] landmarks = { - 1: (0.0, 0.0), # Coordinates for landmark 1 - 2: (300.0, 0.0) # Coordinates for landmark 2 + 4: (0.0, 0.0), # Coordinates for landmark 1 + 6: (300.0, 0.0) # Coordinates for landmark 2 } landmark_colors = [CRED, CGREEN] # Colors used when drawing the landmarks +SIGMA = 1 +SIGMA_THETA = 0.2 +def normal(x, mean, std): + return ( + (np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std) + ) + def jet(x): """Colour map for drawing particles. This function determines the colour of @@ -118,6 +132,29 @@ def initialize_particles(num_particles): 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 calc_weight(particle, landmark_values): + if landmark_values == []: + return + weights = [] + for values in landmark_values: + dist_to_landmark = dist(particle, landmarks[values[0]]) + dist_weight = normal(values[1], dist_to_landmark, SIGMA) + + angle_to_landmark = calc_angle(particle, landmarks[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)) + # Main program # try: @@ -142,7 +179,7 @@ try: velocity = 0.0 # cm/sec angular_velocity = 0.0 # radians/sec - # Initialize the robot (XXX: You do this) + arlo = robot.Robot() # Allocate space for world map world = np.zeros((500,500,3), dtype=np.uint8) @@ -162,44 +199,38 @@ try: action = cv2.waitKey(10) if action == ord('q'): # Quit break - - if not isRunningOnArlo(): - if action == ord('w'): # Forward - velocity += 4.0 - elif action == ord('x'): # Backwards - velocity -= 4.0 - elif action == ord('s'): # Stop - velocity = 0.0 - angular_velocity = 0.0 - elif action == ord('a'): # Left - angular_velocity += 0.2 - elif action == ord('d'): # Right - angular_velocity -= 0.2 - - - - # Use motor controls to update particles - # XXX: Make the robot drive - # XXX: You do this + 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() - + # Detect objects objectIDs, dists, angles = cam.detect_aruco_objects(colour) if not isinstance(objectIDs, type(None)): + landmark_values = [] + # List detected objects for i in range(len(objectIDs)): print("Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i]) - # XXX: Do something for each detected object - remember, the same ID may appear several times + if objectIDs[i] in landmarkIDs: + landmark_values.append((objectIDs[i], dists[i], angles[i])) # Compute particle weights - # XXX: You do this + for p in particles: + calc_weight(p, landmark_values) # Resampling - # XXX: You do this + particles = particle.resample(particles) # Draw detected objects cam.draw_aruco_objects(colour) @@ -208,13 +239,13 @@ try: 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 if showGUI: # Draw map draw_world(est_pose, particles, world) - + # Show frame cv2.imshow(WIN_RF1, colour)