def calculate_likelihood(x, y, theta, z):
"""
param: z: sonar measurement
"""
particle = motion_predict.Particle(x=x, y=y, theta=theta)
m = walls.getWallDist(
particle,
walls.wallmap) # calculate estimated measurment for this particle
# if incidence angle or distance is out of range then skip the update
if m == ultrasound.GARBAGE:
return -1
return np.exp(-np.power(z - m, 2.) /
(2 * np.power(SONAR_STD, 2.))) + SONAR_CONSTANT_LIKELIHOOD
def takeSignature(self, start_angle, end_angle, sig_point):
"""
Take a signature and return LocationSignature()
@param start_angle: orientation angle relative to the robot orienation to start taking sonar measurements from - in degrees
@param end_angle: orientation angle relative to the robot orienation to end taking sonar measurements - in degrees
@param sig_point: point at which the reading is being taken
"""
ls = LocationSignature()
if start_angle > end_angle:
step = -STEP
else:
step = STEP
for angle in range(int(start_angle), int(end_angle), step):
self.setOrientation(float(angle) * math.pi / 180)
reading = ultrasound.get_reading()
if reading > ultrasound.MAX_DIST:
reading = ultrasound.GARBAGE
# Get the absolute orientation at which the measurement is being taken
theta = sig_point.theta + math.radians(angle - 90)
if theta > math.pi:
theta -= 2 * math.pi
elif theta < -math.pi:
theta += 2 * math.pi
# Get the reading that is supposed to be read at that position
particle = motion_predict.Particle(x=sig_point.x,
y=sig_point.y,
theta=theta)
expected_dist = walls.getWallDist(particle, incidence_angle=False)
# if the expected reading is not garbage and the actual reading is garbage, substitute the actual reading with the simulated one
# when another signature is taken at the same point, the reading will either fail and be substituted again or it will succeed
# if successful and no bottle, error will be too small to affect the algorithm; if bottle then the proper difference in signatures will be noted
if expected_dist != ultrasound.GARBAGE and reading == ultrasound.GARBAGE:
reading = expected_dist
# note - if reading is not garbage but it is supposed to be, we should save the reading as it is: we either have a bottle or made a successful reading anyway
ls.sig.append(reading)
return ls
def main():
ultrasound.setup()
walls.wallmap.draw()
WAYPOINTS = [
walls.Point(84, 30),
walls.Point(180, 30),
walls.Point(180, 54),
walls.Point(138, 54),
walls.Point(138, 168),
walls.Point(114, 168),
walls.Point(114, 84),
walls.Point(84, 84),
walls.Point(84, 30)
]
state = motion_predict.State(particles=[
motion_predict.Particle(x=WAYPOINTS[0].x, y=WAYPOINTS[0].y, theta=0)
] * NUMBER_OF_PARTICLES,
weights=[
1.0 / NUMBER_OF_PARTICLES
for _ in range(NUMBER_OF_PARTICLES)
])
draw_particles(state)
for waypoint in WAYPOINTS[1:]:
# waypoint refers to the next destination
while True:
x_is_close = abs(state.x - waypoint.x) <= 1.0
y_is_close = abs(state.y - waypoint.y) <= 1.0
if x_is_close and y_is_close:
# We have reached our destination, rotate!
break
else:
state = motion_predict.navigateToWaypoint(state, waypoint)
state = MCLStep(state)
print state.x, state.y, state.theta
import sys
import os
import getpass
sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/pmotion')
sys.path.append(
os.path.dirname(os.path.realpath(__file__)) + '/ultrasonic_sensors')
sys.path.append(os.path.dirname(os.path.realpath(__file__)) + '/MCL')
import math
import walls
import motion_predict
p = motion_predict.Particle(x=124, y=120, theta=math.radians(30))
print walls.getWallDist(p, incidence_angle=False)
def main():
walls.wallmap.draw()
state = motion_predict.State(
particles=[motion_predict.Particle(
x=BOTTLES[3][1][0].x, y=BOTTLES[3][1][0].y, theta=0)] * NUMBER_OF_PARTICLES,
weights=[1.0 / NUMBER_OF_PARTICLES
for _ in range(NUMBER_OF_PARTICLES)])
mcl.draw_particles(state)
raw_input("Click enter to begin")
# visitpoints is a list of points that we need to visit
for key, visitpoints in BOTTLES:
# print "Going into key = ", key
# print "Entering state is ", state
# mcl_points is a list of lists
area_mcl_points = MCL_POINTS[key]
# Bottles
if key != "FINAL":
# waypoint refers to the next destination
for waypoint, mcl_points in zip(visitpoints, area_mcl_points):
distance = 0.0
# Navigate properly
for i in range(1,3):
#while True:
x_is_close = abs(state.x - waypoint.x) <= WAYPOINT_MIN_OFFSET
y_is_close = abs(state.y - waypoint.y) <= WAYPOINT_MIN_OFFSET
if x_is_close and y_is_close:
# We have reached our destination
break
else:
# TO DO: Smart navigation with not many rotations
# print "state = ", state
state = uncertainNavigate(state, waypoint)
# print "Navigating to ", waypoint
# Run MCL
if key!= "A":
if i != 2:
state = sigPointMCLStep(state, mcl_points)
# print "CURRENT STATE: x=%f, y =%f, theta=%f" % (state.x, state.y, state.theta)
# Make sure your orientation is the same as the orientation a signature must be taken at
state = uncertainRotate(state, waypoint)
# Compare signatures
bottle_loc = get_bottle(waypoint)
# We did not find a bottle, go to the next waypoint
# TO DO: What to do if we went to all waypoints and we still did not hit a bottle
if bottle_loc is None:
# print "BOTTLE NOT DETECTED"
continue
# We have a possible bottle location
else:
# print "bottle_loc = ", bottle_loc
# 1. Try navigating to the bottle.
motor_params.rotate(bottle_loc.angle)
state = state.rotate(math.radians(bottle_loc.angle))
# print "State right before moving towards bottle:"
# print state
nearest_wall_dist = walls.getWallDist(
motion_predict.Particle(x=state.x, y=state.y, theta=state.theta),
incidence_angle=False)
if nearest_wall_dist == ultrasound.GARBAGE:
overshoot = 5.0
else:
# bottle.distance + overshoot == nearest_wall_dist - 10.0
overshoot = nearest_wall_dist - 10.0 - bottle_loc.distance
distance, hit_bottle = motor_params.slow_down_forward(
bottle_loc.distance,
place_rec.bump_termination_callback,
overshoot=overshoot)
# Don't perform MCL here, we are fairly sure that it will
# screw up. (Due to the bottle).
state = state.move_forward(distance)
if hit_bottle:
# print "State right after touching bottle:"
# print state
# 2. If we hit the bottle, we will want to reverse.
motor_params.forward(-distance * 0.8)
state = state.move_forward(-distance * 0.8)
# print "State right after reversing from bottle:"
# print state
# 3. Break if we hit the bottle. Then we go on to
# handle the next bottle area.
state = sigPointMCLStep(state, mcl_points)
# print "State right after performing MCL"
# print state
break
else:
# 4. if we did not hit a bottle, we continue the loop.
# Going to the next waypoint in the area.
motor_params.forward(-distance * 0.8)
state = state.move_forward(-distance * 0.8)
continue
# Final endpoint
else:
waypoint = visitpoints[0]
mcl_points = area_mcl_points[0]
# Navigate properly
for i in range(1,3):
#while True:
x_is_close = abs(state.x - waypoint.x) <= FINAL_WAYPOINT_MIN_OFFSET
y_is_close = abs(state.y - waypoint.y) <= FINAL_WAYPOINT_MIN_OFFSET
if x_is_close and y_is_close:
# We have reached our destination
break
else:
# TO DO: Make sure you are tunning MCL facing to the proper walls
state = uncertainNavigate(state, waypoint)
state = sigPointMCLStep(state, mcl_points)
print "CURRENT STATE: x=%f, y =%f, theta=%f" % (state.x, state.y, state.theta)