def replanning(original_graph, virtual_graph, list_of_H):
PolC = original_graph['PolC']
colors_0 = ['b', 'r', 'g', 'y']
# 'change_plan' is True if the new plan is better than the old one
change_plan = False
R = len(list_of_H)
list_of_robs = []
for r in range(R):
list_of_robs.append(list_of_H[r].id)
id = min(list_of_robs) #id of the robot that is computing the replan
speeds = []
search_speeds = []
colors = []
for r in range(R):
#exec ('H%d = list_of_H[%d]' % (r, r))
#exec ('speeds.append(H%d.specs[0])' % r)
#exec ('search_speeds.append(H%d.specs[1])' % r)
#exec ('colors.append(colors_0[H%d.id])' % r)
speeds.append(list_of_H[r].specs[0])
search_speeds.append(list_of_H[r].specs[1])
colors.append(colors_0[list_of_H[r].id])
old_cost = myLib.get_cost(original_graph, list_of_H, speeds, search_speeds)
set_uv, set_v, set_g, pts = define_subsets(list_of_H, virtual_graph)
print '\nHere is set_v (visited edges) in original inedexes:'
print set_v
print 'Here is set_uv (unvisited edges) in original inedexes:'
print set_uv
print 'Here is set_g (assigned edges) in original inedexes:'
print set_g, '\n\n'
# Map the unvisited nodes with new labels
C = virtual_graph['Ccom'] #matrix with the length costs
Cuv = np.matrix(
C
) # Reduced cost matrix with only the unvisited edges (unvisited virtual nodes)
C = virtual_graph['C_sp'] #matrix with the number of search points
Cuv_sp = np.matrix(
C
) # Reduced cost matrix with only the unvisited edges (unvisited virtual nodes)
#exclude_list = (np.array(set_v) - 1).tolist()
exclude_list = (np.array(set_v) - 1).tolist() + (np.array(set_g) -
1).tolist()
Cuv = np.delete(Cuv, exclude_list, 0) # exclude lines
Cuv = np.delete(Cuv, exclude_list, 1) # exclude columns
Cuv = Cuv.tolist()
Cuv_sp = np.delete(Cuv_sp, exclude_list, 0) # exclude lines
Cuv_sp = np.delete(Cuv_sp, exclude_list, 1) # exclude columns
Cuv_sp = Cuv_sp.tolist()
# Define the depot virtual nodes for the task assignment
depots = []
for r in range(R):
#exec ('depots.append(set_uv.index(H%d.currEdge))' % r)
depots.append(set_uv.index(list_of_H[r].currEdge))
print '\nHere is depot virtual nodes (new indexes):'
print depots
print 'Here is depot virtual nodes (original indexes):'
depots_ori = [] #original depot points??
for r in range(R):
depots_ori.append(set_uv[depots[r]])
print depots_ori, '\n'
print '\n ----- Task assignment function called -----'
#[sol, C_check0, C_check1] = TA.execute_lp(speeds, search_speeds, depots, colors, Cuv, Cuv_sp, pts)
sol = TA.execute_lp(speeds, search_speeds, depots, colors, Cuv, Cuv_sp,
pts)
#print "Here is the solution of the TA problem: \n", sol
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
# Map the solution back to the original indexes
n_uv = len(Cuv)
m_uv = n_uv * (n_uv - 1)
x_var = []
for r in range(R):
#exec ('x%d_var = []' % r)
x_var.append([])
F_var = sol.pop()
for r in range(R):
for k in range(m_uv):
#exec ('x%d_var.insert(0,sol.pop())' % (R - r - 1))
x_var[R - r - 1].insert(0, sol.pop())
division = []
for r in range(R):
division.append([])
k = -1
for i in range(n_uv):
for j in range(n_uv):
if i != j:
k = k + 1
for r in range(R):
#exec ('xAux_var = x%d_var' % r)
xAux_var = x_var[r]
if (xAux_var[k] == 1):
division[r].append(i)
division[r].append(j)
for r in range(R):
division[r] = list(set(division[r]))
#Map the nodes back to the original indexation
for r in range(R):
for k in range(len(division[r])):
division[r][k] = set_uv[division[r][k]]
print '\nAssigned edges for the robots: (before excluding the current edges)'
for r in range(R):
print 'Subset for robot ' + str(list_of_H[r].id) + ': ', division[r]
print '\n'
# Exclude/Remove the used depot virtual nodes (they are edges that were already visited)
for r in range(R):
print 'division[r]', division[r]
print 'division[r].index(list_of_H[r].currEdge)', division[r].index(
list_of_H[r].currEdge)
division[r].pop(division[r].index(list_of_H[r].currEdge))
# Include the used depot virtual nodes in the visited set
for r in range(R):
set_v.append(list_of_H[r].currEdge)
print '\nAssigned edges for the robots:'
for r in range(R):
print 'Subset for robot ' + str(list_of_H[r].id) + ': ', division[r]
print '\n'
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
# Call MST for every robot in the communication graph in order to make the graph connected
print '\n ----- Applying MST to the disconnected subgraphs ------'
subgraphs = []
for r in range(R):
subgraphs.append([])
#pylab.figure(100)
pylab.figure(id)
pylab.subplot(2, R, R + (r + 1))
subgraphs[r] = MST.MSTconnect(original_graph, division[r],
(list_of_H[r]).nextNode, colors[r], True)
for r in range(R):
print 'Subset of edges for robot ' + str(
r) + ': (original indexes)\n', subgraphs[r]
print 'Start node for robot ' + str(r) + ':', (list_of_H[r]).nextNode
# ---------- ---------- ---------- ---------- ----------
# Cal CPP for every graph generated by the MST based algorithm
print '\nApplying CPP to the connected subgraphs'
Hole_path_list = []
for r in range(R):
Hole_path_list.append([])
current_node = (list_of_H[r]).nextNode
edges_listOfTuples = myLib.write_listOfTuples(original_graph,
subgraphs[r])
#Hole_path_list[r] = cppsolver.CPP(sorted(edges_listOfTuples), current_node)
Hole_path_list[r] = CPPlib.main_CPP(sorted(edges_listOfTuples),
current_node)
#for k in range(R):
print 'Route for robot ' + str(r), ': ', Hole_path_list[r]
print '\n'
# ---------- ---------- ---------- ---------- ----------
#Create list T_a
pts_0 = virtual_graph['nodes']
T_a0 = [Intlist()]
for k in range(len(PolC[0]) - 1):
IL = Intlist()
T_a0.append(IL)
for k in range(len(pts_0)):
T_a0[k].data = list(list_of_H[0].T_a[0].data) + list(
list_of_H[1].T_a[1].data)
T_a0[k].data = list(T_a0[k].data)
if k + 1 in set_uv: # if in unvisite list
if k + 1 in division[0]: # if in the assigned to the other one
T_a0[k].data.append(list_of_H[0].id)
elif k + 1 in division[1]:
T_a0[k].data.append(list_of_H[1].id)
T_a0[k].data = list(set(T_a0[k].data))
#Create list T_f
T_f0 = []
for r in range(R):
T_f0 = T_f0 + list(list_of_H[r].T_f)
T_f0 = T_f0 + set_v
T_f0 = list(set(T_f0))
# Create the new list of histories (STILL MUST ADAPT TO A GENERAL NUMBER OF ROBOS)
new_Hists = []
for r in range(R):
H = History()
H.id = list_of_H[r].id
H.e_v = set_v
H.e_uv = division[r]
for r2 in range(R):
if r2 != r:
H.e_g = division[r2] + set_g
H.T_a = T_a0
H.T_f = T_f0
#print "\n\n----------------\n Here is T_f0:",T_f0, "\n----------------\n\n"
H.lastMeeting = []
for r2 in range(R):
H.lastMeeting.append(list_of_H[r2].id)
H.Whole_path = Hole_path_list[r]
new_Hists.append(H)
new_cost = myLib.get_cost(original_graph, new_Hists, speeds, search_speeds)
#print "\n\n-------------------\nHere is new_Hists[0].T_f", new_Hists[0].T_f, "\n-------------------\n\n"
#print "\n\n-------------------\nHere is new_Hists[1].T_f", new_Hists[1].T_f, "\n-------------------\n\n"
print 'Here is old cost: ', old_cost
print 'Here is new cost: ', new_cost
# Check if the new plan is better than the previous one
change_plan = False
if new_cost < old_cost:
change_plan = True
# Plot the disconnected graph
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
#pylab.figure(100)
#pylab.figure(id)
"""
pylab.subplot(2, 2, 1)
pylab.axis('equal')
pylab.axis(virtual_graph['w_s'])
pylab.title('MILP result for robot %d' % H0.id)
pylab.subplot(2, 2, 2)
pylab.axis('equal')
pylab.axis(virtual_graph['w_s'])
pylab.title('MILP result for robot %d' % H1.id)
"""
#Initialize the plot of the result of the TA algorithm
for r in range(R):
pylab.subplot(2, R, (r + 1))
pylab.axis('equal')
pylab.axis(virtual_graph['w_s'])
pylab.title('Task assig. robot %d' % list_of_H[r].id)
for e in range(len(PolC[0])):
[fr, to, cx, cy, cost] = myLib.getCoefs(e, PolC)
p0 = [0.01 * k for k in range(101)]
x = []
y = []
for p in p0:
x.append(0)
y.append(0)
for k in range(6): # Compute refference position
x[-1] = x[-1] + cx[6 - k - 1] * p**k
y[-1] = y[-1] + cy[6 - k - 1] * p**k
"""
pylab.subplot(2, 2, 1)
pylab.plot(x, y, 'k--', linewidth=1.0)
pylab.subplot(2, 2, 2)
pylab.plot(x, y, 'k--', linewidth=1.0)
"""
#Plot the the edge in the background
for r in range(R):
pylab.subplot(2, R, (r + 1))
#pylab.plot(x, y, 'k--', linewidth=1.0)
pylab.plot(x, y, 'k-', linewidth=1.0)
"""
if e + 1 in division[0]:
pylab.subplot(2, 2, 1)
pylab.plot(x, y, colors[0], linewidth=3.0)
if e + 1 in division[1]:
pylab.subplot(2, 2, 2)
pylab.plot(x, y, colors[1], linewidth=3.0)
"""
for r in range(R):
if e + 1 in division[r]:
pylab.subplot(2, R, (r + 1))
pylab.plot(x, y, colors[r], linewidth=3.0)
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
"""
# Plotting the heuristic costs
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
for i in range(len(pts)):
x = pts[i][0]
y = pts[i][1]
pylab.subplot(2, 2, 1)
pylab.text(x, y, ("%.2f" % C_check0[i]), fontsize=8.0)
pylab.subplot(2, 2, 2)
pylab.text(x, y, ("%.2f" % C_check1[i]), fontsize=8.0)
# ---------- ---------- ---------- ---------- --------- ---------- ----------
"""
if not change_plan:
print 'New cost is not better ...'
print '---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------'
print '---------- ---------- ---------- APPLYING THE SIMPLE REPLAN ---------- ---------- ----------'
print '---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------'
#OBS: This simple new plan is necessary.
#If the new plan from MILP->MST->CPP is not better ok. However, we need to run CPP again because we have
#division = [[],[]]
division = []
for r in range(R):
division.append([])
for r in range(R):
for k in list_of_H[r].e_uv:
if k in set_uv:
division[r].append(k)
print 'Here is new division (for the simple replan)'
print division
#Simple replan ---------- ---------- ----------
# Call MST for every robot in the communication graph in order to make the graph connected
pylab.close()
pylab.axis(virtual_graph['w_s'])
print '\n ----- Applying MST to the disconnected subgraphs ------'
subgraphs = []
for r in range(R):
subgraphs.append([])
pylab.subplot(2, R, R + (r + 1))
print 'Here is new division (for the simple replan)'
print division
subgraphs[r] = MST.MSTconnect(original_graph, division[r],
(list_of_H[r]).nextNode, colors[r],
True)
for r in range(R):
print 'Subset of edges for robot ' + str(
list_of_H[r].id) + ': (original indexes)\n', subgraphs[r]
print 'Start node for robot ' + str(list_of_H[r].id) + ':', (
list_of_H[r]).nextNode
# ---------- ---------- ---------- ---------- ----------
# Cal CPP for every graph generated by the MST based algorithm
print '\nApplying CPP to the connected subgraphs'
Hole_path_list = []
for r in range(R):
Hole_path_list.append([])
current_node = (list_of_H[r]).nextNode
edges_listOfTuples = myLib.write_listOfTuples(
original_graph, subgraphs[r])
#Hole_path_list[r] = cppsolver.CPP(sorted(edges_listOfTuples), current_node)
Hole_path_list[r] = CPPlib.main_CPP(sorted(edges_listOfTuples),
current_node)
#for r in range(R):
print 'Route for robot ' + str(
list_of_H[r].id), ': ', Hole_path_list[r]
print '\n'
# ---------- ---------- ---------- ---------- ----------
# Create the new list of histories (STILL MUST ADAPT TO A GENERAL NUMBER OF ROBOS)
new_Hists = []
for r in range(R):
H = History()
H.id = list_of_H[r].id
H.e_v = set_v
H.e_uv = division[r]
for r2 in range(R):
if r2 != r:
H.e_g = division[r2] + set_g
H.T_a = T_a0
H.T_f = T_f0
H.lastMeeting = []
for r2 in range(R):
H.lastMeeting.append(list_of_H[r2].id)
H.Whole_path = Hole_path_list[r]
new_Hists.append(H)
# ---------- ---------- ---------- ---------- ----------
# Set the abort_curr_edge flag in te case to robots are in the same edge
for r1 in range(R):
for r2 in range(r1 + 1, R, 1): #This way r1<r2
if (list_of_H[r1].currEdge == list_of_H[r2].currEdge):
new_Hists[r2].abort_curr_edge = True
print '\nabort_curr_edge strategy:\nrob ', new_Hists[
r1].id, ' / rob ', new_Hists[r2].id, 'set flag'
else:
print '\nabort_curr_edge strategy:\nrob ', new_Hists[
r1].id, ' / rob ', new_Hists[r2].id
# Set the abort_curr_edge flag in te case the edge was previously (thus, completely) visited by another robot
#"""
for r1 in range(R):
for r2 in range(R):
if (new_Hists[r1].currEdge in new_Hists[r2].e_v[0:-1]):
new_Hists[r1].abort_curr_edge = True
#"""
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
"""
THE PROBLEM WITH THIS IS THAT THE MINIMUM ID ROBOT WILL KEEP SERACHING THE SPs THAT WERE ALREADY SEARCHED BY THE MAX ID ROBOT
"""
# Set the available flag as False
for r in range(R):
new_Hists[r].available = False
# Save the result of TA and MST in a image
import time
hour = time.strftime("%Hh%Mm%Ss")
date = time.strftime("d%dm%my%Y")
rp = rospkg.RosPack()
fig_name = rp.get_path('distributed')
fig_name = fig_name + '/imagesResults/Results_' + date + '_' + hour + '.png'
pylab.savefig(fig_name)
# ---------- ---------- ---------- ---------- ----------
# Choose if the result of the planning will be platted or not
SHOW_NEW_PLAN = True
#SHOW_NEW_PLAN = False
if SHOW_NEW_PLAN:
pylab.show()
pylab.close()
return change_plan, Hole_path_list, new_Hists
def replanning_heuristics(original_graph, virtual_graph, list_of_H, FILE_2):
#print 'Here is the beguinning of my function'
PolC = original_graph['PolC']
colors_0 = ['b', 'r', 'g', 'y']
R = len(list_of_H)
print '\n'
for r in range(R):
print 'popped_edges of robot ', list_of_H[r].id, ': ', list_of_H[
r].popped_edges
print '\n'
for r in range(R):
print 'LastMeeting robot ', list_of_H[r].id, ': ', list_of_H[
r].lastMeeting
list_of_robs = []
for r in range(R):
list_of_robs.append(list_of_H[r].id)
id = min(list_of_robs) #id of the robot that is computing the replan
speeds = []
search_speeds = []
colors = []
for r in range(R):
speeds.append(list_of_H[r].specs[0])
search_speeds.append(list_of_H[r].specs[1])
colors.append(colors_0[list_of_H[r].id])
# Compute
old_cost, list_cost = myLib.get_cost(original_graph, list_of_H, speeds,
search_speeds)
# Call function to define the sests E_v, E_uv and E_g
set_uv, set_v, set_g, pts, pts_id, e_vT_f_all = define_subsets_new(
list_of_H, virtual_graph)
# ---------- ---------- ---------- ---------- ----------
print '\nHere is set_v (visited edges) in original inedexes:'
print set_v
print 'Here is set_uv (unvisited edges) in original inedexes:'
print set_uv
print 'Here is set_g (assigned edges) in original inedexes:'
print set_g, '\n\n'
# Map the unvisited nodes with new labels
C = virtual_graph['Ccom'] #matrix with the length costs
Cuv = np.matrix(
C
) # Reduced cost matrix with only the unvisited edges (unvisited virtual nodes)
C = virtual_graph['C_sp'] #matrix with the number of search points
Cuv_sp = np.matrix(
C
) # Reduced cost matrix with only the unvisited edges (unvisited virtual nodes)
exclude_list = (np.array(set_v) - 1).tolist() + (np.array(set_g) -
1).tolist()
Cuv = np.delete(Cuv, exclude_list, 0) # exclude lines
Cuv = np.delete(Cuv, exclude_list, 1) # exclude columns
Cuv = Cuv.tolist()
Cuv_sp = np.delete(Cuv_sp, exclude_list, 0) # exclude lines
Cuv_sp = np.delete(Cuv_sp, exclude_list, 1) # exclude columns
Cuv_sp = Cuv_sp.tolist()
C_edge = []
for e in range(len(PolC[0])):
cost = PolC[0][e][4]
cost = cost.tolist()
cost = cost[0]
cost = cost[0]
C_edge.append(cost)
# --------------------------------------------------------------
# Define the depot virtual nodes for the task assignment
depots = []
for r in range(R):
#exec ('depots.append(set_uv.index(H%d.currEdge))' % r)
depots.append(set_uv.index(list_of_H[r].currEdge))
#THE DEPOT POINT MAY NOT BE IN e_uv
#print '\nHere is depot virtual nodes (new indexes):'
#print depots
#print 'Here is depot virtual nodes (original indexes):'
depots_ori = [] #original depot points??
for r in range(R):
depots_ori.append(set_uv[depots[r]])
#print depots_ori, '\n'
#--------------------------------------------------------------
print '\n ----- Task assignment function called -----'
import time as tm
heu_time = tm.time()
sol, division = TAHEU.heuristic_loop(original_graph, speeds, search_speeds,
Cuv, Cuv_sp, pts, pts_id, depots,
FILE_2)
heu_time = tm.time() - heu_time
print '\n- - - - - - - - - - heu_tot_time: ', heu_time
#PASS THE DEPOT POINTS
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
# Sort the lists
for r in range(R):
division[r] = list(set(division[r]))
# Map the nodes back to the original indexation
for r in range(R):
for k in range(len(division[r])):
division[r][k] = set_uv[division[r][k]]
#"""
# Exclude/Remove the used depot virtual nodes (they are edges that were already visited)
for r1 in range(R):
for r2 in range(R):
if list_of_H[r1].currEdge in division[r2]:
# Remove currEdge only if it is not on e_v, because the robot may only be passing through the edge without searching on it
#print 'AAAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAA\n'
if list_of_H[r1].currEdge in e_vT_f_all:
#if list_of_H[r1].currEdge in list_of_H[r2].T_f:
division[r2].pop(division[r2].index(
list_of_H[r1].currEdge))
#print 'BBBBBBBBBBBBBBBBBB\nBBBBBBBBBBBBBBBBBB\n'
#print 'division[r2]:\n', division[r2]
set_v.append(list_of_H[r1].currEdge)
print 'Edge appended: ', list_of_H[r1].currEdge
# Include the used depot virtual nodes in the visited set
#for r in range(R):
# set_v.append(list_of_H[r].currEdge)
#"""
PL = False
if PL == True:
print '\nAssigned edges for the robots:'
for r in range(R):
print 'Subset for robot ' + str(
list_of_H[r].id) + ': ', division[r]
print '\n'
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
# Call MST for every robot in the communication graph in order to make the graph connected
print '\n ----- Applying MST to the disconnected subgraphs ------'
subgraphs = []
for r in range(R):
subgraphs.append([])
pylab.figure(id)
pylab.subplot(2, R, R + (r + 1))
subgraphs[r] = MST.MSTconnect(original_graph, division[r],
(list_of_H[r]).nextNode, colors[r], True)
PL = False
if PL:
for r in range(R):
print 'Subset of edges for robot ' + str(
list_of_H[r].id) + ': (original indexes)\n', subgraphs[r]
print 'Start node for robot ' + str(list_of_H[r].id) + ':', (
list_of_H[r]).nextNode, '\n'
# ---------- ---------- ---------- ---------- ----------
CPP_time = tm.time()
# Cal CPP for every graph generated by the MST based algorithm
print '\n ----- Applying CPP to the connected subgraphs -----'
Hole_path_list = []
for r in range(R):
Hole_path_list.append([])
current_node = (list_of_H[r]).nextNode
if len(subgraphs[r]) > 0:
edges_listOfTuples = myLib.write_listOfTuples(
original_graph, subgraphs[r])
Hole_path_list[r] = CPPlib.main_CPP(sorted(edges_listOfTuples),
current_node)
else:
Hole_path_list[r] = []
PL = False
if PL:
print 'Route for robot ' + str(
list_of_H[r].id), ': ', Hole_path_list[r], '\n'
print '\n'
# ---------- ---------- ---------- ---------- ----------
CPP_time = tm.time() - CPP_time
# Create list T_a
pts_0 = virtual_graph['nodes']
T_a0 = [Intlist()]
for k in range(len(PolC[0]) - 1):
IL = Intlist()
T_a0.append(IL)
for k in range(len(pts_0)):
T_a0[k].data = list(list_of_H[0].T_a[0].data) + list(
list_of_H[1].T_a[1].data)
T_a0[k].data = list(T_a0[k].data)
if k + 1 in set_uv: # if in unvisite list
if k + 1 in division[0]: # if in the assigned to the other one
T_a0[k].data.append(list_of_H[0].id)
elif k + 1 in division[1]:
T_a0[k].data.append(list_of_H[1].id)
T_a0[k].data = list(set(T_a0[k].data))
# Create list T_f
T_f0 = []
for r in range(R):
T_f0 = T_f0 + list(list_of_H[r].T_f)
T_f0 = T_f0 + set_v
T_f0 = list(set(T_f0))
# Create the new list of histories
new_Hists = []
for r in range(R):
H = History()
H.id = list_of_H[r].id
H.e_v = set_v
H.e_uv = division[r]
for r2 in range(R):
if r2 != r:
H.e_g = H.e_g + division[
r2] # + set_g !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
H.T_a = T_a0
H.T_f = T_f0
H.lastMeeting = []
for r2 in range(R):
H.lastMeeting.append(list_of_H[r2].id)
H.Whole_path = Hole_path_list[r]
new_Hists.append(H)
new_cost, list_cost = myLib.get_cost(original_graph, new_Hists, speeds,
search_speeds)
for r in range(len(list_of_H)):
print 'cost ', list_of_H[r].id, ': ', list_cost[r]
#print 'max cost: ', new_cost
#print 'Old cost: ', old_cost
#print 'New cost: ', new_cost
# Check if the new plan is better than the previous one
change_plan = False
if new_cost < old_cost:
change_plan = True
# Initialize the plot of the result of the TA algorithm
for r in range(R):
pylab.subplot(2, R, (r + 1))
pylab.axis('equal')
pylab.axis(virtual_graph['w_s'])
pylab.title('Task assig. robot %d' % list_of_H[r].id)
for e in range(len(PolC[0])):
[fr, to, cx, cy, cost] = myLib.getCoefs(e, PolC)
p0 = [0.01 * k for k in range(101)]
x = []
y = []
for p in p0:
x.append(0)
y.append(0)
for k in range(6): # Compute refference position
x[-1] = x[-1] + cx[6 - k - 1] * p**k
y[-1] = y[-1] + cy[6 - k - 1] * p**k
# Plot the the edge in the background
for r in range(R):
pylab.subplot(2, R, (r + 1))
#pylab.plot(x, y, 'k--', linewidth=1.0)
pylab.plot(x, y, 'k-', linewidth=1.0)
for r in range(R):
if e + 1 in division[r]:
pylab.subplot(2, R, (r + 1))
pylab.plot(x, y, colors[r], linewidth=3.0)
str_2 = "\t" + str(heu_time)
for r in range(len(list_cost)):
str_2 = str_2 + "\t" + str(list_cost[r])
str_2 = str_2 + "\t" + str(CPP_time)
FILE_2.write(str_2)
# If the new plan is not better compute a simple replan
if not change_plan:
# If the new plan from TA->MST->CPP is not better ok. However, we need to run CPP again because we have
print 'New cost is not better ...'
print '---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------'
print '---------- ---------- ---------- APPLYING THE SIMPLE REPLAN ---------- ---------- ----------'
print '---------- ---------- ---------- ---------- ---------- ---------- ---------- ----------'
division = []
for r in range(R):
division.append([])
for r in range(R):
for k in list_of_H[r].e_uv:
if k in set_uv:
division[r].append(k)
print 'Here is new division (for the simple replan)'
print division
# Call MST for every robot in the communication graph in order to make the graph connected
pylab.close()
pylab.axis(virtual_graph['w_s'])
print '\n ----- Applying MST to the disconnected subgraphs ------'
subgraphs = []
for r in range(R):
subgraphs.append([])
pylab.subplot(2, R, R + (r + 1))
subgraphs[r] = MST.MSTconnect(original_graph, division[r],
(list_of_H[r]).nextNode, colors[r],
True)
for r in range(R):
print 'Subset of edges for robot ' + str(
list_of_H[r].id) + ': (original indexes)\n', subgraphs[r]
print 'Start node for robot ' + str(list_of_H[r].id) + ':', (
list_of_H[r]).nextNode, '\n'
# ---------- ---------- ---------- ---------- ----------
# Cal CPP for every graph generated by the MST based algorithm
print '\n ----- Applying CPP to the connected subgraphs -----'
Hole_path_list = []
for r in range(R):
Hole_path_list.append([])
current_node = (list_of_H[r]).nextNode
if len(subgraphs[r]) > 0:
edges_listOfTuples = myLib.write_listOfTuples(
original_graph, subgraphs[r])
Hole_path_list[r] = CPPlib.main_CPP(sorted(edges_listOfTuples),
current_node)
else:
Hole_path_list[r] = []
print 'Route for robot ' + str(
list_of_H[r].id), ': ', Hole_path_list[r], '\n'
print '\n'
# ---------- ---------- ---------- ---------- ----------
# Create the new list of histories (STILL MUST ADAPT TO A GENERAL NUMBER OF ROBOS)
new_Hists = []
for r in range(R):
H = History()
H.id = list_of_H[r].id
H.e_v = set_v
H.e_uv = division[r]
for r2 in range(R):
if r2 != r:
H.e_g = division[r2] + set_g
H.T_a = T_a0
H.T_f = T_f0
H.lastMeeting = []
for r2 in range(R):
H.lastMeeting.append(list_of_H[r2].id)
H.Whole_path = Hole_path_list[r]
new_Hists.append(H)
# ---------- ---------- ---------- ---------- ----------
"""
Nothing is being done with this yet
"""
# Set the abort_curr_edge flag in te case to robots are in the same edge
for r1 in range(R):
for r2 in range(r1 + 1, R, 1): # This way r1<r2
if (list_of_H[r1].currEdge == list_of_H[r2].currEdge):
new_Hists[r2].abort_curr_edge = True
# """
for r1 in range(R):
for r2 in range(R):
if (new_Hists[r1].currEdge in new_Hists[r2].e_v[0:-1]):
new_Hists[r1].abort_curr_edge = True
# """
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
"""
THE PROBLEM WITH THIS IS THAT THE MINIMUM ID ROBOT WILL KEEP SERACHING THE SPs THAT WERE ALREADY SEARCHED BY THE MAX ID ROBOT
"""
# Set the available flag as False
for r in range(R):
new_Hists[r].available = False
# Save the result of TA and MST in a image
import time
hour = time.strftime("%Hh%Mm%Ss")
date = time.strftime("d%dm%my%Y")
rp = rospkg.RosPack()
fig_name = rp.get_path('distributed')
fig_name = fig_name + '/imagesResults/Results_' + date + '_' + hour + '.png'
pylab.savefig(fig_name)
# ---------- ---------- ---------- ---------- ----------
# Choose if the result of the planning will be platted or not
#SHOW_NEW_PLAN = True
SHOW_NEW_PLAN = False
if SHOW_NEW_PLAN:
pylab.show()
pylab.close()
return change_plan, Hole_path_list, new_Hists
def keep_moving(H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal,
new_task, new_path, original_graph, freq, pose, laserVec, d,
Vd, Kp, id, edge, pub_stage):
C = original_graph['C']
PathM = original_graph['PathM']
PolC = original_graph['PolC']
EdgeMap = original_graph['EdgeMap']
VX = 0
WZ = 0
end_flag = False
change_edge = False #Indicated a change on edge - useful to force robots to finish a edge before accept a replan
if time - time_start > T + 1 / freq:
change_edge = True
time_start = time
if len(pathNode) > 1:
new_path = 1 #robot ended a direct path only
else:
new_task = 1 #robot ended a complete path as well
if new_task == 1:
if len(Hole_path) > 1:
i = Hole_path.pop(0)
j = Hole_path[0]
pathNode = getNodePath(i - 1, j - 1, PathM)
new_task = 0
new_path = 1
else:
print '\nNodes search completed\n'
new_path = 1 #In theory,len(Hole_path) <= 1 implies len(e_uv) = 0 !!!!!!!!!!!!!!!!!!!!!!
if new_path == 1:
#if ((len(H['e_uv']) == 0 and not pop_all_edges_flag)):
if ((len(H['e_uv']) == 0 and not H['popped_edges'])):
#if ((len(H['e_uv']) == 0 and not H['popped_edges'])): #Acho que deve ser isso
#pop_all_edges_flag = True
H['popped_edges'] = True
#H['available'] = False
# H['popped_edges'] = True
print '\n\n---------- ---------- ----------\nPOPPING ALL EDGES\n---------- ---------- ----------\n'
for kk in range(len(PolC[0])):
print kk + 1, '/', len(PolC[0])
if not (kk + 1 in H['e_v']) and not (kk + 1 in H['T_f']):
H['e_uv'].append(kk + 1)
print kk + 1, '/', len(PolC[0]), ' included'
else:
print kk + 1, '/', len(PolC[0])
if (len(H['e_uv']) == 0):
print '\n---------- All the edges were already visited ----------\n'
end_flag = True
VX, WZ = 0, 0
#return H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal, new_task, new_path, VX, WZ, end_flag, edge, change_edge, pop_all_edges_flag
return H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal, new_task, new_path, VX, WZ, end_flag, edge, change_edge
H['e_g'] = []
print "Here is H['e_uv']"
print H['e_uv']
print 'Here is pose', pose
curr_node, lixo = get_current_node(original_graph, pose)
curr_node = curr_node + 1
print 'Here is curr_node', curr_node
vel = Twist()
pub_stage.publish(vel)
connected_subgraph = MST.MSTconnect(original_graph, H['e_uv'],
curr_node, 'k', False)
edges_listOfTuples = write_listOfTuples(original_graph,
connected_subgraph)
Hole_path = CPPlib.main_CPP(sorted(edges_listOfTuples), curr_node)
print 'Here is Hole path'
print Hole_path
pathNode = [Hole_path[0], Hole_path[1]]
i = Hole_path.pop(0)
j = Hole_path[0]
pathNode = getNodePath(i - 1, j - 1, PathM)
new_task = 0
new_path = 1
#elif (len(H['e_uv']) == 0 and pop_all_edges_flag):
#elif (len(H['e_uv']) == 0 and H['popped_edges']):
elif (len(H['T_f']) == len(PolC[0])):
print '\n---------- All popped edges were visited ----------\n'
end_flag = True
#return H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal, new_task, new_path, VX, WZ, end_flag, edge, change_edge, pop_all_edges_flag
return H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal, new_task, new_path, VX, WZ, end_flag, edge, change_edge
i = pathNode.pop(0)
j = pathNode[0]
T = C[i - 1][
j -
1] / Vd #This is valid because i and j are always direct neighbors
[edge, signal] = getEdge(i, j, PolC)
[fr, to, cx, cy, cost] = getCoefs(edge, PolC)
if signal == 1:
p = 0 #start the edge (polynomial) at the beginning and move to the end
elif signal == -1:
p = 1 #start the edge (polynomial) at the end and move to the beginning
new_path = 0
# Update the History
edge = EdgeMap[i - 1][j - 1]
# Remove the edge from the list of unvisited nodes
if edge in H['e_uv']:
#print "\nXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX - "+str(edge)+"\n"
H['e_uv'].pop(H['e_uv'].index(edge))
# Add the edge to the list of visited edges
if not edge in H['e_v']:
H['e_v'].append(edge)
#Write in file that the edge was searched for posterior analysis
rp = rospkg.RosPack()
path = rp.get_path('distributed')
path = path + '/text/visited_' + str(id) + '.txt'
FILE = open(path, 'a')
FILE.write(str(edge) + '\n')
FILE.close()
if not edge in H['T_f']:
H['T_f'].append(edge)
#Print information on screen
print '\nRobot ' + str(id)
print 'e_v:\n', H['e_v']
print 'e_uv:\n', H['e_uv']
print 'e_g:\n', H['e_g']
print 'T_f:\n', H['T_f']
print 'Whole_path:\n', Hole_path
else:
# Compute the velocity according to the polynomial edge
[ux, uy, p] = compute_velocity(cx, cy, p, 1.0 / freq, signal, pose, Vd,
Kp)
# Apply th repulsive potential to avoid collision
[ux, uy] = repulsive_potential(laserVec, pose, ux, uy)
# Apply feedback linearization
[VX, WZ] = feedback_linearization(ux, uy, pose, d)
#return H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal, new_task, new_path, VX, WZ, end_flag, edge, change_edge, pop_all_edges_flag
return H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal, new_task, new_path, VX, WZ, end_flag, edge, change_edge
