def do_step(self):
direction = None
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
current_task = self.rhbp_agent.tasks[active_subtask.parent_task_name]
# TODO correct bug of map disruption in order to try again with the clever meeting point
#self.rhbp_agent.nearby_agents, common_meeting_point = self.rhbp_agent.local_map.get_common_meeting_point(current_task)
self.rhbp_agent.nearby_agents, common_meeting_point = self.rhbp_agent.local_map.get_trivial_meeting_point(
current_task)
# get the position of agents and attached blocks for the connect action
if common_meeting_point is not None:
task_meeting_point = self.rhbp_agent.local_map.meeting_position(current_task, common_meeting_point)
active_subtask.meeting_point = task_meeting_point
path_id, direction = self.rhbp_agent.local_map.get_meeting_point_move(active_subtask, task_meeting_point)
active_subtask.path_to_meeting_point_id = path_id
if direction is not None and direction is not False:
params = [KeyValue(key="direction", value=direction)]
if direction == 'cw' or direction == 'ccw':
rospy.logdebug(
self._agent_name + "::" + self._name + " executing move to meeting point, direction: " + str(
direction))
action_generic_simple(publisher=self._pub_generic_action, action_type=GenericAction.ACTION_TYPE_ROTATE,
params=params)
else:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name + " executing move to meeting point, direction: " + str(direction))
action_generic_simple(publisher=self._pub_generic_action, action_type=GenericAction.ACTION_TYPE_MOVE,
params=params)
def do_step(self):
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
task_name = active_subtask.parent_task_name
params = [KeyValue(key="task", value=task_name)]
rospy.logdebug(self._agent_name + "::" + self._name +
"submitting task " + str(task_name))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_SUBMIT,
params=params)
def do_step(self):
# get the subtask
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
# get the task
active_task = self.rhbp_agent.tasks[active_subtask.parent_task_name]
# get involved agents
nearby_agents = self.rhbp_agent.nearby_agents
# create randomly order copy of nearby agents
nearby_agents_random = nearby_agents[:]
random.shuffle(nearby_agents_random)
# relative to the submitter transform
relative_submitter_ratio = self.rhbp_agent.local_map._from_matrix_to_relative(active_subtask.position \
,self.rhbp_agent.local_map._attached_blocks[0]._position)
agent_to_connect = None
for close_by_agent in nearby_agents_random:
for subtask in active_task.sub_tasks:
if subtask.assigned_agent == close_by_agent and not subtask.complete \
and close_by_agent != self._agent_name:
for block in self.rhbp_agent.local_map._attached_blocks:
# block has to be transform to relative to the submitter
# --> matrix and then rel -->submitter
block_relative_to_submitter = self.rhbp_agent.local_map._from_relative_to_matrix(block._position, \
relative_submitter_ratio)
distance2d = abs(block_relative_to_submitter[0] - subtask.position[0]) \
+ abs(block_relative_to_submitter[1] - subtask.position[1])
distance = abs(distance2d)
##########
#print (self._agent_name)
#print (str(block_relative_to_submitter))
#print (close_by_agent)
#print(str(subtask.position))
#print (distance)
##########
if distance == 1: # if the blocks 1 step away, that is the one to connect
block_position = block._position
agent_to_connect = close_by_agent
if agent_to_connect is not None:
# connect message
params = [
KeyValue(key="y", value=str(block_position[0])),
KeyValue(key="x", value=str(block_position[1])),
KeyValue(key="agent", value=agent_to_connect)
]
rospy.logdebug(self._agent_name + "::" + self._name +
" connecting with " + str(agent_to_connect))
action_generic_simple(
publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_CONNECT,
params=params)
def do_step(self):
path_id, direction = self.rhbp_agent.local_map.get_go_to_goal_area_move(
self.path_to_goal_area_id)
self.path_to_goal_area_id = path_id
if direction is not None:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name +
" executing move to " + str(direction))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_MOVE,
params=params)
def do_step(self):
path_id, direction = self.rhbp_agent.local_map.get_exploration_move(self.exploration_path_id)
# TODO add the iddle behavior through the behavioral network activation
if direction is not None:
self.exploration_path_id = path_id
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name + " executing move to " + str(direction))
action_generic_simple(publisher=self._pub_generic_action, action_type=GenericAction.ACTION_TYPE_MOVE,
params=params)
else:
action_generic_simple(publisher=self._pub_generic_action, action_type=GenericAction.ACTION_TYPE_MOVE,
params=[KeyValue(key="direction", value='INVALID')])
def do_step(self):
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
direction = self.rhbp_agent.local_map.get_direction_to_close_block(
active_subtask.type)
if direction is not None and direction is not False:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name +
"attaching to block in direction " + str(direction))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_ATTACH,
params=params)
def do_step(self):
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
path_id, direction = self.rhbp_agent.local_map.get_go_to_dispenser_move(
active_subtask)
active_subtask.path_to_dispenser_id = path_id
if direction is not None:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name +
" executing move to " + str(direction))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_MOVE,
params=params)
def do_step(self):
active_subtask = self.rhbp_agent.assigned_subtasks[0] # type: SubTask
#TODO REFACTOR TO THE ATTACHED BLOCK AND NOT THE CLOSE ONE
direction_coord = self.rhbp_agent.local_map._attached_blocks[
0]._position
direction = 'e'
for direction_string, coord in global_variables.MOVEMENTS.items():
if np.array_equal(direction_coord, coord):
direction = direction_string
if direction is not None and direction is not False:
params = [KeyValue(key="direction", value=direction)]
rospy.logdebug(self._agent_name + "::" + self._name +
"attaching to block in direction " + str(direction))
action_generic_simple(publisher=self._pub_generic_action,
action_type=GenericAction.ACTION_TYPE_DETACH,
params=params)