def replan_thread(original_graph, virtual_graph, list_of_H,pub_broadcast,id):
# Call the replanning function (Algorithm 2)
change_plan, Hole_path_list, new_Hists = replanning(original_graph, virtual_graph, list_of_H)
pylab.close("all")
# Broadcast new plan to other robots
print '\nCreating Broadcast message ...\n'
B = Broadcast()
B.sender = id
R = len(new_Hists)
B.destinations = []
for r in range(R):
B.destinations.append(new_Hists[r].id)
for r in range(R):
IL = Intlist()
IL.data = list(Hole_path_list[r])
B.new_Hole_paths.append(IL)
B.listOfH = new_Hists
print 'New plan broadcasted'
waitting_new_plan = True
pub_broadcast.publish(B)
def Algorithm_1():
global pose
global pub_rviz, pub_targ, pub_pose, pub_hist
global freq
global time, i
global file_vel
global N
global n, nodes, C, PathM, w_s, PolC
global Vd
global laserVec
global id
global replan_tasks, finish_edge
global H, H_new, Hole_path_new
global list_of_H, list_of_robs, list_of_vels
global Hole_path
global waitting_new_plan
global new_task
global original_graph
global SP, SP_fix
global new_plan_flag
global vel, pub_stage, pub_broadcast, change_plan, Hole_path_list, new_Hists, original_graph, virtual_graph, list_of_H, list_of_robs
global edge, pathNode
new_plan_flag = False
global abort_curr_edge
abort_curr_edge = False
vel = Twist()
T_f_msg = ForbEdges()
#print 'T_f_msg:\n', T_f_msg
T_f_last = []
global count
count = 0
#Define the node, publisher and subscribers
my_str = '/robot_' + str(id) + '/cmd_vel'
pub_stage = rospy.Publisher(
my_str, Twist, queue_size=1) #Publish command velocities to stage
my_str = '/robot_' + str(id) + '/history'
pub_hist = rospy.Publisher(
my_str, History, queue_size=1
) #Publish History to the centralied communication sensor simulator
my_str = 'follow_graph_' + str(id)
rospy.init_node(my_str) #Initialize the node
my_str = '/robot_' + str(id) + '/base_pose_ground_truth'
rospy.Subscriber(my_str, Odometry,
callback_pose) #Subscribe to obtain robot's position
my_str = '/robot_' + str(id) + '/base_scan'
rospy.Subscriber(
my_str, LaserScan,
callback_laser) #Subscribe to obtain robot's lasr scanner data
rospy.Subscriber(
"/comm_graph", HistList, callback_comm_graph
) #Subscribe to eventually receive communication graph event (other robots close)
pub_forb = rospy.Publisher(
'/forb_edges', ForbEdges, queue_size=10
) # Publish the visited edges to be shared always as possible
rospy.Subscriber("/forb_edges", ForbEdges, callback_ForbEdges)
#Read and Write in the new plan topic
pub_broadcast = rospy.Publisher(
'/new_path_topic', Broadcast,
queue_size=4) #Eventually publish new routes to other robots
rospy.Subscriber('/new_path_topic', Broadcast,
callback_new_plan) #Eventually receive a new route
#Publish the searched point in order to ???
pub_SP = rospy.Publisher('/searched_point', Intlist, queue_size=4)
freq = 15.0 # Frequency of operation in Hz
rate = rospy.Rate(freq)
# Wait initialization
while (detected_pose == 0 and not rospy.is_shutdown()):
rate.sleep()
# Run CPP in orer to obtain an initial route ---------- ---------- ----------
[current_node, dist] = myLib.get_current_node(original_graph,
[pose[0], pose[1]])
current_node = current_node + 1
active_edges = range(1, len(PolC[0]) + 1)
edges_listOfTuples = myLib.write_listOfTuples(original_graph, active_edges)
print "Computting first CPP ..."
elapsed = tm.time()
Hole_path = CPPlib.main_CPP(edges_listOfTuples, current_node)
elapsed = tm.time() - elapsed
print 'Elapsed time for CPP: ', elapsed
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
#print 'Hole_path: ', Hole_path
#Uncomment this section in order to have a deterministic starting route
"""
if(id == 0):
Hole_path = [1, 2, 3, 4, 5, 6, 1, 2, 14, 13, 12, 10, 11, 9, 8, 7, 24, 25, 26, 27, 3]
elif (id == 1):
Hole_path = [30, 28, 24, 23, 22, 31, 20, 7, 1, 30, 28, 24, 23, 22, 21, 32, 17, 3]
elif (id == 2):
Hole_path = [33, 15, 16, 29, 21, 22, 31, 11]
elif (id == 3):
Hole_path = [5, 4, 3, 2, 1,14, 15]
"""
"""
if(id == 0):
#Hole_path = [2, 3] + Hole_path
Hole_path = [] + Hole_path
if (id == 1):
Hole_path = [18, 17, 16] + Hole_path
if (id == 2):
Hole_path = [15, 16] + Hole_path
"""
"""
if(id == 0):
Hole_path = [15, 14, 2, 1, 2, 3, 4, 5, 4, 6, 4, 3, 7, 8, 72, 8, 9, 71, 9, 10, 11, 10, 12, 70, 12, 13, 14, 13,
18, 19, 18, 17, 16, 17, 20, 68, 20, 21, 22, 67, 22, 7, 23, 24, 25, 24, 26, 24, 23, 27, 28, 29, 30, 63, 30, 31, 32,
33, 32, 34, 32, 31, 35, 36, 62, 36, 37, 61, 37, 38, 39, 38, 40, 60, 40, 41, 29, 41, 42, 59, 42, 43, 44, 58, 44, 45,
46, 57, 46, 35, 47, 48, 49, 48, 50, 48, 47, 51, 56, 51, 52, 45, 52, 53, 43, 53, 54, 55, 54, 28, 27, 64, 66, 64, 65,
21, 65, 16, 15, 69]
if (id == 1):
Hole_path = [15, 14, 2, 1, 2, 3, 4, 5, 4, 6, 4, 3, 7, 8, 72, 8, 9, 71, 9, 10, 11, 10, 12, 70, 12, 13, 14, 13,
18, 19, 18, 17, 16, 17, 20, 68, 20, 21, 22, 67, 22, 7, 23, 24, 25, 24, 26, 24, 23, 27, 28, 29, 30, 63, 30, 31, 32,
33, 32, 34, 32, 31, 35, 36, 62, 36, 37, 61, 37, 38, 39, 38, 40, 60, 40, 41, 29, 41, 42, 59, 42, 43, 44, 58, 44, 45,
46, 57, 46, 35, 47, 48, 49, 48, 50, 48, 47, 51, 56, 51, 52, 45, 52, 53, 43, 53, 54, 55, 54, 28, 27, 64, 66, 64, 65,
21, 65, 16, 15, 69]
if (id == 2):
Hole_path = [18, 19, 18, 17, 16, 17, 20, 68, 20, 21, 22, 67, 22, 7, 23, 24, 25, 24, 26, 24, 23, 27, 28, 29, 30,
63, 30, 31, 32, 33, 32, 34, 32, 31, 35, 36, 62, 36, 37, 61, 37, 38, 39, 38, 40, 60, 40, 41, 29, 41, 42, 59, 42, 43, 44, 58, 44, 45,
46, 57, 46, 35, 47, 48, 49, 48, 50, 48, 47, 51, 56, 51, 52, 45, 52, 53, 43, 53, 54, 55, 54, 28, 27, 64, 66, 64, 65,
21, 65, 16, 15, 69, 15, 14, 2, 1, 2, 3, 4, 5, 4, 6, 4, 3, 7, 8, 72, 8, 9, 71, 9, 10, 11, 10, 12, 70, 12, 13, 14,
13]
"""
# Publish the History to the centralized communication simulator in order to keep it actualized
Hmsg = History()
Hmsg.id, Hmsg.specs, Hmsg.e_v, Hmsg.e_uv, Hmsg.e_g, Hmsg.T_a, Hmsg.T_f, Hmsg.currEdge, Hmsg.nextNode, Hmsg.pose, Hmsg.lastMeeting, Hmsg.abort_curr_edge, Hmsg.available, Hmsg.popped_edges = id, [
Vd, Vs
], H['e_v'], H['e_uv'], H['e_g'], H['T_a'], H['T_f'], EdgeMap[
Hole_path[0] - 1][Hole_path[1] -
1], Hole_path[0], pose, [], False, True, False
Hmsg.Whole_path = Hole_path
pub_hist.publish(Hmsg)
#Atributting initial values to some variables
time_start = 0
T = -2 #"initial flag value"
pathNode, cx, cy, p, signal, new_task, new_path = [],[],[],[],[],[],[]
edge = -1
#waitting_new_plan = False
finish_edge = False
#pop_all_edges_flag = False
#H['popped_edges'] = False
global stop_the_robot
stop_the_robot = False
#Main loop
while not rospy.is_shutdown():
#if not stop_the_robot:
count = count + 1 #counter
#print 'count: ', count
time = count / float(freq) #variable that counts time
# If there is no need to do replanning, just keep moving through the current edge
if not replan_tasks:
#[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] = myLib.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, pop_all_edges_flag, pub_stage)
[
H, time, time_start, T, pathNode, Hole_path, cx, cy, p, signal,
new_task, new_path, VX, WZ, end_flag, edge, change_edge
] = myLib.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)
# If it is necessary to do replanning ...
else:
# First, just keep moving through the current edge only to finish it
if (not finish_edge):
#[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] = myLib.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, pop_all_edges_flag, pub_stage)
[
H, time, time_start, T, pathNode, Hole_path, cx, cy, p,
signal, new_task, new_path, VX, WZ, end_flag, edge,
change_edge
] = myLib.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)
if change_edge:
finish_edge = True
print '\nEdge finished\n\nWaitting new plan broadcast\n'
VX, WZ = 0, 0
vel.linear.x, vel.angular.z = VX, WZ
pub_stage.publish(vel)
else:
VX, WZ = 0, 0
vel.linear.x, vel.angular.z = 0, 0
pub_stage.publish(vel)
if end_flag:
# Terminate the execution
vel.linear.x, vel.angular.z = 0, 0
pub_stage.publish(vel)
break
# --------------------------------------------------------------------------------
Threshold = 0.20 # if the distance of robot is less than threshold
[SP, closeFlag, SP_id,
SP_edge] = myLib.CheckOnSP(pose[0:2], SP, SP_fix, Threshold)
if (len(H['e_v']) > 0 and not stop_the_robot):
#if (closeFlag and SP_edge == (H['e_v'])[-1] and not abort_curr_edge):
if (closeFlag and SP_edge == (H['e_v'])[-1]
and not abort_curr_edge):
print 'Robot searching in a search point', str(SP_id), '...'
print 'SP_edge = ', SP_edge
cnt = 0
while (
cnt < 2 * pi
): # robot rotates and search based on its Searching Speed
# Publish the computed speed to stage
rate.sleep()
vel.linear.x, vel.angular.z = 0, Vs
pub_stage.publish(vel)
cnt = cnt + (1.0 / freq) * Vs
IL = Intlist()
IL.data = [SP_id, id]
pub_SP.publish(IL)
#Publish the computed speed to stage
if not stop_the_robot:
vel.linear.x, vel.angular.z = VX, WZ
pub_stage.publish(vel)
else:
while stop_the_robot:
rate.sleep()
#Write the computed velocities to a txt file
if not file_vel.closed:
mystr = str(VX) + "\t" + str(WZ) + "\t" + "\t" + str(time) + "\n"
file_vel.write(mystr)
#Publish the History to the centralized communication simulator in order to keep it actualized
Hmsg = History()
Hmsg.id, Hmsg.specs, Hmsg.e_v, Hmsg.e_uv, Hmsg.e_g, Hmsg.T_a, Hmsg.T_f, Hmsg.currEdge, Hmsg.nextNode, Hmsg.pose, Hmsg.lastMeeting, Hmsg.abort_curr_edge = id, [
Vd, Vs
], H['e_v'], H['e_uv'], H['e_g'], H['T_a'], H['T_f'], edge, pathNode[
0], pose, H['lastMeeting'], False
Hmsg.Whole_path = Hole_path
Hmsg.available = H['available']
Hmsg.popped_edges = H['popped_edges']
pub_hist.publish(Hmsg)
#Waitting a replan AND new plan received AND current edge finished
if (replan_tasks and new_plan_flag and finish_edge):
finish_edge = False
new_plan_flag = False
#H = copy.deepcopy(H_new)
H['e_v'] = copy.deepcopy(H_new['e_v'])
#H['e_v'] = list(set(H['e_v']+H_new['e_v'])) #!!!!!!!!!!!!!!!!!!!!!!!!!
H['e_uv'] = copy.deepcopy(H_new['e_uv'])
H['e_g'] = copy.deepcopy(H_new['e_g'])
H['T_a'] = copy.deepcopy(H_new['T_a'])
#H['T_f'] = copy.deepcopy(H_new['T_f'])
#H['lastMeeting'] = copy.deepcopy(H_new['lastMeeting']) #!!!!!!!
Hole_path = copy.deepcopy(Hole_path_new)
replan_tasks = False
new_task = 1
abort_curr_edge = False
H['available'] = True
print '\n\nNEW PLAN UPDATED\n\n'
if H['T_f'] != T_f_last:
T_f_last = copy.deepcopy(H['T_f'])
T_f_msg.sender = id
T_f_msg.destinations = [-1]
T_f_msg.forbiden_Edges = H['T_f']
pub_forb.publish(T_f_msg)
# Wait
rate.sleep()
def callback_comm_graph(data):
# This function recieves messages from the centralized sensor simulator
global list_of_H, list_of_robs, list_of_vels, H
global replan_tasks, finish_edge, id
global vel, pub_stage, pub_broadcast, change_plan, Hole_path_list, new_Hists, original_graph, virtual_graph, list_of_H, list_of_robs
global count
global stop_the_robot
global waitting_new_plan
global new_plan_flag
global pub_hist
global edge, pathNode
R = len(data.listOfH)
ids = []
for r in range(R):
ids.append(data.listOfH[r].id)
#If this robot is involved in the communication graph
if id in ids:
# Uptate lastMeeting from now
list_of_robs = data.robList
H['lastMeeting'] = list(list_of_robs)
#Publish the History to the centralized communication simulator in order to keep it actualized
Hmsg = History()
Hmsg.id, Hmsg.specs, Hmsg.e_v, Hmsg.e_uv, Hmsg.e_g, Hmsg.T_a, Hmsg.T_f, Hmsg.currEdge, Hmsg.nextNode, Hmsg.pose, Hmsg.lastMeeting, Hmsg.abort_curr_edge = id, [
Vd, Vs
], H['e_v'], H['e_uv'], H['e_g'], H['T_a'], H['T_f'], edge, pathNode[
0], pose, H['lastMeeting'], False
Hmsg.Whole_path = Hole_path
H['available'] = False
Hmsg.available = H['available']
Hmsg.popped_edges = H['popped_edges']
pub_hist.publish(Hmsg)
# If some positive change (which adds information) happend in the communication graph
if data.comGraphEvent == True:
replan_tasks = True
finish_edge = False
list_of_H = data.listOfH
print '\n ---------- Communication graph event received ----------\n'
print 'Finishing Edge ...'
stop_the_robot = False
# Compute the new routes if the current robot has munimum index
if (id == min(list_of_robs)):
# Stop the robot
VX, WZ = 0, 0
vel.linear.x, vel.angular.z = VX, WZ
pub_stage.publish(vel)
#Flag to stop the robot while it is computting a replan
stop_the_robot = True
total_time = tm.time()
# Call the replanning function (Algorithm 2) ---------- ---------- ----------
#Old replanning method
#change_plan, Hole_path_list, new_Hists = Alg2.replanning(original_graph, virtual_graph, list_of_H)
FILE_2 = open('delete_me.txt', 'w')
# New (better) replanning method
change_plan, Hole_path_list, new_Hists = Alg2.replanning_heuristics(
original_graph, virtual_graph, list_of_H, FILE_2)
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
print 'Tital time of replanning: ', tm.time() - total_time
#Broadcast new plan to other robots
print '\nCreating Broadcast message ...\n'
B = Broadcast()
B.sender = id
R = len(new_Hists)
B.destinations = []
for r in range(R):
B.destinations.append(new_Hists[r].id)
for r in range(R):
IL = Intlist()
IL.data = list(Hole_path_list[r])
B.new_Hole_paths.append(IL)
B.listOfH = new_Hists
pub_broadcast.publish(B)
print 'New plan broadcasted'
stop_the_robot = False
# -----------------------------------------------
return
#Read the original graph
original_graph = myLib.read_graph("Original_graph_" + EXP_NAME + ".mat")
n = original_graph['n']
nodes = original_graph['nodes']
C = original_graph['C']
PathM = original_graph['PathM']
w_s = original_graph['w_s']
PolC = original_graph['PolC']
EdgeMap = original_graph['EdgeMap']
#Initialization of the history of each robot
e_v = [] # visited
e_uv = range(1, len(PolC[0]) + 1) # unvisited (must be visited)
e_g = [] # assigned to other robots
IL = Intlist()
T_a = [IL for i in range(len(PolC[0]))
] # list of list of robots assigned to an edge
T_f = [] # list of list of robots forbidden to visit an edge
lastMeeting = [
] #list of who was in the communicatio graph in the last meeting
H = {
'id': id,
'specs': [Vd, Vs],
'e_v': e_v,
'e_uv': e_uv,
'e_g': e_g,
'T_a': T_a,
'T_f': T_f,
'lastMeeting': lastMeeting,
'Whole_path': [],
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 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 run_expriment():
global poses
Whole_path_0 = [
11, 9, 10, 9, 8, 7, 72, 7, 8, 12, 11, 12, 13, 45, 13, 14, 15, 29, 30,
31, 30, 32, 30, 29, 28, 42, 43, 42, 18, 17, 38, 20, 38, 37, 39, 37, 36,
22, 36, 2, 1, 2, 3, 4, 3, 28, 27, 33, 27, 26, 34, 26, 24, 25, 24, 23,
35, 23, 22, 21, 40, 21, 20, 19, 41, 19, 18, 17, 16, 44, 16, 15, 14, 5,
4, 73, 4, 5, 6, 7, 6, 61, 60, 66, 60, 59, 67, 59, 57, 58, 57, 56, 68,
69, 68, 70, 68, 56, 55, 63, 62, 61, 62, 65, 62, 63, 64, 63, 55, 51, 52,
53, 52, 54, 52, 51, 47, 48, 49, 48, 50, 48, 47, 46, 71, 46
]
R = 4
#path = path_0 + "/experiments/timming/" + "exp_A2_" + str(R) + "_robs.txt"
#path = path_0 + "/experiments/timming/" + "exp_A2_x" + str(R) + "_robs.txt"
#path = path_0 + "/experiments/heterogeneity/" + "exp_heterogeneity_A2_" + str(R) + "_robs.txt"
#path = path_0 + "/experiments/heterogeneity/" + "exp_heterogeneity_A2_x" + str(R) + "_robs.txt"
FILE_2 = open(path, 'w')
"""
Change inside the TA_Heuristcs.py script the lines:
Power_robot_len = speeds
Power_robot_sp = search_speeds
"""
for bat in range(1, 21, 1):
list_of_H = []
print 'list_of_H:\n', list_of_H
for r in range(R):
H0 = History()
IL = Intlist()
H0.id = r #int16
H0.specs = [Vd[r], Vs[r]] #float32[]
H0.e_v = [] #int16[]
H0.e_uv = range(1, len(PolC[0]) + 1) #int16[]
H0.e_g = [] #int16[]
H0.T_a = [IL for i in range(len(PolC[0]))] #Intlist[]
H0.T_f = [] #int16[]
[current_node,
dist] = myLib.get_current_node(original_graph,
poses[R - 1][bat - 1][r])
current_node = current_node + 1
print 'current_node = ', current_node
H0.currEdge = current_node #int16
Whole_path = Whole_path_0
for k in range(len(Whole_path_0)):
if Whole_path[0] == current_node:
break
else:
Whole_path = Whole_path[1:] + Whole_path[:1]
Whole_path.append(Whole_path[0])
print 'Whole_path: ', Whole_path
H0.nextNode = Whole_path[1] #int16
H0.pose = poses[R - 1][bat - 1][r] #?? #float32[]
H0.lastMeeting = [] #int16[]
H0.Whole_path = Whole_path #int16[]
H0.abort_curr_edge = False #bool
H0.available = True #bool
H0.popped_edges = False #bool
#list_of_H.listOfH.append(H0)
list_of_H.append(H0)
#print 'list_of_H:\n', list_of_H
# New (better) replanning method
tot_time = tm.time()
change_plan, Hole_path_list, new_Hists = Alg2.replanning_heuristics(
original_graph, virtual_graph, list_of_H, FILE_2)
tot_time = tm.time() - tot_time
print '\ntot_time: ', tot_time
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
FILE_2.write("\t" + str(tot_time) + "\t\n")
FILE_2.close()
"""
loop_time
atri_time
heu
cost0
cost1
tot_tot_time
"""
#Main loop
while not rospy.is_shutdown():
# Create list of History
# New (better) replanning method
# change_plan, Hole_path_list, new_Hists = Alg2.replanning_heuristics(original_graph, virtual_graph, list_of_H)
# ---------- ---------- ---------- ---------- ---------- ---------- ----------
break
# Wait
rate.sleep()