315 lines
9.7 KiB
Python
315 lines
9.7 KiB
Python
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()
|