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
