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