def _attempt_extraction(self, gem_type: str, estimate_x: float,
estimate_y: float):
"""Attempt to extract a gem from the current x,y location.
Extract gem if current location is within EXTRACTION_DISTANCE of specified gem_type.
Otherwise, pause for WAIT_PENALTY
"""
for gem_location in self.gem_locs_on_map:
if gem_location['type'] == gem_type:
robot_distance = robot.compute_distance(
(self.robot.x, self.robot.y),
(gem_location['x'], gem_location['y']))
translated_x = estimate_x + self._start_position['x']
translated_y = estimate_y + self._start_position['y']
estimate_distance = robot.compute_distance(
(translated_x, translated_y),
(gem_location['x'], gem_location['y']))
if robot_distance <= self.EXTRACTION_DISTANCE and estimate_distance <= self.EXTRACTION_DISTANCE:
self.collected_gems.append(gem_location)
self.gem_locs_on_map.remove(gem_location)
self.gem_checklist.remove(gem_location['type'])
return
time.sleep(self.WAIT_PENALTY)
if VERBOSE_FLAG:
print(
f"*** Location ({self.robot.x}, {self.robot.y}) does not contain a gem type <{gem_type}> within "
f"the extraction distance.")
def _attempt_down(self, x, y):
# - The robot may put a box down within a distance of 0.5 of the robot's center.
# The cost to set a box down is 1.5 (regardless of the direction in which the robot puts down the box).
# - If a box is placed on the '@' space, it is considered delivered and is removed from the ware-
# house.
# Illegal moves (do not set box down but incur cost):
# - putting down a box too far away or so that it's touching a wall, the warehouse exterior,
# another box, or the robot
# - putting down a box while not holding a box
try:
self._increase_total_cost_by(self.BOX_DOWN_COST)
destination = (x, y)
destination_is_adjacent = robot.compute_distance(
(self.robot.x, self.robot.y), destination) <= 0.5
destination_is_open = self._is_open_for_box(destination)
destination_is_within_warehouse = self._is_box_within_warehouse(
destination)
robot_has_box = self._robot_has_box()
is_legal_down = (destination_is_adjacent and destination_is_open
and destination_is_within_warehouse
and robot_has_box)
if is_legal_down:
self._down_box(destination)
except ValueError:
raise Exception(
'improperly formatted down destination: {} {}'.format(
row, col))
def _attempt_lift(self, box_id):
# - The robot may pick up a box that is within a distance of 0.5 of the robot's center.
# - The cost to pick up a box is 2, regardless of the direction the box is relative to the robot.
# - While holding a box, the robot may not pick up another box.
# Illegal lifts (do not lift a box but incur cost of lift):
# - picking up a box that doesn't exist or is too far away
# - picking up a box while already holding one
try:
self._increase_total_cost_by(self.BOX_LIFT_COST)
box_position = (self.boxes[box_id]['ctr_x'],
self.boxes[box_id]['ctr_y'])
box_is_adjacent = robot.compute_distance(
(self.robot.x, self.robot.y), box_position) <= 0.5
robot_has_box = self._robot_has_box()
is_legal_lift = box_is_adjacent and (not robot_has_box)
if is_legal_lift:
self._lift_box(box_id)
else:
print "*** Could not lift box: box_is_adjacent = {}, robot_has_box = {} ".format(
box_is_adjacent, robot_has_box)
except KeyError:
raise Exception('improper key {}'.format(box_id))
def _is_in_lineofsight(self, marker):
# end points of line-of-sight
t1 = marker
t0 = (self.robot.x, self.robot.y)
# distance_to_marker
distance_to_marker = robot.compute_distance(t0, t1)
# Does a box block the line-of-sight
for box_id, box in self.boxes.iteritems():
for s in box['segments']:
dst, intercept_point = self._linesegment_intersection(
t0, t1, s[0], s[1])
if dst < distance_to_marker - 0.001: #Avoid self occlusions
return False
# Does a wall (obstacle) block the line-of-sight
for o in self.obstacles:
for s in o['segments']:
dst, intercept_point = self._linesegment_intersection(
t0, t1, s[0], s[1])
if dst < distance_to_marker - 0.001: #Avoid self occlusions
return False
return True
def run_with_params(self, params):
"""Run test case using desired parameters.
Args:
params(dict): a dictionary of test parameters.
"""
state = State(params['area_map'])
dist_error = params['test_tolerance'] * 2.0
try:
rover_slam = ice_rover.SLAM()
for move in params['move']:
meas = state.generate_measurements()
belief = rover_slam.process_measurements(meas)
truth = (state.robot.x - state._start_position['x'],
state.robot.y - state._start_position['y'])
action = move.split()
state.update_according_to(move)
rover_slam.process_movement(float(action[1]), float(action[2]),
NOISE_MOVE)
dist_error = robot.compute_distance(belief, truth)
if VERBOSE_FLAG:
print "Current Belief:", belief
print "True Position:", truth
print "Error:", dist_error, "\n"
except Exception as exc:
self.last_result = self.FAIL_TEMPLATE.format(
message=traceback.format_exc(),
expected="exception",
location="exception",
**params)
self.last_credit = 0.0
self.fail(str(exc))
if dist_error < params['test_tolerance']:
result = self.SCORE_TEMPLATE.format(expected=truth,
location=belief,
score=1.0,
**params)
score = 1.0
else:
result = self.FAIL_TEMPLATE.format(
message='Distance greater than tolerance {}'.format(
params['test_tolerance']),
expected=truth,
location=belief,
**params)
score = 0.0
self.last_result = result
self.last_credit = score
self.assertTrue(
dist_error < params['test_tolerance'],
'Location error {} as a distance must be less than {}'.format(
dist_error, params['test_tolerance']))
def _is_traversable(self, destination, distance, steering):
NUDGE_DISTANCE = 0.01
# end points of trajectory
t1 = destination
if not self.robot_is_crashed:
t0 = (self.robot.x, self.robot.y)
# the distance to check against
chk_distance = distance
else:
# in the event the robot is crashed don't use the current robot point to evaluate
# collisions because by definition it is colliding. Define the trajectory line
# segment start point just a bit farther out to see if a new collision will occur.
t0 = self.robot.find_next_point(steering, NUDGE_DISTANCE)
chk_distance = distance - NUDGE_DISTANCE
# Is the path too close to any box
min_distance_to_intercept = chk_distance
self.robot_is_crashed = False
for box_id, box in self.boxes.iteritems():
# do a coarse check to see if the center of the obstacle
# is too far away to be concerned with
dst = self._distance_point_to_line_segment( (box['ctr_x'], box['ctr_y']), t0, t1 )
if (dst <= self.BOX_DIAG + self.ROBOT_RADIUS):
# refine the intercept computation
dst, intercept_point = self._check_intersection(t0,t1,box)
if dst < min_distance_to_intercept:
min_distance_to_intercept = dst
min_intercept_point = intercept_point
# Is the path too close to any obstacle
for o in self.obstacles :
# do a coarse check to see if the center of the obstacle
# is too far away to be concerned with
dst = self._distance_point_to_line_segment( (o['ctr_x'],o['ctr_y']), t0, t1 )
if (dst <= self.OBSTACLE_DIAG + self.ROBOT_RADIUS):
# refine the intercept computation
dst, intercept_point = self._check_intersection(t0,t1,o)
if dst < min_distance_to_intercept:
min_distance_to_intercept = dst
min_intercept_point = intercept_point
# Check the edges of the warehouse
dst, intercept_point = self._check_intersection(t0,t1,self.warehouse_limits)
if dst < min_distance_to_intercept:
min_distance_to_intercept = dst
min_intercept_point = intercept_point
if min_distance_to_intercept < chk_distance :
self.robot_is_crashed = True
print "*** Robot crashed at {p[0]:6.2f} {p[1]:6.2f} ***".format( p = min_intercept_point )
return False, robot.compute_distance( (self.robot.x, self.robot.y), min_intercept_point )
return True, distance
def plan_move_point(self, target, distance, dropoff = False):
moveList = []
if self.verbose:
print "Planning move to", target
# Decide whether to move back to center first
dist_center = self.distance_box_point(self.CurrentBox, [self.robot.x, self.robot.y])
distance_target = robot.compute_distance([self.robot.x, self.robot.y], target)
if distance_target < 0.8:
# if we are too close to the target, no need to plan anything else
pass
else:
if dist_center > 0.3:
# Move back to the center
if self.verbose:
print "current distance to the center is ", dist_center
moveList.append({'type': OnlineDeliveryPlanner.MOVE, 'target': self.CurrentBox})
ValueGrid, actions = self.CreateValue(target, distance)
# follow the value list
i, j = self.CurrentBox
count = 0
while ValueGrid[i][j] > 0 and count < 30:
count += 1
move = actions[i][j]
if move > len(OnlineDeliveryPlanner.DELTA):
if self.verbose:
print "move value not yet initiated at cell",i, j
return
else:
i = i + OnlineDeliveryPlanner.DELTA[move][0]
j = j + OnlineDeliveryPlanner.DELTA[move][1]
moveList.append({'type': OnlineDeliveryPlanner.MOVE, 'target': [i, j]})
# Add the last action
if not self.exploring:
# if only exploring, we don't need a last action
if dropoff:
last_action = OnlineDeliveryPlanner.DOWN
else:
last_action = OnlineDeliveryPlanner.LIFT
moveList.append({'type': last_action, 'target': target})
if self.verbose:
self.PrintMoveList(moveList)
moveList.reverse()
self.MoveList = moveList
return
def run_with_params(self, params):
"""Run test case using desired parameters.
Args:
params(dict): a dictionary of test parameters.
"""
state = State(params['area_map'])
rover_slam = ice_rover.SLAM()
for move in params['move']:
meas = state.generate_measurements()
belief = rover_slam.process_measurements(meas)
truth = (state.robot.x - state._start_position['x'],
state.robot.y - state._start_position['y'])
action = move.split()
state.update_according_to(move)
rover_slam.process_movement(float(action[1]), float(action[2]))
dist_error = robot.compute_distance(belief, truth)
if VERBOSE_FLAG:
print "Current Belief:", belief
print "True Position:", truth
print "Error:", dist_error, "\n"
if dist_error < params['test_tolerance']:
self.results.append(
self.SCORE_TEMPLATE.format(expected=truth,
location=belief,
score=1.0,
**params))
score = 1.0
self.credit.append(score)
else:
self.results.append(
self.FAIL_TEMPLATE.format(
message='Distance greater than tolerance {}'.format(
params['test_tolerance']),
**params))
score = 0.0
self.credit.append(score)
self.assertTrue(
dist_error < params['test_tolerance'],
'Location error {} as a distance must be less than {}'.format(
dist_error, params['test_tolerance']))
print('test case {}: {:.0%}'.format(params['test_case'], score))
def next_box(self):
if self.firstBox:
self.firstBox = False
if len(self.BoxPositions) > 1:
min_dist = 100
select = -1
for i in range(len(self.BoxPositions)):
tmp_dist = robot.compute_distance([self.robot.x, self.robot.y], self.BoxPositions[i])
if tmp_dist <= min_dist:
select = i
min_dist = tmp_dist
if select >= 0:
return self.BoxPositions.pop(select)
else:
return self.BoxPositions.pop()
else:
if len(self.BoxPositions) > 0:
return self.BoxPositions.pop(0)
def _attempt_down(self, x, y):
"""Attempt down action if valid.
The robot may put a box down within a distance of 0.5 of the robot's center.
The cost to set a box down is 1.5 (regardless of the direction in which the robot puts down the box).
If a box is placed on the '@' space, it is considered delivered and is removed from the ware-house.
Illegal moves (do not set box down but incur cost):
putting down a box too far away or so that it's touching a wall, the warehouse exterior, another box,
or the robot
putting down a box while not holding a box
Args:
x: x location to set down box.
y: y location to set down box.
Raises:
ValueError: if improperly formatted down destination.
"""
try:
self._increase_total_cost_by(self.BOX_DOWN_COST)
destination = (x, y)
destination_is_adjacent = robot.compute_distance(
(self.robot.x, self.robot.y), destination) <= 0.5
destination_is_open = self._is_open(destination, self.BOX_SIZE)
destination_is_within_warehouse = self._is_within_warehouse(
destination, self.BOX_SIZE)
robot_has_box = self._robot_has_box()
is_legal_down = (destination_is_adjacent and destination_is_open
and destination_is_within_warehouse
and robot_has_box)
if is_legal_down:
self._down_box(destination)
except ValueError:
# raise Exception('improperly formatted down destination: {} {}'.format(x, y))
print 'improperly formatted down destination: {} {}'.format(x, y)
def _is_traversable(self, destination, distance, steering):
"""Verify path to destination is traversable.
Check the path to make sure the robot can traverse it without running into
boxes, obstacles, or the warehouse walls
Args:
destination(tuple): location to travel to.
distance(float): Distance to travel.
steering(float): Angle to turn before moving.
Return:
True if traversable.
"""
nudge_distance = 0.01
# end points of trajectory
t1 = destination
if not self.robot_is_crashed:
t0 = (self.robot.x, self.robot.y)
# the distance to check against
chk_distance = distance
else:
# in the event the robot is crashed don't use the current robot point to evaluate
# collisions because by definition it is colliding. Define the trajectory line
# segment start point just a bit farther out to see if a new collision will occur.
t0 = self.robot.find_next_point(steering, nudge_distance)
chk_distance = distance - nudge_distance
# check that the robot will start inside the warehouse and outside any obstacles
# a sanity check on initial location and deals with potential edge cases with NUDGE
if (not self._is_within_warehouse(t0, self.ROBOT_RADIUS)
or not self._is_open(t0, self.ROBOT_RADIUS)):
self._log(
"*** Robot stuck at {p[0]:6.2f} {p[1]:6.2f}: Try a different heading ***"
.format(p=(self.robot.x, self.robot.y)))
return False, 0.0
# Is the path too close to any box
min_distance_to_intercept = chk_distance
min_intercept_point = (None, None)
self.robot_is_crashed = False
for box_id, box in self.boxes.iteritems():
# do a coarse check to see if the center of the obstacle
# is too far away to be concerned with
dst = self._distance_point_to_line_segment(
(box['ctr_x'], box['ctr_y']), t0, t1)
if dst <= self.BOX_DIAG + self.ROBOT_RADIUS:
# refine the intercept computation
dst, intercept_point = self._check_intersection(t0, t1, box)
if dst < min_distance_to_intercept:
min_distance_to_intercept = dst
min_intercept_point = intercept_point
# Is the path too close to any obstacle
for o in self.obstacles:
# do a coarse check to see if the center of the obstacle
# is too far away to be concerned with
dst = self._distance_point_to_line_segment(
(o['ctr_x'], o['ctr_y']), t0, t1)
if dst <= self.OBSTACLE_DIAG + self.ROBOT_RADIUS:
# refine the intercept computation
dst, intercept_point = self._check_intersection(t0, t1, o)
if dst < min_distance_to_intercept:
min_distance_to_intercept = dst
min_intercept_point = intercept_point
# Check the edges of the warehouse
dst, intercept_point = self._check_intersection(
t0, t1, self.warehouse_limits)
if dst < min_distance_to_intercept:
min_distance_to_intercept = dst
min_intercept_point = intercept_point
if min_distance_to_intercept < chk_distance:
self.robot_is_crashed = True
self._log(
"*** Robot crashed at {p[0]:6.2f} {p[1]:6.2f} ***".format(
p=min_intercept_point))
return False, robot.compute_distance((self.robot.x, self.robot.y),
min_intercept_point)
return True, distance
def return_next_move(self):
# return the next move according to move list
# move list may be of simple format
# need some extra re-formatting
if len(self.MoveList) > 0:
res = self.MoveList.pop()
else:
if self.verbose:
print "Move List is empty"
return None
# Copy the last move
self.LastMove = res
# res should be a dictionary
# res has type, target
# target is the index of the box
distance = 0.0
turningAngle = 0.0
if res['type'] == OnlineDeliveryPlanner.MOVE:
nextPosition = self.Helper.Get_Coord_box(res['target'])
distance, turningAngle = self.robot.measure_distance_and_bearing_to(nextPosition)
tmp_res = self.move_to_point(nextPosition, res)
elif res['type'] == OnlineDeliveryPlanner.DOWN:
distance, turningAngle = self.robot.measure_distance_and_bearing_to(res['target'])
nextDist = 0.0
shiftMove = False
if distance > OnlineDeliveryPlanner.DISTANCE_LIMIT_DROP:
nextDist = OnlineDeliveryPlanner.DISTANCE_MOVE_DROP
nextPosition = self.Helper.ShiftTarget([self.robot.x, self.robot.y], res['target'], nextDist)
nextDist1 = robot.compute_distance(nextPosition, res['target'])
if self.verbose:
print "Need to move from current position to", nextPosition, " new distance = ", nextDist1
print "Current distance", distance
tmp_res = self.move_to_point(nextPosition, res)
else:
tmp_res = [res['type'], turningAngle]
elif res['type'] == OnlineDeliveryPlanner.LIFT:
distance, turningAngle = self.robot.measure_distance_and_bearing_to(res['target'])
nextDist = 0.0
shiftMove = False
# TO DO:
if self.FoundBox == False:
if self.verbose:
print "Box not found", res['target']
self.TargetBox = None
self.exploring = True
self.MoveList = []
return None
if self.verbose:
if x != None:
print "Spotted box - dist-bearing", x.ActualCoord, x.dist, x.bearing
print "Max steering", self.robot.max_steering
if self.Relift == True:
# Codes have been removed
else:
# Refine the measurement, just in case we are too far off
if x == None and self.FoundBox == True:
# Some times we found the box, but when we move closer, the box disappears
tmp_res = [OnlineDeliveryPlanner.LIFT, turningAngle]
else:
distance = x.dist
sign = 1
if x.bearing < 0:
sign = -1
if distance > OnlineDeliveryPlanner.DISTANCE_LIMIT_LIFT:
nextDist = distance - OnlineDeliveryPlanner.DISTANCE_LIMIT_LIFT
self.MoveList.append(res)
if abs(x.bearing) > self.robot.max_steering:
tmp_res = [OnlineDeliveryPlanner.MOVE, sign * min(abs(x.bearing), self.robot.max_steering), 0.0]
else:
tmp_res = [OnlineDeliveryPlanner.MOVE, sign * min(abs(x.bearing), self.robot.max_steering), nextDist]
else:
tmp_res = [OnlineDeliveryPlanner.LIFT, x.bearing]
return tmp_res
def run_with_params(self, params: Dict):
"""Run test case using desired parameters.
Args:
params: a dictionary of test parameters.
"""
state = State(params['area_map'],
measure_distance_noise=params['robot_distance_noise'],
measure_bearing_noise=params['robot_bearing_noise'])
robot_dist_error = float('inf')
landmark_dist_errors = dict()
state_beliefs = list()
ground_truth = list()
try:
gem_finder_slam = gem_finder.SLAM()
# calculate robot position error
for move in params['move']:
meas = state.generate_measurements()
gem_finder_slam.process_measurements(meas)
action = move.split()
state.update_according_to(move)
belief = gem_finder_slam.process_movement(
float(action[1]), float(action[2]))
truth = (state.robot.x - state._start_position['x'],
state.robot.y - state._start_position['y'])
robot_dist_error = robot.compute_distance(belief, truth)
if VERBOSE_FLAG:
print("Current Belief:", belief)
print("True Position:", truth)
print("Error:", robot_dist_error, "\n")
state_beliefs.append(belief)
ground_truth.append(truth)
# calculate landmark errors
for landmark in state.orig_gem_checklist:
student_landmark_x, student_landmark_y = gem_finder_slam.get_coordinates_by_landmark_id(
landmark['id'])
translated_x = student_landmark_x + state._start_position['x']
translated_y = student_landmark_y + state._start_position['y']
landmark_dist_errors[landmark['id']] = robot.compute_distance(
(translated_x, translated_y),
(landmark['x'], landmark['y']))
except Exception as exc:
self.last_result = self.FAIL_TEMPLATE.format(
message=traceback.format_exc(),
expected="exception",
location="exception",
**params)
self.last_credit = 0.0
self.fail(str(exc))
max_robot_score = 0.5
max_landmark_score = 0.5
robot_score = 0.0
landmark_score = 0.0
# calculate score for robot distance error
if robot_dist_error < params['robot_tolerance']:
robot_score += max_robot_score
# calculate score for landmark distance errors
missed_landmarks = list()
for landmark_type, landmark_error in landmark_dist_errors.items():
if landmark_error < params['landmark_tolerance']:
landmark_score += max_landmark_score / len(
state.orig_gem_checklist)
else:
missed_landmarks.append({
'landmark': landmark_type,
'error': landmark_error
})
robot_score = round(robot_score, 5)
landmark_score = round(landmark_score, 5)
total_score = robot_score + landmark_score
if total_score >= 1.0:
result = self.SCORE_TEMPLATE.format(expected=ground_truth,
location=state_beliefs,
score=total_score,
**params)
else:
if robot_score < max_robot_score:
robot_message = f"Robot location error {robot_dist_error} is greater than {params['robot_tolerance']}. "
else:
robot_message = ''
if landmark_score < max_landmark_score:
landmark_message = f"A landmark locations are greater than {params['landmark_tolerance']}"
else:
landmark_message = ''
result = self.FAIL_TEMPLATE.format(message=robot_message +
landmark_message,
expected=ground_truth,
location=state_beliefs,
**params)
self.last_result = result
self.last_credit = total_score
self.assertTrue(
robot_score >= max_robot_score,
f"Robot location error {robot_dist_error} is greater than {params['robot_tolerance']}"
)
self.assertTrue(
landmark_score >= max_landmark_score,
f"A landmark location error is greater than {params['landmark_tolerance']}\n{missed_landmarks}"
)
