def searching(self):
"""
Searching using BFS.
:return: planning path, action in each node, visited nodes in the planning process
"""
q_bfs = queue.QueueFIFO() # first-in-first-out queue
q_bfs.put(self.xI)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
while not q_bfs.empty():
x_current = q_bfs.get()
if x_current == self.xG:
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
for u_next in self.u_set: # explore neighborhoods of current node
x_next = tuple(
[x_current[i] + u_next[i] for i in range(len(x_current))])
if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles
q_bfs.put(x_next)
parent[x_next] = x_current
action[x_next] = u_next
[path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent,
action) # extract path
return path_bfs, action_bfs
def simulation(self, xI, xG, policy):
"""
simulate a path using converged policy.
:param xI: starting state
:param xG: goal state
:param policy: converged policy
:return: simulation path
"""
plt.figure(1) # path animation
tools.show_map(xI, xG, self.obs, self.lose,
self.name1) # show background
x, path = xI, []
while True:
u = self.u_set[policy[x]]
x_next = (x[0] + u[0], x[1] + u[1])
if x_next in self.obs:
print("Collision!") # collision: simulation failed
else:
x = x_next
if x_next in xG:
break
else:
tools.plot_dots(x) # each state in optimal path
path.append(x)
plt.show()
return path
def searching(self):
"""
Searching using A_star.
:return: planning path, action in each node, visited nodes in the planning process
"""
q_astar = queue.QueuePrior() # priority queue
q_astar.put(self.xI, 0)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
cost = {self.xI: 0}
while not q_astar.empty():
x_current = q_astar.get()
if x_current == self.xG: # stop condition
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
for u_next in self.u_set: # explore neighborhoods of current node
x_next = tuple(
[x_current[i] + u_next[i] for i in range(len(x_current))])
if x_next not in self.obs:
new_cost = cost[x_current] + self.get_cost(
x_current, u_next)
if x_next not in cost or new_cost < cost[
x_next]: # conditions for updating cost
cost[x_next] = new_cost
priority = new_cost + self.Heuristic(
x_next, self.xG, self.heuristic_type)
q_astar.put(
x_next, priority
) # put node into queue using priority "f+h"
parent[x_next] = x_current
action[x_next] = u_next
[path_astar,
actions_astar] = tools.extract_path(self.xI, self.xG, parent, action)
return path_astar, actions_astar
def searching(self):
"""
Searching using Dijkstra.
:return: planning path, action in each node, visited nodes in the planning process
"""
q_dijk = queue.QueuePrior() # priority queue
q_dijk.put(self.xI, 0)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
cost = {self.xI: 0}
while not q_dijk.empty():
x_current = q_dijk.get()
if x_current == self.xG: # stop condition
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
for u_next in self.u_set: # explore neighborhoods of current node
x_next = tuple(
[x_current[i] + u_next[i] for i in range(len(x_current))])
if x_next not in self.obs: # node not visited and not in obstacles
new_cost = cost[x_current] + self.get_cost(
x_current, u_next)
if x_next not in cost or new_cost < cost[x_next]:
cost[x_next] = new_cost
priority = new_cost
q_dijk.put(
x_next, priority
) # put node into queue using cost to come as priority
parent[x_next] = x_current
action[x_next] = u_next
[path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent,
action)
return path_dijk, action_dijk