NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

😼

Browse Source
This commit is contained in:
Sebastian Giovanni Murgia Kristensen
2022-10-26 13:37:40 +02:00
parent c78c596db2
commit 616d902ee2
1 changed files with 51 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
rally2.py
View File

@ -1,4 +1,5 @@
import time import time
from turtle import right
import numpy as np import numpy as np
import cv2 import cv2
@ -6,19 +7,19 @@ from selflocalization import particle, camera
import robot import robot
LANDMARKS = [7,11,10,6] LANDMARKS = [7, 11, 10, 6]
LANDMARK_POSITIONS = { LANDMARK_POSITIONS = {
7: [0,0], 7: [0, 0],
11: [300,0], 11: [300, 0],
10: [0,400], 10: [0, 400],
6: [300,400] 6: [300, 400]
} }
POWER = 70 POWER = 70
TURN_T = 7.9 # 1 degree TURN_T = 7.9 # 1 degree
DRIVE_T = 22 # 1 centimeter DRIVE_T = 22 # 1 centimeter
RIGHT_WHEEL_OFFSET = 4 RIGHT_WHEEL_OFFSET = 4
@ -27,16 +28,17 @@ CLOCKWISE_OFFSET = 0.96
FOCAL_LENGTH = 1691 FOCAL_LENGTH = 1691
CAMERA_MATRIX = np.array( CAMERA_MATRIX = np.array(
[[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360],[0,0,1]], [[FOCAL_LENGTH, 0, 512], [0, FOCAL_LENGTH, 360], [0, 0, 1]],
dtype=np.float32 dtype=np.float32
) )
DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32) DIST_COEF = np.array([0, 0, 0, 0, 0], dtype=np.float32)
SIGMA = 3 SIGMA = 3
SIGMA_THETA = 0.3 SIGMA_THETA = 0.3
NUM_PARTICLES = 1000 NUM_PARTICLES = 1000
def look_around(noah, particles, cam, est_pose): def look_around(noah, particles, cam, est_pose):
for _ in range(24): for _ in range(24):
# Fetch next frame # Fetch next frame
@ -49,7 +51,8 @@ def look_around(noah, particles, cam, est_pose):
# List detected objects # List detected objects
for i in range(len(objectIDs)): for i in range(len(objectIDs)):
print("Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i]) print(
"Object ID = ", objectIDs[i], ", Distance = ", dists[i], ", angle = ", angles[i])
if objectIDs[i] in LANDMARKS: if objectIDs[i] in LANDMARKS:
landmark_values.append((objectIDs[i], dists[i], angles[i])) landmark_values.append((objectIDs[i], dists[i], angles[i]))
@ -69,7 +72,8 @@ def look_around(noah, particles, cam, est_pose):
p.setWeight(1.0/NUM_PARTICLES) p.setWeight(1.0/NUM_PARTICLES)
particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose # The estimate of the robots current pose
est_pose = particle.estimate_pose(particles)
calc_weight(est_pose, landmark_values) calc_weight(est_pose, landmark_values)
noah.go_diff(POWER, POWER, 0, 1) noah.go_diff(POWER, POWER, 0, 1)
time.sleep((15 * TURN_T)/1000) time.sleep((15 * TURN_T)/1000)
@ -77,9 +81,11 @@ def look_around(noah, particles, cam, est_pose):
for p in particles: for p in particles:
p.setTheta(p.theta + np.deg2rad(15)) p.setTheta(p.theta + np.deg2rad(15))
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose # The estimate of the robots current pose
est_pose = particle.estimate_pose(particles)
return particles, est_pose return particles, est_pose
def turn_towards_landmark(noah, particles, est_pose, landmark): def turn_towards_landmark(noah, particles, est_pose, landmark):
current_position = np.array([est_pose.x, est_pose.y]) current_position = np.array([est_pose.x, est_pose.y])
current_theta = est_pose.theta current_theta = est_pose.theta
@ -99,10 +105,11 @@ def turn_towards_landmark(noah, particles, est_pose, landmark):
time.sleep((abs(360 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000) time.sleep((abs(360 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
noah.stop() noah.stop()
# The estimate of the robots current pose
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose est_pose = particle.estimate_pose(particles)
return particles, est_pose return particles, est_pose
def time_to_landmark(est_pose, landmark): def time_to_landmark(est_pose, landmark):
"""Kør indenfor 1 meter""" """Kør indenfor 1 meter"""
current_position = np.array([est_pose.x, est_pose.y]) current_position = np.array([est_pose.x, est_pose.y])
@ -112,19 +119,29 @@ def time_to_landmark(est_pose, landmark):
drive_distance = np.sqrt(relative_pos[0]**2 + relative_pos[1]**2) - 100 drive_distance = np.sqrt(relative_pos[0]**2 + relative_pos[1]**2) - 100
return (DRIVE_T * drive_distance)/1000 return (DRIVE_T * drive_distance)/1000
def drive_until_stopped(noah): def drive_until_stopped(noah):
noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
start = time.time() start = time.time()
while True: while True:
# TODO blind vinkel
forward_dist = noah.read_front_sensor() forward_dist = noah.read_front_sensor()
if forward_dist < 1000: if forward_dist < 1000:
noah.stop() noah.stop()
break break
left_dist = noah.read_left_sensor()
if left_dist < 400:
noah.stop()
break
right_dist = noah.read_right_sensor()
if right_dist < 400:
noah.stop()
break
time.sleep(0.01)
end = time.time() end = time.time()
return end - start return end - start
def drunk_drive(noah): def drunk_drive(noah):
start = time.time() start = time.time()
end = start + 2 end = start + 2
@ -146,6 +163,7 @@ def drunk_drive(noah):
time.sleep(0.01) time.sleep(0.01)
def inch_closer(noah): def inch_closer(noah):
noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1) noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
while True: while True:
@ -154,21 +172,26 @@ def inch_closer(noah):
noah.stop() noah.stop()
break break
def initialize_particles(num_particles): def initialize_particles(num_particles):
particles = [] particles = []
for _ in range(num_particles): for _ 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) 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) particles.append(p)
return particles return particles
def dist(particle, landmark): def dist(particle, landmark):
return np.sqrt((landmark[0]-particle.x)**2+(landmark[1]-particle.y)**2) return np.sqrt((landmark[0]-particle.x)**2+(landmark[1]-particle.y)**2)
def calc_angle(particle, landmark, dist): def calc_angle(particle, landmark, dist):
e_theta = np.array([np.cos(particle.theta), np.sin(particle.theta)]).T 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_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 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)) return np.sign(np.dot(e_landmark, e_hat_theta)) * np.arccos(np.dot(e_landmark, e_theta))
@ -178,6 +201,7 @@ def normal(x, mean, std):
(np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std) (np.exp(-(((x - mean)**2)/(2 * std**2))))/(np.sqrt(2 * np.pi) * std)
) )
def calc_weight(particle, landmark_values): def calc_weight(particle, landmark_values):
if landmark_values == []: if landmark_values == []:
return return
@ -186,12 +210,14 @@ def calc_weight(particle, landmark_values):
dist_to_landmark = dist(particle, LANDMARK_POSITIONS[values[0]]) dist_to_landmark = dist(particle, LANDMARK_POSITIONS[values[0]])
dist_weight = normal(values[1], dist_to_landmark, SIGMA) dist_weight = normal(values[1], dist_to_landmark, SIGMA)
angle_to_landmark = calc_angle(particle, LANDMARK_POSITIONS[values[0]], dist_to_landmark) angle_to_landmark = calc_angle(
particle, LANDMARK_POSITIONS[values[0]], dist_to_landmark)
angle_weight = normal(values[2], angle_to_landmark, SIGMA_THETA) angle_weight = normal(values[2], angle_to_landmark, SIGMA_THETA)
weights.append(dist_weight * angle_weight) weights.append(dist_weight * angle_weight)
particle.setWeight(np.product(weights)) particle.setWeight(np.product(weights))
def main(): def main():
landmark_order = LANDMARKS + [ landmark_order = LANDMARKS + [
LANDMARKS[0] LANDMARKS[0]
@ -199,15 +225,17 @@ def main():
particles = initialize_particles(NUM_PARTICLES) particles = initialize_particles(NUM_PARTICLES)
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose # The estimate of the robots current pose
est_pose = particle.estimate_pose(particles)
noah = robot.Robot() # Noah er vores robots navn noah = robot.Robot() # Noah er vores robots navn
cam = camera.Camera(0, 'arlo', useCaptureThread = True) cam = camera.Camera(0, 'arlo', useCaptureThread=True)
for landmark in landmark_order: for landmark in landmark_order:
while True: while True:
particles, est_pose = look_around(noah, particles, cam, est_pose) particles, est_pose = look_around(noah, particles, cam, est_pose)
particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark) particles, est_pose = turn_towards_landmark(
noah, particles, est_pose, landmark)
drive_time = time_to_landmark(est_pose, landmark) drive_time = time_to_landmark(est_pose, landmark)
if not abs(drive_until_stopped(noah) - drive_time) < 0.5: if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
drunk_drive(noah) drunk_drive(noah)
@ -217,4 +245,4 @@ def main():
if __name__ == "__main__": if __name__ == "__main__":
main() main()