def continue_FM_search(self):
nodes_popped=0
if self.image_frames != 0:
self.plot_cost, NULL = min(self.frontier.elements)
while True:
try:
c_priority, c_node = self.frontier.pop()
except KeyError:
break
nodes_popped+=1
u_A = c_priority - self.heuristic_fun(c_node, self.end_node)
self.cost_to_come[c_node] = u_A
for n_node, tau_k in self.graph.neighbours(c_node):
# if n_node not in cost_to_come
parent_update = []
u_B = u_A + tau_k + 1.0
adjacency = (n_node[0]-c_node[0], n_node[1]-c_node[1])
for adjacent_node in self.adjacency_list[adjacency]:
B_node = (n_node[0]+adjacent_node[0], n_node[1]+adjacent_node[1])
if B_node in self.cost_to_come and self.cost_to_come[B_node] < u_B:
u_B = self.cost_to_come[B_node]
BB_node = B_node
if tau_k > abs(u_A - u_B):
c_cost = 0.5*(u_A + u_B + math.sqrt(2*tau_k**2 - (u_A - u_B)**2))
parent_update = [c_node, BB_node]
else:
if u_A <= u_B:
c_cost = u_A + tau_k
parent_update = [c_node]
else:
c_cost = u_B + tau_k
parent_update = [BB_node]
if n_node not in self.cost_to_come or self.cost_to_come[n_node] > c_cost:
self.frontier.push(n_node, c_cost + self.heuristic_fun(n_node, self.end_node))
self.parent_list[n_node] = parent_update
if self.image_frames != 0 and u_A > self.plot_cost :
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come, start_nodes=self.start_node))
self.plot_cost += self.delta_plot
if (c_node == self.end_node) or (self.frontier.count() <= 0):
# self.frontier.push(c_node, u_A)
break
# Append final frame
if self.image_frames != 0:
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come))
# print "FM search: nodes popped: {0}".format(nodes_popped)
self.search_nodes = nodes_popped
def ReSearch(self, Q, A, P_g, C_b):
c_n = 0
nodes_popped = 0
while (Q.count() > 0) and (c_n < C_b/2):
try:
c_n, n = Q.pop()
nodes_popped+=1
except KeyError:
break
if n in A:
return c_n + A[n]
A[n] = c_n
for m, tau in self.graph.neighbours(n):
c_k = c_n + tau + 1.0
for k, temp in self.graph.neighbours(m):
if k in A and A[k] < c_k:
c_k = A[k]
if tau > abs(c_n - c_k):
c_cost = 0.5*(c_n + c_k + math.sqrt(2*tau**2 - (c_n - c_k)**2))
else:
if c_n <= c_k:
c_cost = c_n + tau
else:
c_cost = c_k + tau
if m in A and P_g[m] < P_g[n]:
if c_cost < A[m]:
del A[m]
Q.push(m, c_cost)
P_g[m] = P_g[n]
else:
Q.push(m, c_cost)
P_g[m] = P_g[n]
if self.image_frames != 0 and c_n > self.plot_cost :
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, A))
self.plot_cost = c_n + self.delta_plot
if self.image_frames != 0:
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, A))
print "biFM ReSearch: nodes popped: {0}".format(nodes_popped)
self.search_nodes=nodes_popped
return C_b
def make_video(self, leading_frame=[], trailing_frame=[]):
graph_frame, TEMP = fm_plottools.draw_grid(self.axes, self.graph)
costpath_frame = fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come, self.path)
corridor_frame = fm_plottools.draw_corridor(self.axes, self.graph, self.cost_to_come, self.corridor, self.corridor_interface, self.path)
path_frame, TEMP = fm_plottools.draw_grid(self.axes, self.graph, self.path)
video_frames = []
frame_hold = int(len(self.image_frames)/8)
if len(leading_frame) > 0:
for ii in range(frame_hold): video_frames.append(leading_frame)
for ii in range(frame_hold): video_frames.append(graph_frame)
video_frames.extend(self.image_frames)
for ii in range(frame_hold): video_frames.append(costpath_frame)
for ii in range(frame_hold): video_frames.append(corridor_frame)
for ii in range(frame_hold): video_frames.append(path_frame)
if len(trailing_frame) > 0:
for ii in range(frame_hold): video_frames.append(trailing_frame)
return video_frames
def make_pictures(self, fig_dir):
self.axes.figure.set_size_inches(9,6)
graph_frame, barlims = fm_plottools.draw_grid(self.axes, self.graph)
barlims = np.floor(barlims)
delta = np.floor((barlims[1]-barlims[0])/6*10)/10
cbar = self.axes.figure.colorbar(graph_frame[0], ticks=[barlims[0]+x*delta for x in range(7)])
self.axes.figure.savefig(fig_dir+'graph_cost.pdf', bbox_inches='tight')
path_frame, barlims = fm_plottools.draw_grid(self.axes, self.graph, self.path)
self.axes.figure.savefig(fig_dir+'graph_path.pdf', bbox_inches='tight')
cbar.ax.clear()
costpath_frame = fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come, self.path)
barlims = [min(self.cost_to_come.values()), max(self.cost_to_come.values())]
delta = np.floor((barlims[1]-barlims[0])/6*10)/10
self.axes.figure.colorbar(costpath_frame[0], ticks=[barlims[0]+x*delta for x in range(7)], cax=cbar.ax)
self.axes.figure.savefig(fig_dir+'path_cost.pdf', bbox_inches='tight')
corridor_frame = fm_plottools.draw_corridor(self.axes, self.graph, self.cost_to_come, self.corridor, self.corridor_interface, self.path)
self.axes.figure.savefig(fig_dir+'corridor.pdf', bbox_inches='tight')
else:
if u_A <= u_B:
c_cost = u_A + tau_k
else:
c_cost = u_B + tau_k
if n_node not in cost_to_come:
frontier.push(n_node, c_cost)
elif n_node in interface:
if c_cost + self.FastMarcherGS.cost_to_come[n_node] < self.updated_min_path_cost:
self.updated_min_path_cost = c_cost + self.FastMarcherGS.cost_to_come[n_node]
midnode = c_node
#print "Better path found!"
if self.image_frames != 0 and u_A > self.plot_cost :
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, cost_to_come))
self.plot_cost = u_A + self.delta_plot
if self.image_frames != 0:
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come))
# print "biFM search: nodes popped: {0}".format(nodes_popped)
self.search_nodes=nodes_popped
# Append final frame
if self.image_frames != 0:
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, cost_to_come))
# print "fbFM update: nodes popped: {0}".format(nodes_popped)
self.search_nodes = nodes_popped
if recalc_path:
if midnode != (-1,-1):
if "GP_model" not in locals():
V_Ireland = np.array([[0, 0], [-43, -38], [-70, -94], [-60, -150], [0, -180], [54, -152], [85, -70], [0, 0]])
start_node = (0, -2)
goal_node = (-30, -150)
model_file = os.path.expanduser("~") + "/catkin_ws/src/ros_lutra/data/IrelandLnModel.pkl"
fh = open(model_file, "rb")
GP_model = pickle.load(fh)
mean_depth = pickle.load(fh)
fh.close()
op_region = OperatingRegion(V_Ireland, start_node, goal_node)
true_g = fm_graphtools.CostmapGridFixedObs(
op_region.width, op_region.height, obstacles=op_region.obstacles, bl_corner=[op_region.left, op_region.bottom]
)
explorer_cost = bfm_explorer.mat_cost_function_GP(true_g, GP_cost_function, max_depth=4.5, mean_depth=mean_depth)
true_g.cost_fun = explorer_cost.calc_cost
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_goal(goal_node)
tFM.set_start(start_node)
tFM.search()
tFM.pull_path()
f0, a0 = fm_plottools.init_fig()
f1, a1 = fm_plottools.init_fig()
f2, a2 = fm_plottools.init_fig()
fm_plottools.draw_grid(a0, true_g, tFM.path)
fm_plottools.draw_costmap(a1, true_g, tFM.FastMarcherSG.cost_to_come, tFM.path)
fm_plottools.draw_fbfmcost(a2, true_g, tFM.path_cost, tFM.path)
plt.show()
def plot_cost_frame(self, cost_to_come,loc):
tempframe=fm_plottools.draw_costmap(self.axes, self.FastMarcherSG.graph, cost_to_come)
if loc != None:
tempframe.append(self.axes.plot(loc[0], loc[1], 'wx', mew=2, ms=10)[0])
tempframe.append(self.axes.plot(loc[0], loc[1], 'wo', mew=1, ms=80, mfc='none', mec='w' )[0])
return tempframe
def update(self, new_cost, recalc_path = 0):
# New cost should be added as a dictionary, with elements [(node) : delta_cost]
midnode = (-1,-1)
# Strip zero cost nodes and obstacle nodes
new_cost = {node:new_cost[node] for node in new_cost if ((new_cost[node] != 0) and (node in self.path_cost))}
self.graph.clear_delta_costs()
self.graph.add_delta_costs(new_cost)
# All nodes that have changed cost are killed
kill_list = set(new_cost.keys())
# Interface nodes are neighbours of a killed node with a lower cost (that are not also being killed)
interface = set()
cost_to_come = copy.copy(self.FastMarcherSG.cost_to_come)
for node in new_cost:
for nnode, costTEMP in self.graph.neighbours(node):
if (cost_to_come[nnode] < cost_to_come[node]):
interface.add(nnode)
# Find boundary points closest to start and goal
min_cts = self.min_path_cost
min_ctg = self.min_path_cost
frontier = fm_graphtools.PriorityQueue([])
frontier.clear()
if self.start_node in kill_list:
frontier.push(self.start_node, 0)
# Push all interface nodes onto the frontier
for internode in interface:
if (internode in self.path_cost) and (internode not in kill_list):
frontier.push(internode, cost_to_come[internode])
min_cts = min(min_cts, cost_to_come[internode])
min_ctg = min(min_ctg, self.FastMarcherGS.cost_to_come[internode])
if min_cts+min_ctg > self.min_path_cost:
self.updated_min_path_cost = self.min_path_cost
return
temp_path_cost = copy.copy(self.path_cost)
for killnode in kill_list:
del cost_to_come[killnode]
del temp_path_cost[killnode]
# if killnode in new_parent_list: del new_parent_list[killnode]
# Current minimum path cost
self.updated_min_path_cost = min(temp_path_cost.values())
#if self.image_frames != 0:
# self.axes.clear()
# fm_plottools.draw_corridor(self.axes, self.graph, self.cost_to_come, self.cost_to_come.keys(), interface-kill_list)
# self.axes.figure.savefig('/home/nick/Dropbox/work/FastMarching/fig/map_update.pdf', bbox_inches='tight')
nodes_popped=0
if self.image_frames != 0:
self.plot_cost, NULL = min(frontier.elements)
u_A = 0
# I changed this from u_A + min_ctg < self.min_path_cost
while u_A + min_ctg < self.min_path_cost:
try:
c_priority, c_node = frontier.pop()
except KeyError:
break
nodes_popped+=1
u_A = c_priority
cost_to_come[c_node] = u_A
for n_node, tau_k in self.graph.neighbours(c_node):
u_B = u_A + tau_k + 1.0
adjacency = (n_node[0]-c_node[0], n_node[1]-c_node[1])
for adjacent_node in self.FastMarcherSG.adjacency_list[adjacency]:
B_node = (n_node[0]+adjacent_node[0], n_node[1]+adjacent_node[1])
if B_node in cost_to_come and cost_to_come[B_node] < u_B:
u_B = cost_to_come[B_node]
if tau_k > abs(u_A - u_B):
c_cost = 0.5*(u_A + u_B + math.sqrt(2*tau_k**2 - (u_A - u_B)**2))
else:
if u_A <= u_B:
c_cost = u_A + tau_k
else:
c_cost = u_B + tau_k
if n_node not in cost_to_come:
frontier.push(n_node, c_cost)
elif n_node in interface:
if c_cost + self.FastMarcherGS.cost_to_come[n_node] < self.updated_min_path_cost:
self.updated_min_path_cost = c_cost + self.FastMarcherGS.cost_to_come[n_node]
midnode = c_node
#print "Better path found!"
if self.image_frames != 0 and u_A > self.plot_cost :
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, cost_to_come))
self.plot_cost = u_A + self.delta_plot
# Append final frame
if self.image_frames != 0:
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, cost_to_come))
# print "fbFM update: nodes popped: {0}".format(nodes_popped)
self.search_nodes = nodes_popped
if recalc_path:
if midnode != (-1,-1):
tempctc = copy.copy(self.FastMarcherSG.cost_to_come)
self.FastMarcherSG.cost_to_come = cost_to_come
path1 = self.FastMarcherSG.path_source_to_point(self.start_node, midnode)
path2 = self.FastMarcherGS.path_source_to_point(self.end_node, midnode)
path2.remove(midnode)
path2.reverse()
path1.extend(path2)
self.updated_path = path1
self.FastMarcherSG.cost_to_come = tempctc
else:
self.updated_path = copy.copy(self.path)
return
def continue_bFM_search(self):
if self.image_frames != 0:
self.plot_cost, NULL = min(self.frontier.elements)
finished = False
nodes_popped = 0
while (self.frontier.count() > 0) and finished == False:
try:
c_priority, c_node = self.frontier.pop()
if self.global_parent[c_node] != c_node:
for node in self.parent_list[c_node]:
if node not in self.cost_to_come:
raise ValueError("InvalidNode")
nodes_popped+=1
except ValueError:
continue
except KeyError:
break
u_A = c_priority
self.cost_to_come[c_node] = u_A
for n_node, tau_k in self.graph.neighbours(c_node):
# if n_node not in cost_to_come
parent_update = []
u_B = u_A + tau_k + 1.0
adjacency = (n_node[0]-c_node[0], n_node[1]-c_node[1])
for adjacent_node in self.adjacency_list[adjacency]:
B_node = (n_node[0]+adjacent_node[0], n_node[1]+adjacent_node[1])
if (B_node in self.cost_to_come) and \
(self.cost_to_come[B_node] < u_B) and \
(self.global_parent[B_node] == self.global_parent[c_node]):
u_B = self.cost_to_come[B_node]
BB_node = B_node
if tau_k > abs(u_A - u_B):
c_cost = 0.5*(u_A + u_B + math.sqrt(2*tau_k**2 - (u_A - u_B)**2))
parent_update = [c_node, BB_node]
else:
if u_A <= u_B:
c_cost = u_A + tau_k
parent_update = [c_node]
else:
c_cost = u_B + tau_k
parent_update = [BB_node]
if n_node not in self.cost_to_come :
self.frontier.push(n_node, c_cost)
self.parent_list[n_node] = parent_update
self.global_parent[n_node] = self.global_parent[c_node]
elif self.global_parent[n_node] == self.global_parent[c_node] and self.cost_to_come[n_node] > c_cost:
self.frontier.push(n_node, c_cost)
self.parent_list[n_node] = parent_update
elif self.global_parent[n_node] != self.global_parent[c_node]:
self.best_midnode = c_node
self.best_midfacenode = n_node
self.best_cost = c_cost + self.cost_to_come[n_node]
self.frontier.push(c_node, u_A)
finished = True
if self.image_frames != 0 and u_A > self.plot_cost :
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come))
self.plot_cost += self.delta_plot
if self.image_frames != 0:
self.image_frames.append(fm_plottools.draw_costmap(self.axes, self.graph, self.cost_to_come))
# print "biFM search: nodes popped: {0}".format(nodes_popped)
self.search_nodes=nodes_popped
