✨
This commit is contained in:
NikolajDanger
@ -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
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user