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 Bot(threading.Thread):
'''Producer/Consumer Bot utilising TaskQueue.'''
def __init__(self, id, buffer=5):
threading.Thread.__init__(self)
self.id = id
self.daemon = True
self.buffer_time = buffer
self.q = TaskQueue()
self.last_action_time = 0
self.new_task = threading.Event()
self.task_done = threading.Event()
def schedule(self, tasks):
'''Schedule a new task by starting an independent thread.
- tasks (list): tasks to be scheduled.'''
t = threading.Thread(target=self._add_tasks,
args=(tasks, ),
daemon=True)
t.start()
def _add_tasks(self, tasks):
'''Adding tasks from a list to task queue and waiting if queue is full.'''
for task in tasks:
if self.q.full():
print('Producer: Queue full - sleeping')
self.task_done.wait()
print('Producer: Queue not full now, adding task..')
self.task_done.clear()
self.q.put(task)
print(f'Producer: Scheduled {task}')
self.new_task.set()
def run(self):
'''Runs the thread which executes tasks.
- stay idle until scheduled task time'''
while True:
if self.q.empty():
self.new_task.wait()
print('Bot: Woke up')
self.new_task.clear()
task = self._idle()
print(f'Bot executing {task}')
task.execute()
self.last_action_time = time()
self.q.task_done()
self.task_done.set()
def _idle(self):
'''Wait for next scheduled task in queue.
- If tasks are added to queue, check next task again'''
next_task = self.q.peek()
time_until = self._time_until(next_task)
print(f'Bot: Time until next task {time_until}')
flag = self.new_task.wait(timeout=time_until)
if flag:
print('Bot: New task added to queue - checking next task')
self.new_task.clear()
return self._idle()
else:
return self.q.get()
def _time_until(self, task):
'''Calculate time in seconds until scheduled task.
- Convert task.time datetime to epoch time
- Add buffer delay if scheduled task is too close to last action time'''
now = time()
time_since = now - self.last_action_time
scheduled = int(task.time.strftime('%s'))
time_until = scheduled - now
if time_until < 0:
if time_since < self.buffer_time:
time_until = time_until + self.buffer_time
else:
time_until = 0
return time_until
