def plan(self,
start_state,
dest_state,
max_num_steps,
max_steering_radius,
dest_reached_radius,
live_view=True):
"""
Returns a path as a sequence of states [start_state, ..., dest_state]
if dest_state is reachable from start_state. Otherwise returns [start_state].
Assume both source and destination are in free space.
"""
assert (self.state_is_free(start_state))
assert (self.state_is_free(dest_state))
# The set containing the nodes of the tree
tree_nodes = set()
tree_nodes.add(start_state)
# image to be used to display the tree
img = np.copy(self.world)
cv2.circle(img, (start_state.x, start_state.y), 2, (255, 0, 0))
plan = [start_state]
for step in range(max_num_steps):
# TODO: Use the methods of this class as in the slides to
# compute s_new correctly. The code here has several problems, as you'll see
# in the output.
s_nearest = start_state
s_new = self.sample_state()
s_new.parent = start_state
if self.path_is_obstacle_free(s_nearest, s_new):
tree_nodes.add(s_new)
s_nearest.children.append(s_new)
# If we approach the destination within a few pixels
# we're done. Return the path.
if s_new.euclidean_distance(dest_state) < dest_reached_radius:
dest_state.parent = s_new
plan = self._follow_parent_pointers(dest_state)
break
# plot the new node and edge
cv2.circle(img, (s_new.x, s_new.y), 3, (0, 0, 255), 3)
cv2.line(img, (s_nearest.x, s_nearest.y), (s_new.x, s_new.y),
(255, 0, 0), 2)
# Keep showing the image for a bit even
# if we don't add a new node and edge
if live_view:
cv2.imshow('image', img)
cv2.waitKey(100)
if live_view:
draw_plan(img, plan, bgr=(0, 0, 255), thickness=2, show_live=True)
cv2.waitKey(0)
return plan
def plan(self, start_state, dest_state, max_num_steps, max_steering_radius,
dest_reached_radius):
"""
Returns a path as a sequence of states [start_state, ..., dest_state]
if dest_state is reachable from start_state. Otherwise returns [start_state].
Assume both source and destination are in free space.
"""
## n is max_num_steps
## V is Set of nodes
## E is Set of edges
assert (self.state_is_free(start_state))
assert (self.state_is_free(dest_state))
# The set containing the nodes of the tree
tree_nodes = set()
tree_nodes.add(start_state)
# image to be used to display the tree
img = np.copy(self.world)
plan = [start_state]
for step in xrange(max_num_steps):
# TODO: Use the methods of this class as in the slides to
# compute s_new
## Set variable nodes
s_rand = self.sample_state()
s_nearest = self.find_closest_state(tree_nodes, s_rand)
s_new = self.steer_towards(s_nearest, s_rand, max_steering_radius)
if self.path_is_obstacle_free(s_nearest, s_new):
tree_nodes.add(s_new)
s_nearest.children.append(s_new)
# If we approach the destination within a few pixels
# we're done. Return the path.
if s_new.euclidean_distance(dest_state) < dest_reached_radius:
dest_state.parent = s_new
plan = self._follow_parent_pointers(dest_state)
break
# plot the new node and edge
cv2.circle(img, (s_new.x, s_new.y), 2, (0, 0, 0))
cv2.line(img, (s_nearest.x, s_nearest.y), (s_new.x, s_new.y),
(255, 0, 0)) ##Red
#time.sleep(3)
# Keep showing the image for a bit even
# if we don't add a new node and edge
cv2.imshow('image', img)
cv2.waitKey(10)
draw_plan(img, plan, bgr=(0, 0, 255), thickness=2)
cv2.waitKey(0)
return [start_state]
def run_helper(world, planning_model, start_state, dest_state, model_type,
filepath):
start = datetime.now()
try:
start_state = start_state
dest_state = dest_state
plan = astar.plan(start_state, dest_state, model_type="ASTAR")
draw_plan(world=world, plan=plan, filepath=filepath)
except Exception as e:
print(e)
print("Model runtime is {} for model {} ".format(datetime.now() - start,
model_type))
# load the image
image = Image.open(sys.argv[1])
# convert image to numpy array
world = asarray(image)
astar = AStarPlanner(world)
start_state = State(10, 10)
# TODO: Change the goal state to 3 different values, saving and running between each
# in order to produce your images to submit
dest_state = State(275, 350)
print("astar_planner.py is attempting to find a path from (" +
str(start_state.x) + ',' + str(start_state.y) + ') to (' +
str(dest_state.x) + ',' + str(dest_state.y) +
') in the image named ' + sys.argv[1] + '.')
print('This may take a few moments...')
#Time used for testing to see speedup compared to Dijkstra's
start_time = time.time()
plan = astar.plan(start_state, dest_state)
end_time = time.time()
draw_plan(world,
plan,
bgr=(0, 0, 255),
thickness=2,
show_live=False,
filename='astar_result.png')
print('Planning complete in ' + str(end_time - start_time) +
'. See image astar_result.png to check the plan.')
max_num_steps = 1000 # max number of nodes to be added to the tree
max_steering_radius = 30 # pixels
dest_reached_radius = 50 # pixels
plan_2 = rrt.plan(start_state,
dest_state_2,
max_num_steps,
max_steering_radius,
dest_reached_radius,
live_view=True)
print('RRT planning complete. Saving image.')
draw_plan(world,
plan_2,
bgr=(0, 0, 255),
thickness=2,
show_live=False,
filename='rrt_result2_XinranLi.png')
plan_1 = rrt.plan(start_state,
dest_state_1,
max_num_steps,
max_steering_radius,
dest_reached_radius,
live_view=True)
print('RRT planning complete. Saving image.')
draw_plan(world,
plan_1,
bgr=(0, 0, 255),
thickness=2,
show_live=False,
# to visit it then update its priority in the queue, and also its
# distance to come and its parent
if (ns not in Q) or (alternative_dist_to_come_to_ns <
dist_to_come[ns.x, ns.y]):
Q[ns] = alternative_dist_to_come_to_ns + self.euclidean_dist(
ns, dest_state)
dist_to_come[ns.x, ns.y] = alternative_dist_to_come_to_ns
parents[ns] = s
return [start_state]
if __name__ == "__main__":
if len(sys.argv) < 2:
print "Usage: astar_planner.py occupancy_grid.pkl"
sys.exit(1)
pkl_file = open(sys.argv[1], 'rb')
# world is a numpy array with dimensions (rows, cols, 3 color channels)
world = pickle.load(pkl_file)
pkl_file.close()
astar = AStarPlanner(world)
start_state = State(10, 10)
dest_state = State(500, 100)
plan, visited = astar.plan(start_state, dest_state)
draw_plan(world, plan)
draw_visited(world, visited)
