def test_production():
for sig_point in SIGNATURE_POINTS:
ls_normal = LocationSignature()
ls_normal.read(sig_point, NORMAL_DIR)
raw_input("Place a bottle somewhere and press enter")
ls_bottle = rot_sensor.takeSignature(sig_point.rstart, sig_point.rend,
sig_point)
ls_bottle.save(sig_point, BOTTLE_DIR)
bottle_loc = get_bottle_belief(ls_bottle, ls_normal, sig_point)
if bottle_loc is not None:
print bottle_loc
motor_params.rotate(bottle_loc.angle)
motor_params.interface.setMotorRotationSpeedReferences(
motor_params.motors, [8.0, 8.0])
left = 0
right = 0
motor_params.slow_down_forward(bottle_loc.distance,
bump_termination_callback)
else:
print "Bottle loc is None!"
raw_input(
"Place the robot in a the next signature point for a new test and press enter"
)
def uncertainRotate(state, dest):
"""
Arguments:
state - of type State
dest - absolute endpoint
"""
goal_theta = dest.theta - state.theta
delta_theta_rad = goal_theta
if delta_theta_rad > math.pi:
delta_theta_rad -= 2*math.pi
elif delta_theta_rad < -math.pi:
delta_theta_rad += 2*math.pi
motor_params.rotate(delta_theta_rad / math.pi * 180.0)
return state.rotate(delta_theta_rad)
def uncertainNavigate(state, dest):
"""
Arguments:
state - of type State
dest - absolute orientation
"""
goal_theta = math.atan2(dest.y - state.y, dest.x - state.x)
dist = math.sqrt((dest.y - state.y) ** 2 + (dest.x - state.x) ** 2)
delta_theta_rad = goal_theta - state.theta
if delta_theta_rad > math.pi:
delta_theta_rad -= 2*math.pi
elif delta_theta_rad < -math.pi:
delta_theta_rad += 2*math.pi
motor_params.rotate(delta_theta_rad / math.pi * 180.0)
state = state.rotate(delta_theta_rad)
motor_params.forward(dist)
return state.move_forward(dist)
def navigateToWaypoint(state, dest):
"""
NOTE: USE THIS ONLY FOR MCL
Arguments:
state - of type State
dest - of type walls.Point
"""
goal_theta = math.atan2(dest.y - state.y, dest.x - state.x)
delta_theta_rad = goal_theta - state.theta
if delta_theta_rad > math.pi:
delta_theta_rad -= 2 * math.pi
elif delta_theta_rad < -math.pi:
delta_theta_rad += 2 * math.pi
motor_params.rotate(delta_theta_rad / math.pi * 180.0)
state = state.rotate(delta_theta_rad)
if (delta_theta_rad / math.pi *
180) > 45 or delta_theta_rad / math.pi * 180 < -45:
return state
dist = min(20.0, math.sqrt((state.y - dest.y)**2 + (state.x - dest.x)**2))
motor_params.forward(dist)
return state.move_forward(dist)
def test_performance():
for sig_point in SIGNATURE_POINTS:
ls_normal = rot_sensor.takeSignature(sig_point.rstart, sig_point.rend,
sig_point)
ls_normal.save(sig_point, NORMAL_DIR)
raw_input("Place a bottle somewhere and press enter")
ls_bottle = rot_sensor.takeSignature(sig_point.rstart, sig_point.rend,
sig_point)
ls_bottle.save(sig_point, BOTTLE_DIR)
bottle_loc = get_bottle_belief(ls_normal, ls_bottle, sig_point)
print bottle_loc
motor_params.rotate(bottle_loc.angle)
motor_params.forward(bottle_loc.distance)
raw_input(
"Place the robot in a the next signature point for a new test and press enter"
)
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)
import motor_params as mp angle = (360.0 / 332.5) * 180.0 distance = (40.0 / 38.84) * 40 mp.forward(distance) mp.rotate(angle) mp.forward(distance) mp.rotate(angle)
import motor_params
input = float(raw_input("Enter angle: "))
if input < 0:
angle = (345.0 / 360.0) * input
else:
angle = (345.0 / 360.0) * input
motor_params.rotate(angle)
motor_params.rotate(angle)
motor_params.rotate(angle)
motor_params.rotate(angle)
