def main():
graph_builder = GraphBuilder("SCC.txt")
graph = graph_builder.build_graph()
start_node = graph.get_node("1")
dfs = DFS(graph, start_node)
nodes = dfs.run()
for node in nodes:
print(node.id)
def eventLoop(self, screen):
for event in pg.event.get():
if event.type == pg.QUIT:
pg.quit()
sys.exit()
if event.type == pg.MOUSEWHEEL:
self.setZoom(event.y)
self.clearScreen(screen)
if event.type == pg.MOUSEBUTTONDOWN:
mouse_pos = event.pos # gets mouse position
for btn in self.buttons:
if btn.wasPressed(event):
if btn.text.lower() == "dfs":
graph = self.objects[0]
dfs = DFS(graph)
dfs.run(screen)
if btn.text.lower() == "bfs":
graph = self.objects[0]
dfs = BFS(graph)
dfs.run(screen)
def run(self):
self.listenConfiguration()
algorithm = self.algorithmType.get()
if (algorithm == "A*"):
AStarAlgorithm = AStar(self.map,self.startGrid,self.targetGrid,self.isPathVisible,self.screen)
return AStarAlgorithm.run()
elif (algorithm == "BFS"):
BFSAlgorithm = BFS(self.map,self.startGrid,self.targetGrid,self.isPathVisible,self.screen)
return BFSAlgorithm.run()
elif (algorithm == "DFS"):
DFSAlgorithm = DFS(self.map, self.startGrid, self.targetGrid, self.isPathVisible,self.screen)
return DFSAlgorithm.run()
elif (algorithm == "Dijkstra"):
DijkstraAlgorithm = Dijkstra(self.map, self.startGrid, self.targetGrid, self.isPathVisible,self.screen)
return DijkstraAlgorithm.run()
else:
return
def allocate(self, robots, tasks):
'''
Allocate tasks to robots
'''
rospy.loginfo("Searching for best robots allocation!")
# print("\nROBOTS\n{}\n".format(robots))
# print("\nTASKS:\n{}\n\n\n".format(tasks))
# Sort tasks by priority
tasks.sort_values(by=['priority'], ascending=True, inplace=True)
# Build the Tree with all possible options
search = DFS(robots,tasks)
# Receive a node containing allocated tasks
tasks_node = search.run()
robots_info_me.acquire()
###### Send TASKS to ROBOTS ############################################################
if tasks_node:
rospy.loginfo("\n\nSelected tasks: {}\n".format(tasks_node.getValue()))
for t in tasks_node.getValue():
task_pub = rospy.Publisher("/{}/task".format(t[1]),task_message, queue_size=10)
if t[0] != robots_info.loc[t[1],'current_task_id']:
# Build the message for task requisition
task_msg = task_message()
task_msg.id = t[0]
if tasks.loc[t[0],'maneuver'] == 'search':
if 'pioneer3at' in t[1]:
task_msg.task = 'exploration'
elif 'UAV' in t[1]:
task_msg.task = 'victim_search'
else:
task_msg.task = tasks.loc[t[0],'maneuver']
task_msg.gas_sensor = tasks.loc[t[0],'gs']
task_msg.victim_sensor = tasks.loc[t[0],'vs']
# Define task position or region
if tasks.loc[t[0],'maneuver'] == 'approach':
pos = Twist()
pos.linear.x = tasks.loc[t[0],'position']['x']
pos.linear.y = tasks.loc[t[0],'position']['y']
pos.linear.z = tasks.loc[t[0],'position']['z']
pos.angular.z = tasks.loc[t[0],'position']['theta']
task_msg.position.append(pos)
elif tasks.loc[t[0],'maneuver'] in ['assessment', 'search']:
pos = Twist()
pos.linear.x = tasks.loc[t[0],'region']['x0']
pos.linear.y = tasks.loc[t[0],'region']['y0']
# pos.linear.z = tasks.loc[t[0],'region']['z0']
task_msg.position.append(pos)
pos = Twist()
pos.linear.x = tasks.loc[t[0],'region']['x1']
pos.linear.y = tasks.loc[t[0],'region']['y1']
# pos.linear.z = tasks.loc[t[0],'region']['z1']
task_msg.position.append(pos)
# Wait till it recognizes the subscribers
r = rospy.Rate(20)
while(task_pub.get_num_connections()<1):
r.sleep()
# Publish the task
task_pub.publish(task_msg)
# task_pub.unregister()
#Update robot info
robots_info.loc[t[1],'last_task_id'] = robots_info.loc[t[1],'current_task_id']
robots_info.loc[t[1],'current_task_id'] = t[0]
########################################################################################
## Abort tasks of unallocated robots
for r in robots_info.index:
if (not tasks_node) or (not [t for t in tasks_node.getValue() if t[1] == r]) and (robots_info.loc[r,'current_task_id']):
# Send empty message
if robots_info.loc[r,'status'] != 'unable':
task_pub = rospy.Publisher("/{}/task".format(r),task_message, queue_size=10)
task_msg = task_message()
task_pub.publish(task_msg)
#Update robot info
robots_info.loc[r,'last_task_id'] = robots_info.loc[r,'current_task_id']
robots_info.loc[r,'current_task_id'] = None
robots_info_me.release()
return tasks_node
