def get_compabitible_task_for_agent(self, agent, queue):
compatible_task_list = TaskQueue()
for task in self.tasks.get_queue():
if task.is_agent_compatible(agent):
compatible_task_list.enqueue(task)
if compatible_task_list.is_empty():
print "No compatible tasks found for agent %s" %agent.name
return None
return compatible_task_list
class Agent():
def __init__(self,
name,
pose=Pose(),
size=(1.0, 1.0, 1.0),
capabilities=[],
movement_speed=0.1):
# Tuple contain the x,y,z coordinates of the agent
self.pose = pose # Rospy pose data structure, used in Unity to determine space
self.name = name # Name of this particular agent, needs to be unique
self.size = size # the x,y,z size of the agent
self.task_queue = TaskQueue()
self.capabilities = capabilities
self.status = ""
self.current_action = "idle"
self.movement_speed = movement_speed
self.is_at_task = False
self.ros_controller = ROSController()
def get_pose(self):
return self.pose
## TODO, fill in this with a custom message.
def get_message(self):
pass
def add_task(self, task):
self.task_queue.enqueue(task)
def get_task(self):
return self.task_queue.peek()
def set_current_task(self, task):
self.is_at_task = False
self.task_queue.prioritise(task)
def update_status(self):
if self.task_queue.is_empty():
self.status = "%s is idle." % self.name
else:
task_name = self.task_queue.peek().name
self.status = "%s is %s task[ %s ] currently." % (
self.name, self.current_action, task_name)
def get_status(self):
self.update_status()
return self.status
def go_to_current_task(self):
if self.pose.position == self.task_queue.peek().pose.position:
print("Agent %s is currently at the it's current task!" %
self.name)
self.is_at_task = True
return True
self.current_action = "moving to task"
self.move_towards_target(self.task_queue.peek().pose.position)
return False
def move_towards_target(self, target_pos):
target_diff_x = target_pos.x - self.pose.position.x
target_diff_y = target_pos.y - self.pose.position.y
target_diff_z = target_pos.z - self.pose.position.z
self.pose.position.x += self.get_move_vector(target_diff_x)
self.pose.position.y += self.get_move_vector(target_diff_y)
self.pose.position.z += self.get_move_vector(target_diff_z)
direction_y = math.atan2(self.get_move_vector(target_diff_y),
self.get_move_vector(target_diff_x))
quat = quaternion_from_euler(0, direction_y, 0)
self.pose.orientation.x = quat[0]
self.pose.orientation.y = quat[1]
self.pose.orientation.z = quat[2]
self.pose.orientation.w = quat[3]
def get_move_vector(self, target_x):
if abs(target_x) > self.movement_speed:
if target_x < 0:
return -self.movement_speed
else:
return self.movement_speed
return target_x
def complete_current_task(self):
if self.task_queue.peek().is_completed():
print("Current task %s being done by %s has been completed." %
(self.task_queue.peek().name, self.name))
self.ros_controller.publish_string_update_message(
"%s has been completed by %s" %
(self.task_queue.peek().name, self.name))
self.task_queue.dequeue()
self.is_at_task = False
self.current_action = "completed task"
return True
print "Current task %s has not yet been completed by agent %s." % (
self.task_queue.peek().name, self.name)
return False
# OVERRIDE THIS FUNCTION WITH SPECIFIC AGENT FUNCTION
def do_work(self):
pass
class GameWorld():
def __init__(self, size=(100, 100), agents=[], tasks=TaskQueue(), AI_ON=True):
self.size = size
self.ai_on = False;
self.agents = agents
self.tasks = tasks
self.map = np.ones(size)
self.user_priority_queue = TaskQueue()
self.world_task_queue = TaskQueue()
self.generate_agents()
self.generate_world_tasks()
if(self.ai_on):
self.assign_tasks()
self.ros_controller = ROSController()
self.restart = False
#self.agents[0].task_queue.prioritise(self.world_task_queue.get_queue()[0])
def update(self):
# Prints the tasks in each of the agent queues for debugging reasons.
#for agent in self.agents:
# print ("Agent: %s has the tasks: %s" % (agent.name, ",".join([t.name for t in agent.task_queue.get_queue()])))
if not self.world_task_queue.is_empty():
#print("Publishing tasks")
self.ros_controller.publish_tasks(self.world_task_queue.get_queue())
#print("Publishing Agents")
self.ros_controller.publish_agents(self.agents)
## This checks if the game is meant to run yet
if self.ros_controller.game_start:
if(self.ai_on):
timer = Timer()
self.check_scenario_complete()
for agent in self.agents:
agent.do_work()
if self.ros_controller.recieved_assign_message:
#print("I SHOULDN'T HAVE A ASSIGN MESSAGE YET: %s" % self.ros_controller.recieved_assign_message)
self.ros_controller.recieved_assign_message = False
assigned_agent = self.find_agent_in_list(self.ros_controller.user_assign_task[0])
assigned_task = self.find_task_in_queue(self.ros_controller.user_assign_task[1])
assigned_agent.add_task(assigned_task)
print("Assigned agent %s to do task %s" % (assigned_agent.name, assigned_task.name))
if self.ros_controller.recieved_move_message:
self.ros_controller.recieved_move_message = False
assigned_agent = self.find_agent_in_list(self.ros_controller.user_assign_move[0])
pose = self.pose_constructor(float(self.ros_controller.user_assign_move[1][0]),
float(self.ros_controller.user_assign_move[1][1]),0,0,0,0,1)
assigned_agent.set_current_task(MoveTask("TempMoveTask", "Agent needs to move to this spot", end_pose=pose, agent=assigned_agent))
print("Assigned agent %s to move to %s" %(assigned_agent.name, pose))
# DEBUG STUFF!
# print("!!!!!!CURRENT LOCATION OF AGENTS AND THEIR TARGETS!!!!!!")
# for agent in self.agents:
#
# print("AGENT: %s - Position:\n %s \n TASK: %s - Position:\n %s" % (agent.name,
# agent.pose.position,
# agent.task_queue.peek().name,
# agent.task_queue.peek().pose.position))
# print("---------------------------------------------------------")
def find_task_in_queue(self, task_name):
for task in self.world_task_queue.get_queue():
if task.name == task_name:
return task
print("Could not find the task..")
return None
def find_agent_in_list(self, agent_name):
for agent in self.agents:
print("ListAgent: %s == InputAgent: %s = %s" % (agent.name, agent_name, agent.name == agent_name) )
if agent.name == agent_name:
return agent
print [ agent.name for agent in self.agents]
print "Could not find the agent!"
return None
def check_scenario_complete(self):
for t in self.world_task_queue.get_queue():
if not t.is_complete:
return False
timer = Timer()
while not self.ros_controller.got_end_confirmation:
message = String()
message.data = "finished"
self.ros_controller.publish_update_message(message)
message.data = "end time|%s" %timer.get_elasped_time()
self.ros_controller.publish_update_message(message)
time.sleep(0.05)
print("All assigned tasks have been completed!")
self.restart = True
def add_agent(self, agent):
self.agents.append(agent)
def add_task(self, task):
self.tasks.enqueue(task)
self.world_task_queue.enqueue(task)
def get_nearest_tasks(self, agent):
nearest_task = None
# Finds the furthest possible distance between two points of a rectangluar grid.
nearest_dist = math.sqrt(math.pow(self.size[0], 2) + math.pow(self.size[1], 2))
for task in agent.task_queue.get_queue():
if not task.assigned_agent:
dist = math.sqrt(math.pow(task.pose.position.x - agent.pose.position.x, 2)
+ math.pow(task.pose.position.y - agent.pose.position.y, 2)
+ math.pow(task.pose.position.z - agent.pose.position.z, 2))
if dist < nearest_dist:
nearest_task = task
# Ensures no "current task" is assigned to multiple agents,
#self.tasks.remove(nearest_task)
task.assigned_agent = agent
return nearest_task
def sort_nearest_tasks(self, agent, queue):
compatible_list = [task for task in queue.queue if task.is_agent_compatible(agent)]
dist_list = []
for task in compatible_list:
dist = math.sqrt(math.pow(task.pose.position.x - agent.pose.position.x, 2)
+ math.pow(task.pose.position.y - agent.pose.position.y, 2)
+ math.pow(task.pose.position.z - agent.pose.position.z, 2))
dist_list.append(dist)
zipped = zip(dist_list, compatible_list)
zipped.sort()
dist_list, sorted_list = zip(*zipped)
queue.queue = list(sorted_list)
return queue
def get_compabitible_task_for_agent(self, agent, queue):
compatible_task_list = TaskQueue()
for task in self.tasks.get_queue():
if task.is_agent_compatible(agent):
compatible_task_list.enqueue(task)
if compatible_task_list.is_empty():
print "No compatible tasks found for agent %s" %agent.name
return None
return compatible_task_list
def assign_tasks(self):
for agent in self.agents:
# Unneccessary but whatever.
task_queue = copy.copy(self.world_task_queue)
agent.task_queue = self.sort_nearest_tasks(agent, task_queue)
def generate_agents(self, num_of_agents=0):
pose1 = Pose()
pose1.position.x = 0
self.add_agent(ConstructionAgent('Construction Agent 1', self.pose_constructor(60, 50, 0, 0, 0, 0, 1)))
self.add_agent(ConstructionAgent('Construction Agent 2', self.pose_constructor(40, 50, 0, 0, 0, 0, 1)))
self.add_agent(ConstructionAgent('Construction Agent 3', self.pose_constructor(50, 40, 0, 0, 0, 0, 1)))
self.add_agent(TransportAgent('Transport Agent 1', pose=self.pose_constructor(50, 60, 0, 0, 0, 0, 1)))
def pose_constructor(self, posX, posY, posZ, quadX, quadY, quadZ, quadW):
tempPose = Pose()
tempPose.position.x = posX
tempPose.position.y = posY
tempPose.position.z = posZ
tempPose.orientation.x = quadX
tempPose.orientation.y = quadY
tempPose.orientation.z = quadZ
tempPose.orientation.w = quadW
return tempPose
def generate_world_tasks(self, no_of_task=0):
self.add_task(ConstructionTask('Construction Task 1',"Construction required in this area",
pose=self.pose_constructor(50, 10, 0, 0, 0, 0, 1)))
self.add_task(ConstructionTask('Construction Task 2', "Construction required in this area",
pose=self.pose_constructor(60, 20, 0, 0, 0, 0, 1)))
self.add_task(ConstructionTask('Construction Task 3', "Construction required in this area",
pose=self.pose_constructor(40, 27, 0, 0, 0, 0, 1)))
self.add_task(ConstructionTask('Construction Task 4', "Construction required in this area",
pose=self.pose_constructor(10, 65, 0, 0, 0, 0, 1)))
self.add_task(ConstructionTask('Construction Task 5', "Construction required in this area",
pose=self.pose_constructor(65, 40, 0, 0, 0, 0, 1)))
self.add_task(ConstructionTask('Construction Task 6', "Construction required in this area",
pose=self.pose_constructor(15, 50, 0, 0, 0, 0, 1)))
self.add_task(TransportTask('Transport Task 1', "Transport required",
pose=self.pose_constructor(45, 65, 0, 0, 0, 0, 1), end_pose=self.pose_constructor(10, 80, 0, 0, 0, 0, 1)))
self.add_task(TransportTask('Transport Task 2', "Transport required",
pose=self.pose_constructor(15, 75, 0, 0, 0, 0, 1),
end_pose=self.pose_constructor(30, 10, 0, 0, 0, 0, 1)))
self.add_task(TransportTask('Transport Task 3', "Transport required",
pose=self.pose_constructor(55, 70, 0, 0, 0, 0, 1),
end_pose=self.pose_constructor(35, 80, 0, 0, 0, 0, 1)))
