def invalid_region(self, position):
"""
Invalidates the region surrounding a single block.
This should be used after the robot changes the environment for an reason.
"""
self.invalid_cache()
invalidation_size = INVALIDATION_REGION / 2
for cluster in self.predict_block_locations():
if util.get_distance(cluster.coord, position) < cluster.radius:
invalidation_size = max(invalidation_size, cluster.radius)
if(cluster.known_index is not None):
print("[info] Block in invalidation cluster, removing")
self._confirmed_blocks.pop(cluster.known_index)
# Also delete overlapping blocks if they are not in a cluster
for index in range(len(self._confirmed_blocks) - 1, 0, -1):
block_pos, _ = self._confirmed_blocks[index]
if util.get_distance(block_pos, position) < invalidation_size:
# Purge this block
print("[info] Block in invalidation region, removing")
self._confirmed_blocks.pop(index)
invalid_min = _to_screenspace(np.array(position) - invalidation_size)
invalid_max = _to_screenspace(np.array(position) + invalidation_size)
invalid_size = tuple(invalid_max - invalid_min)
self._probability_sum[invalid_min[0]:invalid_max[0], invalid_min[1]:invalid_max[1]] = np.zeros(invalid_size)
self._probability_count[invalid_min[0]:invalid_max[0], invalid_min[1]:invalid_max[1]] = np.ones(invalid_size)
def invalid_region(self, block_locations, position, invalidation_size):
"""
Invalidates the region surrounding a single block.
This should be used after the robot changes the environment for an reason.
"""
# Remove each block in a known position
for cluster in block_locations:
invalidation_radius = max(invalidation_size, cluster.radius)
if util.get_distance(cluster.coord, position) < invalidation_radius:
if(cluster.known_block is not None):
if cluster.known_block not in self.confirmed_blocks:
util.log_msg(util.Logging.WARNING, "unexpected block not in confirmed_blocks")
continue
util.log_msg(util.Logging.WARNING, "Block in invalidation cluster, removing")
self.confirmed_blocks.remove(cluster.known_block)
# Also delete overlapping blocks if they are not in a cluster
for index in range(len(self.confirmed_blocks) - 1, 0, -1):
block_pos, _ = self.confirmed_blocks[index]
if util.get_distance(block_pos, position) < invalidation_size:
# Purge this block
util.log_msg(util.Logging.INFO, "Block in invalidation region, removing")
self.confirmed_blocks.pop(index)
def predict_block_locations(self, cluster_candidates, occupancy_map):
# Smooth clusters into single block
clusters = get_cluster_average(cluster_candidates, occupancy_map)
# Some block colors are known, mark these in the clusters object
for block in self.confirmed_blocks:
block_location, block_color = block
# Check if block is in radius of any clusters
color_consumed = False
# NOTE: this is a back hackfix
ClosestCluster = namedtuple("ClosestBlock", ["distance", "cluster"])
closest_cluster = ClosestCluster(np.inf, None)
for cluster in clusters:
cluster_distance = util.get_distance(cluster.coord, block_location)
if cluster_distance < mapping.CLUSTER_PROXIMITY_THRESHOLD:
if (cluster.color is not None):
util.log_msg(util.Logging.WARNING, "Multiple blocks identified in the same cluster")
color_consumed = True
cluster.assign_known_color(block)
if cluster_distance < closest_cluster.distance:
closest_cluster = ClosestCluster(
cluster_distance,
cluster
)
if not color_consumed:
# Hackfix, no cluster identified properly, assign to closest one
if closest_cluster.distance < mapping.CLUSTER_FUDGE_DISTANCE:
closest_cluster.cluster.assign_known_color(block)
continue
new_cluster = ClusterLocation(
block_location, 20, 10, mapping.BLOCK_OBSTACLE_WIDTH
)
new_cluster.assign_known_color(block)
clusters.append(new_cluster)
# Ensure no clusters are inside the spawn region
filtered_clusters = []
for cluster in clusters:
# Each robot spawn position is defined here
for robot_name in util.ROBOT_SPAWN:
spawn_pos = util.ROBOT_SPAWN[robot_name]
if util.get_distance(spawn_pos, cluster.coord) < mapping.SPAWN_REGION_RADIUS:
break # Block is inside spawn region, don't recollect it
else:
# Block was not inside any spawn radius, lets save it
filtered_clusters.append(cluster)
return filtered_clusters
def vote_for_route(self, new_waypoints):
# Check if routes are identical. This must be right, stick with it
if new_waypoints == self.waypoints:
self.votes = min(100, self.votes + 5)
return
routes_different = False
if len(new_waypoints) < len(self.waypoints):
# Check if the route just got shorter (or its completely different)
for new_p, old_p in zip(new_waypoints[::-1], self.waypoints[::-1]):
if GOAL_TOLERANCE > util.get_distance(new_p, old_p):
routes_different = True
break
if not routes_different:
# Route is just the same (but shorter)
self.waypoints = new_waypoints
return
# Routes are certainly different, vote against current route
if len(new_waypoints) < len(self.waypoints):
self.votes -= 5 # Shorter routes are preferable
else:
self.votes -= 2
# If the current route is unpopular, change it
if self.votes < 0:
self.waypoints = new_waypoints
self.votes = 50
def navigate_waypoints(self,
positioning_system,
tolerance=0.03,
reverse=False):
"""
For this tick, attempt to navigate towards the next waypoint.
Returns True if the final waypoint has been reached.
"""
if self.__waypoint_index >= len(self.__waypoints):
self.__waypoints = []
self.__waypoint_index = 0
return True
target_waypoint = self.__waypoints[self.__waypoint_index]
current_position = positioning_system.get_2D_position()
self.navigate_toward_point(positioning_system, target_waypoint,
reverse)
if util.get_distance(current_position, target_waypoint) < tolerance:
self.__waypoint_index += 1
if self.__waypoint_index == len(self.__waypoints):
self.__waypoints = []
self.__waypoint_index = 0
return True
return False
def predict_block_locations(self,
robot_states,
generate_fake_blocks=False):
"""
Returns a list of ClusterLocation objects, containing the positions of blocks
and the relative certainties.
"""
if self.__cache_block_locations is not None:
return self.__cache_block_locations
occupancy_map = self.get_occupancy_map()
cluster_candidates = self.fuzzy_mapping.generate_potential_clusters()
block_locations = self.hard_mapping.predict_block_locations(
cluster_candidates, occupancy_map)
if generate_fake_blocks:
# Try to avoid stalemates by patrolling
for block in PATROL_COORDINATES:
for robot_name in robot_states:
# Robots are near, phantom block is now a problem
if util.get_distance(robot_states[robot_name].position,
block) < PATROL_DECAY:
break
else:
block_locations.append(ClusterLocation(block, 1, 1, 0.4))
self.__cache_block_locations = block_locations
return self.__cache_block_locations
def request_block_dropoff(self, robot_name):
"""
The robot is holding a block, ensure it returns to its spawn position.
If the robot is close enough for mapping to be useless, let the robot takeover.
"""
robot_position = self.robot_states[robot_name].position
# Robot is close enough to handle everything by itself
destination_distance = util.get_distance(robot_position, util.ROBOT_SPAWN[robot_name])
if destination_distance < ROBOT_TAKEOVER_DISTANCE:
self.robot_paths[robot_name].reset_votes()
self.radio.send_message(protocol.AskRobotDeposit(
robot_name, util.ROBOT_SPAWN[robot_name]
))
return
# More complex routes needs to avoid collisions
closest_path = self.pathfinding.get_spawn_path(
robot_name, self.robot_states, self.robot_paths, robot_position
)
# Check if it was possible to find a path
if not closest_path.success:
# Send last waypoint message anyway
# But ensure at least part of it is legal
self.robot_paths[robot_name].waypoints = self.pathfinding.trim_legal_waypoints(
self.robot_paths[robot_name].waypoints,
robot_name,
self.robot_states,
self.robot_paths,
True
)
if len(self.robot_paths[robot_name].waypoints) > 0:
self.radio.send_message(protocol.GiveRobotTarget(
robot_name, self.robot_paths[robot_name].waypoints[0]
))
return
# Begin navigating back home
# Navigate to most popular waypoint
self.robot_paths[robot_name].vote_for_route(closest_path.waypoints)
if not closest_path.require_popout:
self.robot_paths[robot_name].crop_waypoints(self.robot_states[robot_name].position)
if len(self.robot_paths[robot_name].waypoints) > 0:
self.radio.send_message(protocol.GiveRobotTarget(
robot_name, self.robot_paths[robot_name].waypoints[0]
))
else:
self.robot_paths[robot_name].reset_votes()
def crop_waypoints(self, robot_position):
"""
Dynamically updates the robot's waypoints preventing circling.
NOTE: The robot is allowed to 'skip' waypoints
"""
current_waypoint = -1
for index, waypoint in enumerate(self.waypoints):
if util.get_distance(waypoint, robot_position) < WAYPOINT_TOLERANCE:
# Robot is close enough to waypoint to skip straight to this point in its path
current_waypoint = index + 1
if current_waypoint != -1:
# New waypoint was found, skip to this point
self.waypoints = self.waypoints[current_waypoint:]
def tick(self):
# Check for robot messages, take an necessary actions
self.process_robot_messages()
self.mapping_controller.output_to_displays()
# If robots are navigating to a block, ensure it still exists
block_locations = self.mapping_controller.predict_block_locations()
for robot_name in self.robot_targets:
block_still_exists = False
target_position = self.robot_targets[robot_name]
if not target_position.is_block:
continue # Only check if blocks exist for now
# Loop through all known blocks
for block in block_locations:
if util.get_distance(
block.coord,
target_position.coord) < APPROXIMATE_POSITION:
block_still_exists = True
if not block_still_exists and \
util.get_distance(self.robot_positions[robot_name], target_position.coord) > NO_TURNING_BACK_REGION:
print("[info] purging block location")
self.radio.send_message(protocol.RemoveWaypoints(robot_name))
def __call__(self):
if self.opening_pincer:
return self.pincer_controller.open_pincer()
current_position = self.positioning_system.get_2D_position()
if util.get_distance(current_position,
self.dropoff_target) < DROPOFF_RADIUS:
is_aligned = self.drive_controller.turn_toward_point(
self.positioning_system, self.dropoff_target,
DROPOFF_ANGLE_TOLERANCE)
if is_aligned:
self.drive_controller.halt()
self.opening_pincer = True
else:
self.drive_controller.navigate_toward_point(
self.positioning_system, self.dropoff_target)
return False
def request_block_collection(self, robot_name):
"""
Find the closest block of the correct color, ask the robot to drive towards it.
If the robot is close enough for mapping to be useless, let the robot takeover.
"""
robot_position = self.robot_states[robot_name].position
block_locations = self.mapping_controller.predict_block_locations(
self.robot_states, self.should_patrol(robot_name))
closest_path = self.pathfinding.get_nearest_block_path(
block_locations, robot_name, self.robot_states, self.robot_paths,
robot_position
)
# Check if map was detailed enough to find path
if not closest_path.success:
# Send last waypoint message anyway
self.robot_paths[robot_name].waypoints = self.pathfinding.trim_legal_waypoints(
self.robot_paths[robot_name].waypoints,
robot_name,
self.robot_states,
self.robot_paths,
False
)
if len(self.robot_paths[robot_name].waypoints) > 0:
self.radio.send_message(protocol.GiveRobotTarget(
robot_name, self.robot_paths[robot_name].waypoints[0]
))
return
# Should to robot switch over to this new path?
self.robot_paths[robot_name].vote_for_route(closest_path.waypoints)
if closest_path.require_popout:
self.robot_paths[robot_name].crop_waypoints(self.robot_states[robot_name].position)
chosen_path = self.robot_paths[robot_name].waypoints
if len(chosen_path) == 0:
self.robot_paths[robot_name].reset_votes()
return
next_waypoint = chosen_path[0]
last_waypoint = chosen_path[-1]
# Should the robot fine tune this part itself?
target_distance = util.get_distance(robot_position, last_waypoint)
if target_distance < ROBOT_TAKEOVER_DISTANCE:
self.radio.send_message(protocol.AskRobotSearch(
robot_name, last_waypoint
))
return
# Robot needs better instructions, it cannot do this by itself
# Sometimes waypoints will flicker and robot will be trapped
# Smooth neighboring waypoints to minimize this
if len(chosen_path) > 1:
next_waypoint = (
0.5 * (next_waypoint[0] + chosen_path[1][0]),
0.5 * (next_waypoint[1] + chosen_path[1][1])
)
# Send the robot its new target position (TODO prevent collisions)
self.radio.send_message(protocol.GiveRobotTarget(
robot_name, next_waypoint
))
def get_cluster_average(cluster_candidates, weights):
"""
Groups together True pixels and returns a list of ClusterLocation objects.
Output is entirely in world coordinates.
"""
# Err, comment to explain this absolute mess of a line
intense_coords = dict(zip(zip(*np.where(cluster_candidates)), itertools.repeat(False)))
strides = np.array([(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (-1, -1), (1, -1), (-1, 1)])
# Almost functional iterator
def find_cluster_coords(coord):
# Needs to be a tuple for indexing
index = tuple(coord)
if not util.tuple_in_bound(index, mapping.MAP_RESULTION) or \
not cluster_candidates[index] or intense_coords[index]:
# Block has already been counted or shouldn't be included
return []
# Mark this pixel to avoid repeated counting
intense_coords[index] = True
return list(reduce(
lambda old, stride: np.concatenate((old, find_cluster_coords(coord + stride))),
strides, coord
))
# Now finally, group the clusters
clusters_coords = []
for coord in intense_coords:
if intense_coords[coord] or len(coord) == 0:
continue # Already processed
cluster_coords = np.reshape(find_cluster_coords(np.array(coord)), (-1, 2)).astype(int)
clusters_coords.append(cluster_coords)
# Now do some statistics to find centers
cluster_locations = [] # list of BlockLocation
for cluster_coords in clusters_coords:
if len(cluster_coords) < mapping.CLUSTER_THRESHOLD:
continue
total_weight = 0
cluster_sum = np.array((0.0, 0.0))
for coord in cluster_coords:
total_weight += weights[tuple(coord)]
cluster_sum += coord * weights[tuple(coord)]
# Create a cluster representing the average properties of these coordinates
# Convert everything to worldspace
average_position = mapping._to_worldspace(cluster_sum / total_weight)
radius = 0
for coord in cluster_coords:
world_coord = mapping._to_worldspace(coord)
radius = max(radius, util.get_distance(average_position, world_coord))
cluster_locations.append(ClusterLocation(average_position, len(cluster_coords), total_weight, radius))
# Merge nearby clusters
merged_clusters = []
for cluster in cluster_locations:
for large_cluster in merged_clusters:
cluster_distance = util.get_distance(large_cluster.coord, cluster.coord)
# Test if clusters are close
if cluster_distance < mapping.CLUSTER_OVERLAP_THRESHOLD * (large_cluster.radius + cluster.radius):
# Clusters overlap, take a weighted average and change the original cluster
large_cluster.merge_cluster(cluster)
break
else:
# Cluster could not be merged into another cluster, add it to merged_clusters
merged_clusters.append(cluster)
return merged_clusters
def calculate_route(self, robot_name, arena_map, start, goal):
"""
Uses A* algorithm and knowledge of current path of other robot, along with obstacles
in arena to avoid collisions and find shortest path and convert that into a list of
waypoints.
"""
walks = ((1, 0, 1), (1, 1, 2**0.5), (0, 1, 1), (-1, 1, 2**0.5),
(-1, 0, 1), (-1, -1, 2**0.5), (0, -1, 1), (1, -1, 2**0.5))
distances = -np.ones((arena_map.shape), dtype=np.float64)
directions = -np.ones((arena_map.shape), dtype=np.int32)
path_mask = np.zeros((arena_map.shape), dtype=np.bool)
death_mask = np.zeros((arena_map.shape), dtype=np.bool)
distances[start] = 0
# PriorityQueue to order squares to visit by their distance + manhatte_distance to the goal
# meaning that it can find the optimal route without exploring the whole map
# Format: (distance from start + manhattan distance to goal, (x, z))
to_explore = PriorityQueue()
to_explore.put((0, start))
# create a bool map, where an obstacle is True and free space is False
arena_map = np.invert(arena_map)
# add the locations of already delivered blocks to the "Do Not Travel" areas
death_mask = death_mask | dilate(self.mask_delivered_blocks(arena_map),
7)
# leave a safety margin around areas of "Do Not Travel"
arena_map = arena_map | dilate(arena_map, 8)
# remove the area around the current target location, to eliminate the block to pick
# up from the mask of areas not to be traversed
arena_map[max(0, goal[0] - 10):min(arena_map.shape[0] - 1, goal[0] +
10),
max(0, goal[1] - 10):min(arena_map.shape[1] - 1, goal[1] +
10)] = False
# add the paths of the other robot to the "Do Not Travel" areas
for other_name in self.path_masks:
# Dilate all other robot paths
if other_name == robot_name:
continue
death_mask = death_mask | dilate(self.path_masks[other_name], 8)
self.send_to_display(arena_map)
# explore the map with added heuristic until it is all explored or you have reached the goal
while not to_explore.empty():
_, (cur_x, cur_z) = to_explore.get()
if (cur_x, cur_z) == goal:
break
for direction, (x, z, dist) in enumerate(walks):
# check if movement is in map
if not (0 <= cur_x + x < arena_map.shape[0]
and 0 <= cur_z + z < arena_map.shape[1]):
continue
if death_mask[cur_x + x, cur_z + z]:
continue
if arena_map[cur_x + x, cur_z +
z] == 1: # check if movement is in obstacle
dist *= 1000
# if already processed and too expensive, skip reevaluation
if distances[cur_x + x, cur_z + z] != -1:
if distances[cur_x, cur_z] + dist >= distances[cur_x + x,
cur_z + z]:
continue
directions[cur_x + x, cur_z + z] = direction
manhattan_dist = util.get_distance((cur_x + x, cur_z + z),
goal)
distances[cur_x + x,
cur_z + z] = distances[cur_x, cur_z] + dist
# store the newly reached square ready to explore its neighbours
to_explore.put(
(distances[cur_x + x, cur_z + z] + manhattan_dist,
(cur_x + x, cur_z + z)))
else:
return float('inf'), [], None, path_mask.copy() # No path found
# Create a list of waypoints for the robot to navigate along
waypoints = []
# Store location where robot needs to stop in order to pick up or drop off
# block at desired location. Required due to robot position being a few cm
# back compared to block position
release_pos = None
cur_x, cur_z = goal
path_mask[goal] = 1
direction = directions[goal]
# backtrack from the goal following the saved directions used to reach each square
while (cur_x, cur_z) != start:
cur_x -= walks[direction][0]
cur_z -= walks[direction][1]
goal_dist = distances[goal] - distances[cur_x, cur_z]
if 3 < goal_dist < 5:
release_pos = self.grid_to_world_coord((cur_x, cur_z))
if 9 < goal_dist < 11:
waypoints = [self.grid_to_world_coord((cur_x, cur_z))]
path_mask[cur_x, cur_z] = 1
if directions[cur_x, cur_z] != direction:
waypoints.append(self.grid_to_world_coord((cur_x, cur_z)))
direction = directions[cur_x, cur_z]
return distances[goal], waypoints, release_pos, path_mask.copy()
def predict_block_locations(self):
if self.__cache_block_locations is not None:
return self.__cache_block_locations
GRID_SIZE = (
int(MAP_RESULTION[0] / 5),
int(MAP_RESULTION[1] / 5)
)
# Identify all clusters
intensity_map = np.empty(MAP_RESULTION)
occupancy_map = self.get_occupancy_map()
mean_amplitude = np.quantile(occupancy_map, 0.99)
# Account for local variation in intensity
for x in range(0, MAP_RESULTION[0], 16):
for y in range(0, MAP_RESULTION[1], 16):
map_segment = occupancy_map[x:x + GRID_SIZE[0], y:y + GRID_SIZE[1]]
peak_amplitude = np.quantile(map_segment, 0.98)
intensity_map[x:x + GRID_SIZE[0], y:y + GRID_SIZE[1]] = map_segment > peak_amplitude
cluster_candidates = np.logical_and(
intensity_map,
np.logical_and(
occupancy_map > mean_amplitude,
occupancy_map > FORCE_INVAILD_THRESHOLD
)
)
# Smooth clusters into single block
clusters = get_cluster_average(cluster_candidates, occupancy_map)
# Some block colors are known, mark these in the clusters object
for index, (block_location, block_color) in enumerate(self._confirmed_blocks):
# Check if block is in radius of any clusters
color_consumed = False
# NOTE: this is a back hackfix
ClosestCluster = namedtuple("ClosestBlock", ["distance", "cluster"])
closest_cluster = ClosestCluster(np.inf, None)
for cluster in clusters:
cluster_distance = util.get_distance(cluster.coord, block_location)
if cluster_distance < CLUSTER_PROXIMITY_THRESHOLD:
if (cluster.color is not None):
print("[Warning] Multiple blocks identified in the same cluster")
if color_consumed:
print("[Warning] The same blocks has been assigned to multiple clusters")
color_consumed = True
cluster.assign_known_color(index, block_color)
if cluster_distance < closest_cluster.distance:
closest_cluster = ClosestCluster(
cluster_distance,
cluster
)
if not color_consumed:
# Hackfix, no cluster identified properly, assign to closest one
# print("[Warning] Block color has been identified but does not exist on map")
# closest_cluster.cluster.assign_known_color(index, block_color)
new_cluster = ClusterLocation(
block_location, 20, 10, BLOCK_OBSTACLE_WIDTH
)
new_cluster.assign_known_color(index, block_color)
clusters.append(new_cluster)
self.__cache_block_locations = clusters
return self.__cache_block_locations
def __calculate_route(self, robot_name, robot_states, other_paths,
start_pos, goal_pos, is_returning):
"""
Uses A* algorithm and knowledge of current path of other robot, along with obstacles
in arena to avoid collisions and find shortest path and convert that into a list of
waypoints.
"""
start, goal = tuple(_to_screenspace(start_pos)), tuple(
_to_screenspace(goal_pos))
# Setup the pathfinding
UNIT_STEP = 2
DIAGONAL_STEP = 3 # 3/2 ≈ √2
walks = np.array([(1, 0, UNIT_STEP), (1, 1, DIAGONAL_STEP),
(0, 1, UNIT_STEP), (-1, 1, DIAGONAL_STEP),
(-1, 0, UNIT_STEP), (-1, -1, DIAGONAL_STEP),
(0, -1, UNIT_STEP), (1, -1, DIAGONAL_STEP)],
dtype=np.int32)
pathfinding_map = self._simple_map.copy()
pathfinding_map[goal[0] - 1:goal[0] + 2,
goal[1] - 1:goal[1] + 2] = True
other_robot_map = self._generate_cooperation_map(
robot_name, robot_states, other_paths, is_returning)
popout_position = None
# Ensure the starting point is correct
if not pathfinding_map[start] or not other_robot_map[start]:
# The robot is in an illegal square, it must leave this square before continuing.
popout_position = self.get_popout_position(
robot_name, start_pos, pathfinding_map & other_robot_map)
if popout_position is not None:
start = tuple(_to_screenspace(popout_position))
if robot_name == "Small":
pixels = util.get_intensity_map_pixels(pathfinding_map
& other_robot_map)
pixels[start] = (255, 0, 0)
pixels[goal] = (0, 0, 255)
util.display_numpy_pixels(self._display_navigation, pixels)
costs = np.inf * np.ones(SIMPLIFIED_RESOLUTION)
directions = np.empty(SIMPLIFIED_RESOLUTION, dtype=np.int32)
# PriorityQueue to order squares to visit by their distance + manhatte_distance to the goal
# meaning that it can find the optimal route without exploring the whole map
# Format: (distance from start + manhattan distance to goal, (x, z))
to_explore = [(0, start)]
costs[start] = 0
# explore the map with added heuristic until it is all explored or you have reached the goal
while len(to_explore) > 0:
_, current_pos = heapq.heappop(to_explore)
for direction_id, (x, z, step_dist) in enumerate(walks):
new_pos = (current_pos[0] + x, current_pos[1] + z)
# check if movement is in map
if not util.tuple_in_bound(new_pos, SIMPLIFIED_RESOLUTION):
continue
# if already processed and too expensive, skip reevaluation
new_cost = costs[current_pos] + step_dist
if not other_robot_map[new_pos]:
# Avoid all robot/ robot collisions
continue
# check if movement is in obstacle
# Targets cannot be obstacles
if new_pos != goal and (not pathfinding_map[new_pos]):
new_cost += 30
continue
if new_cost >= costs[new_pos] or new_cost >= costs[goal]:
continue
manhattan_dist = abs(new_pos[0] - goal[0]) + abs(new_pos[1] -
goal[1])
costs[new_pos] = new_cost
directions[new_pos] = direction_id
# store the newly reached square ready to explore its neighbours
heapq.heappush(to_explore,
(new_cost + manhattan_dist, new_pos))
# Check if pathfinding succeeded
if np.isinf(costs[goal]):
# if popout_position is not None:
# return PathfindingResult([popout_position], 1000, goal_pos, True)
return PathfindingResult.NotFound(goal_pos)
# Create a list of waypoints for the robot to navigate along
waypoints = [tuple(goal_pos)]
# backtrack from the goal following the saved directions used to reach each square
current_coord = goal
direction_index = directions[current_coord]
while current_coord != start:
current_coord = (current_coord[0] - walks[direction_index][0],
current_coord[1] - walks[direction_index][1])
# Bad edge case here, the algorithm really should have accommodated this from the start
if current_coord == start:
break
# Output a minimum number of waypoint to allow better control
waypoints.append(tuple(_to_worldspace(current_coord)))
direction_index = directions[current_coord]
# Get the pathfinding error (to ensure true closest block is selected)
if goal != start:
penultimate_coord = (goal[0] - walks[directions[goal]][0],
goal[1] - walks[directions[goal]][1])
penultimate_cost = costs[penultimate_coord]
cost_approximation = penultimate_cost + util.get_distance(
goal_pos, _to_worldspace(penultimate_coord))
else:
cost_approximation = costs[goal]
if popout_position is None:
return PathfindingResult(waypoints[::-1], cost_approximation,
goal_pos, False)
return PathfindingResult(waypoints[::-1], cost_approximation, goal_pos,
False)