def stop(wait):
global nextmove
global currentmove
global est_pose
# send stop command to the adrino
# calculate and apply the movemt to the particles
msg = 's'
serialRead.write(msg.encode('ascii'))
runtime = time.time() - movestarttime
waittime = time.time() + wait
if runtime > 1.0:
if currentmove == "F":
if runtime < 1.2:
distance = 0
else:
distance = forward_time_2_cm(runtime)
for p in particles:
x = p.getX() + (np.cos(p.getTheta()) * distance)
y = p.getY() + ((-np.sin(p.getTheta())) * distance)
p.setX(x)
p.setY(y)
add_uncertainty(particles, 5, 0.2)
elif currentmove == "H":
distance = cw_time_2_deg(runtime) * (np.pi / 180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta - distance)
add_uncertainty(particles, 1, 0.2)
elif currentmove == "L":
distance = ccw_time_2_deg(runtime) * (np.pi / 180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta + distance)
add_uncertainty(particles, 1, 0.2)
currentmove = "0"
est_pose = estimate_pose(particles)
def stop(wait):
global nextmove
global currentmove
global est_pose
# send stop command to the adrino
# calculate and apply the movemt to the particles
msg = 's'
serialRead.write(msg.encode('ascii'))
runtime = time.time() - movestarttime
waittime = time.time() + wait
if runtime > 1.0:
if currentmove == "F":
if runtime < 1.2:
distance = 0
else:
distance = forward_time_2_cm(runtime)
for p in particles:
x = p.getX() + (np.cos(p.getTheta()) * distance)
y = p.getY() + ((-np.sin(p.getTheta())) * distance)
p.setX(x)
p.setY(y)
add_uncertainty(particles, 5, 0.2)
elif currentmove == "H":
distance = cw_time_2_deg(runtime) * (np.pi/180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta - distance)
add_uncertainty(particles, 1, 0.2)
elif currentmove == "L":
distance = ccw_time_2_deg(runtime) * (np.pi/180)
for p in particles:
theta = p.getTheta()
p.setTheta(theta + distance)
add_uncertainty(particles, 1, 0.2)
currentmove = "0"
est_pose = estimate_pose(particles)
cv2.namedWindow(WIN_World)
cv2.moveWindow(WIN_World, 500, 50)
# Initialize particles
num_particles = 250
particles = []
for i in range(num_particles):
# Random starting points. (x,y) \in [-1000, 1000]^2, theta \in [-pi, pi].
p = par.Particle(2000.0 * np.random.ranf() - 1000,
2000.0 * np.random.ranf() - 1000,
2.0 * np.pi * np.random.ranf() - np.pi,
1.0 / num_particles)
#p = particle.Particle(2000.0*np.random.ranf() - 1000, 2000.0*np.random.ranf() - 1000, np.pi+3.0*np.pi/4.0, 1.0/num_particles)
particles.append(p)
est_pose = par.estimate_pose(
particles) # The estimate of the robots current pose
# Driving parameters
velocity = 0.0
# cm/sec
angular_velocity = 0.0
# radians/sec
# Initialize the robot (XXX: You do this)
arlo = robot.Robot()
lastSeenLM = None
LMInSight = False
lastMeasuredAngle = 0
translationInOneSecond = 100
rotationInOneSecond = 0.79 # 1.13826
weightMean = 0
def detect_objects():
global goals
global target
global particles
global landmark_x
global landmark_y
# Detect objects
if DEMO:
objectType, measured_distance, measured_angle, colourProb = cam.get_object(
colour)
else:
objectType = 'vertical'
measured_distance = 100
measured_angle = deg_2_rad(90.0)
colourProb = (0, 0, 0)
if objectType != 'none':
""" FOR EXAM """
for goal in goals:
# if we find one of the goals
if goal.match(objectType, colourProb):
# calculate position of the goal
x = np.cos(measured_angle) * measured_distance
y = -np.sin(measured_angle) * measured_distance
print measured_distance
# set its position
goal.setPosition(x, y)
goal.setTheta(measured_angle)
# announce that we're clever enough to identify which goal and where it is :)
print "Found", goal.getIdentifier(), "at (", goal.getX(
), ",", goal.getY(), "), theta: ", goal.getTheta()
landmark_x = goal.getUX()
landmark_y = goal.getUY()
# is it the one we want to visit?
if goal == goals[0]:
target = goal
# Compute particle weights
sigma_D = 15
sigma_A = 0.2
for p in particles:
X = p.getX()
Y = p.getY()
if (X - landmark_x) == 0 and (Y - landmark_y) == 0:
p.setWeight(0)
else:
distance = np.sqrt((X - landmark_x) * (X - landmark_x) +
(Y - landmark_y) * (Y - landmark_y))
theta = p.getTheta()
dx = np.cos(theta)
dy = -np.sin(theta)
lx = landmark_x - X
ly = landmark_y - Y
angle = (lx * dx + ly * dy) / (np.sqrt(lx * lx + ly * ly) *
np.sqrt(lx * lx + ly * ly))
if angle < -1:
angle = -1
elif angle > 1:
angle = 1
D = gaussian_distribution(measured_distance, distance, sigma_D)
A = gaussian_distribution(measured_angle, np.arccos(angle),
sigma_A)
p.setWeight(D * A)
# Resampling
particles = resample(particles, 100)
# Draw detected pattern
if DEMO:
cam.draw_object(colour)
else:
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0 / num_particles)
est_pose = estimate_pose(
particles) # The estimate of the robots current pose
def movecommand(command, wait):
global nextmove
global waittime
global currentmove
global movestarttime
emergency = False
#sends command to the adrino, saves the current time and command.
if command == "F":
msg = 'r1\n'
serialRead.write(msg.encode('ascii'))
front = serialRead.readline()
print "infrared sensor read: ", front
front_val = float(front)
if front_val > 10.0 and front_val < 80:
est_pose = est_pose = estimate_pose(particles)
a = np.cos(est_pose.getTheta()) * front_val + est_pose.getX()
b = -np.sin(est_pose.getTheta()) * front_val + est_pose.getY()
too_close = False
for goal in goals:
minx = minimum(a, goal.getUX())
miny = minimum(b, goal.getUY())
maxx = maximum(a, goal.getUX())
maxy = maximum(b, goal.getUY())
da = maxx - minx
db = maxy - miny
dc = np.sqrt(a * a + b * b)
if dc < 75:
too_close = True
break
if not too_close:
avoid.append(Landmark("box", (a, b), GREEN, "irrelvant"))
if not checkpath(0.0, wait):
notrouble = False
if inside_bubble:
msg = 'p 8 1.0'
else:
offroad = 0
while not notrouble and offroad < 4:
if offroad < 0:
offroad = (-offroad) + 1
else:
offroad = -offroad
notrouble = checkpath(offroad, wait)
if notrouble:
if offroad < 0:
turn = "H"
turnlength = cw_deg_2_time(rad_2_deg(offroad))
else:
turn = "L"
turnlength = ccw_deg_2_time(rad_2_deg(offroad))
nextmove = [[turn, turnlength], ["F", wait]]
emergency = True
else:
msg = 'p 8.0 1.0'
msg = 'p 8 1.0'
elif command == "L":
msg = 'p 300.0 1.0'
elif command == "H":
msg = 'p -310.0 1.0'
else:
print "WARNING: unknown move command!"
if not emergency:
if DEMO:
serialRead.write(msg.encode('ascii'))
movestarttime = time.time()
if len(nextmove) == 0:
nextmove = [["S", 0.3]]
else:
nextmove.pop(0)
nextmove.insert(0, ["S", 0.3])
currentmove = command
waittime = time.time() + wait
cv2.namedWindow(WIN_World)
cv2.moveWindow(WIN_World, 500, 50)
# Initialize particles
num_particles = 250
particles = []
for i in range(num_particles):
# Random starting points. (x,y) \in [-1000, 1000]^2, theta \in [-pi, pi].
p = par.Particle(2000.0 * np.random.ranf() - 1000, 2000.0 * np.random.ranf() -
1000, 2.0 * np.pi * np.random.ranf() - np.pi, 1.0 / num_particles)
#p = particle.Particle(2000.0*np.random.ranf() - 1000, 2000.0*np.random.ranf() - 1000, np.pi+3.0*np.pi/4.0, 1.0/num_particles)
particles.append(p)
# The estimate of the robots current pose
est_pose = par.estimate_pose(particles)
# Driving parameters
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)
# Draw map
draw_world(est_pose, particles, world)
WIN_World = "World view";
cv2.namedWindow(WIN_World);
cv2.moveWindow(WIN_World, 500 , 50);
# Initialize particles
num_particles = 1000
particles = []
for i in range(num_particles):
# Random starting points. (x,y) \in [-1000, 1000]^2, theta \in [-pi, pi].
p = particle.Particle(2000.0*np.random.ranf() - 1000, 2000.0*np.random.ranf() - 1000, 2.0*np.pi*np.random.ranf() - np.pi, 1.0/num_particles)
particles.append(p)
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
# Driving parameters
velocity = 0.0; # cm/sec
angular_velocity = 0.0; # radians/sec
# Initialize the robot (XXX: You do this)
# Allocate space for world map
world = np.zeros((500,500,3), dtype=np.uint8)
# Draw map
draw_world(est_pose, particles, world)
print "Opening and initializing camera"
def detect_objects():
global goals
global target
global particles
global landmark_x
global landmark_y
# Detect objects
if DEMO:
objectType, measured_distance, measured_angle, colourProb = cam.get_object(colour)
else:
objectType = 'vertical'
measured_distance = 100
measured_angle = deg_2_rad(90.0)
colourProb = (0,0,0)
if objectType != 'none':
""" FOR EXAM """
for goal in goals:
# if we find one of the goals
if goal.match(objectType, colourProb):
# calculate position of the goal
x = np.cos(measured_angle) * measured_distance
y = -np.sin(measured_angle) * measured_distance
print measured_distance
# set its position
goal.setPosition(x, y)
goal.setTheta(measured_angle)
# announce that we're clever enough to identify which goal and where it is :)
print "Found", goal.getIdentifier(), "at (", goal.getX(), ",", goal.getY(), "), theta: ", goal.getTheta()
landmark_x = goal.getUX()
landmark_y = goal.getUY()
# is it the one we want to visit?
if goal == goals[0]:
target = goal
# Compute particle weights
sigma_D = 15
sigma_A = 0.2
for p in particles:
X = p.getX()
Y = p.getY()
if (X - landmark_x) == 0 and (Y - landmark_y) == 0:
p.setWeight(0)
else:
distance = np.sqrt((X - landmark_x) * (X - landmark_x) + (Y - landmark_y) * (Y - landmark_y))
theta = p.getTheta()
dx = np.cos(theta)
dy = -np.sin(theta)
lx = landmark_x - X
ly = landmark_y - Y
angle = (lx*dx + ly*dy) / (np.sqrt(lx * lx + ly * ly) * np.sqrt(lx * lx + ly * ly))
if angle < -1:
angle = -1
elif angle > 1:
angle = 1
D = gaussian_distribution(measured_distance, distance, sigma_D)
A = gaussian_distribution(measured_angle, np.arccos(angle), sigma_A)
p.setWeight(D * A)
# Resampling
particles = resample(particles, 100)
# Draw detected pattern
if DEMO:
cam.draw_object(colour)
else:
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0/num_particles)
est_pose = estimate_pose(particles) # The estimate of the robots current pose
def movecommand (command, wait):
global nextmove
global waittime
global currentmove
global movestarttime
emergency = False
#sends command to the adrino, saves the current time and command.
if command == "F":
msg = 'r1\n'
serialRead.write(msg.encode('ascii'))
front = serialRead.readline()
print "infrared sensor read: ", front
front_val = float(front)
if front_val > 10.0 and front_val < 80:
est_pose = est_pose = estimate_pose(particles)
a = np.cos(est_pose.getTheta()) * front_val + est_pose.getX()
b = -np.sin(est_pose.getTheta()) * front_val + est_pose.getY()
too_close = False
for goal in goals:
minx = minimum(a, goal.getUX())
miny = minimum(b, goal.getUY())
maxx = maximum(a, goal.getUX())
maxy = maximum(b, goal.getUY())
da = maxx - minx
db = maxy - miny
dc = np.sqrt(a * a + b * b)
if dc < 75:
too_close = True
break
if not too_close:
avoid.append(Landmark("box", (a,b), GREEN, "irrelvant"))
if not checkpath(0.0, wait):
notrouble = False
if inside_bubble:
msg = 'p 8 1.0'
else:
offroad = 0
while not notrouble and offroad < 4:
if offroad < 0:
offroad = (-offroad) + 1
else:
offroad = -offroad
notrouble = checkpath(offroad,wait)
if notrouble:
if offroad < 0:
turn = "H"
turnlength = cw_deg_2_time(rad_2_deg(offroad))
else:
turn = "L"
turnlength = ccw_deg_2_time(rad_2_deg(offroad))
nextmove = [[turn, turnlength],["F",wait]]
emergency = True
else:
msg = 'p 8.0 1.0'
msg = 'p 8 1.0'
elif command == "L":
msg = 'p 300.0 1.0'
elif command == "H":
msg = 'p -310.0 1.0'
else:
print "WARNING: unknown move command!"
if not emergency:
if DEMO:
serialRead.write(msg.encode('ascii'))
movestarttime = time.time()
if len(nextmove) == 0:
nextmove = [["S", 0.3]]
else:
nextmove.pop(0)
nextmove.insert(0, ["S", 0.3])
currentmove = command
waittime = time.time() + wait
def update_particles(particles, cam, velocity, angular_velocity, world,
WIN_RF1, WIN_World):
#print 'update: ' + str(angular_velocity)
cv2.waitKey(4)
num_particles = len(particles)
for p in particles:
# calculates new orientation
curr_angle = add_to_angular_v2(np.degrees(p.getTheta()),
angular_velocity)
if velocity > 0.0:
[x, y] = move_vector(p, velocity)
particle.move_particle(p, x, y, curr_angle)
else:
particle.move_particle(p, 0.0, 0.0, curr_angle)
if velocity != 0.0:
particle.add_uncertainty(particles, 12, 5)
if velocity == 0.0 and angular_velocity != 0.0:
particle.add_uncertainty(particles, 0, 5)
# Fetch next frame
colour, distorted = cam.get_colour()
# Detect objects
objectType, measured_distance, measured_angle, colourProb = cam.get_object(
colour)
if objectType != 'none':
obs_landmark = ret_landmark(colourProb, objectType)
observed_obj = [
objectType, measured_distance, measured_angle, obs_landmark
]
list_of_particles = weight_particles(particles,
np.degrees(measured_angle),
measured_distance, obs_landmark)
particles = resample_particles(list_of_particles[:, [0, 2]])
particle.add_uncertainty(particles, 15, 5)
cam.draw_object(colour)
else:
observed_obj = [None, None, None, None]
for p in particles:
p.setWeight(1.0 / num_particles)
particle.add_uncertainty(particles, 10, 5)
est_pose = particle.estimate_pose(particles)
draw_world(est_pose, particles, world)
#Show frame
cv2.imshow(WIN_RF1, colour)
#Show world
cv2.imshow(WIN_World, world)
return {
'est_pos': est_pose,
'obs_obj': observed_obj,
'particles': particles
}
def estimate_position(particles):
return particle.estimate_pose(particles)
def update_particles(particles, cam, velocity, angular_velocity, world,
WIN_RF1, WIN_World):
raw_input()
print 'update: ' + str(angular_velocity)
cv2.waitKey(4)
num_particles = len(particles)
for p in particles:
# calculates new orientation
curr_angle = add_to_angular_v2(np.degrees(p.getTheta()), angular_velocity)
print 'theta_rad: ' + str(p.getTheta())
print 'theta_deg: ' + str(np.degrees(p.getTheta()))
print 'cur_ang_deg: ' + str(np.degrees(curr_angle))
if velocity > 0.0:
[x, y] = move_vector(p, velocity)
particle.move_particle(p, x, y, curr_angle)
else:
particle.move_particle(p, 0.0, 0.0, curr_angle)
print 'cur_ang_rad: ' + str(curr_angle)
if velocity != 0.0:
particle.add_uncertainty(particles, 12, 15)
if velocity == 0.0 and angular_velocity != 0.0:
particle.add_uncertainty(particles, 0, 15)
# Fetch next frame
colour, distorted = cam.get_colour()
# Detect objects
objectType, measured_distance, measured_angle, colourProb = cam.get_object(
colour)
if objectType != 'none':
obs_landmark = ret_landmark(colourProb, objectType)
observed_obj = [objectType, measured_distance, measured_angle,
obs_landmark]
list_of_particles = weight_particles(particles,
np.degrees(measured_angle),
measured_distance, obs_landmark)
particles = []
for count in range(0, num_particles):
rando = np.random.uniform(0.0,1.0) # np.random.normal(0.0, 1.0, 1)
# dicto = {'i': 500,
# 'n': 2}
p = when_in_range(list_of_particles,
0,
num_particles,
rando)
particles.append(
particle.Particle(p.getX(), p.getY(), p.getTheta(),
1.0 / num_particles))
print 'list_of_particles: ' + str(list_of_particles)
print 'particles: ' + str(particles)
particle.add_uncertainty(particles, 5, 2)
# new random particles added
#for c in range(0, int(math.ceil(num_particles * 0.05))):
# p = particle.Particle(500.0 * np.random.ranf() - 100,
# 500.0 * np.random.ranf() - 100,
# 2.0 * np.pi * np.random.ranf() - np.pi, 0.0)
# particles.append(p)
# Draw detected pattern
cam.draw_object(colour)
else:
observed_obj = [None, None, None, None]
# No observation - reset weights to uniform distribution
for p in particles:
p.setWeight(1.0 / num_particles)
particle.add_uncertainty(particles, 5, 2)
# est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
# return [est_pose, observed_obj]
est_pose = particle.estimate_pose(
particles) # The estimate of the robots current pose
print 'Updated pose: ' + str([est_pose.getX(), est_pose.getY()])
draw_world(est_pose, particles, world)
# Show frame
cv2.imshow(WIN_RF1, colour)
# Show world
cv2.imshow(WIN_World, world)
return {'est_pos': est_pose,
'obs_obj': observed_obj,
'particles': particles}
def localize_self(particles, seen_landmarks, landmarks, cam, world, WIN_World, WIN_RF1, draw_fun, oneMeter = 2.0):
deg_turned = 0.0
converged = False
box_location = (0,0)
num_particles = len(particles)
oneMeter_ret = oneMeter
while deg_turned < 391 and not converged:
print("Getting next frame")
colour = cam.get_colour()
saw_box, box_location, seen_landmarks, measured_angle, measured_distance = camfun.get_frame(cam, landmarks, seen_landmarks, box_location, colour)
turn_angle = 30
if saw_box:
#for i in range(10):
particles = sample.sample_resample(particles, num_particles, box_location, measured_angle, measured_distance)
turn_angle = 45
'''
if len(seen_landmarks) <= 1:
if measured_angle > 0:
smov.move_robot(oneMeter, particles, 'l', np.degrees(measured_angle))
else:
smov.move_robot(oneMeter, particles, 'r', -np.degrees(measured_angle))
remaining, stop_dir = smov.move_robot(oneMeter, particles, 'f', 50)
prev_sens_dist = measured_distance
motor_dist = 50 - (remaining * 100)
_, _, _, _, sens_dist = camfun.get_frame(cam, landmarks, seen_landmarks, box_location, colour)
real_dist = (prev_sens_dist - sens_dist)
ratio = motor_dist / real_dist
oneMeter_ret = oneMeter * ratio
time.sleep(0.2)
print("prev_dist, new_dist:", measured_distance, sens_dist)
print("Prev oneMeter, new oneMeter:", oneMeter, oneMeter_ret)
turn_angle = 45
'''
else:
for p in particles:
p.setWeight(1.0/num_particles)
print("Localizing...")
stdX = [elem.getX() for elem in particles]
stdY = [elem.getY() for elem in particles]
converged = np.std(stdX) < 80 and np.std(stdY) < 80 and len(seen_landmarks) > 1
if not converged:
smov.move_robot(oneMeter, particles, 'l', turn_angle)
deg_turned += turn_angle
est_pose = particle.estimate_pose(particles)
draw_fun(est_pose, particles, world)
# Show frame
cv2.imshow(WIN_RF1, colour)
# Show world
cv2.imshow(WIN_World, world)
if converged:
print("Localization complete!")
return particles, est_pose, oneMeter_ret
else:
print("Couldn't localize, trying new position")
smov.move_robot(oneMeter, particles, 'f', 50)
return localize_self(oneMeter, particles, [], landmarks, cam, world, WIN_World, WIN_RF1, draw_world)