def handle_additional_action(self, additional_action, error_task):
if (additional_action == AdditionalAction.DUMP_ITEM_ON_HAND):
additional_actions = ActionList()
rospy.logwarn("Dumping object [%s] to the current table" % (error_task.object_str))
TaskFormatter.format_place_task(error_task, additional_actions.task_list)
rospy.loginfo("Additional actions:")
print(additional_actions)
return additional_actions
elif (additional_action == AdditionalAction.DUMP_ITEM_ON_HAND_AND_PICK_TO_CAP):
additional_actions = ActionList()
rospy.logwarn("Dumping object [%s] to the current table" % (error_task.object_str))
TaskFormatter.format_place_task(error_task, additional_actions.task_list)
current_cap = self.holding_capacity - self.task_replanner.items_on_hand
rospy.logwarn("Current capacity is: ", current_cap)
rospy.loginfo("Additional actions:")
print(additional_actions)
return additional_actions
elif (additional_action == AdditionalAction.PICK_AND_DUMP_ON_TABLE):
additional_actions = ActionList()
rospy.logwarn("Picking object [%s] from ROBOT" % (error_task.object_str))
TaskFormatter.format_pick_from_robot(error_task, additional_actions.task_list)
rospy.logwarn("Dumpinggggg. Please ignore random destination.")
error_task.set_destination(1) # Set a random destination so mux doesn't call precision place
TaskFormatter.format_place_task(error_task, additional_actions.task_list)
rospy.loginfo("Additional actions:")
print(additional_actions)
return additional_actions
return None
def __init__(self, yaml_path, verbose=False):
self.input_list = ActionList()
self.original_task_list = None
self.in_progress_list = ActionList()
self.output_list = TaskList()
self.items_on_hand = -1
self.task_sorter = TaskSorter(yaml_path)
self.error_index = -1
self.error_case = -1
self.verbose = verbose
def __init__(self):
self.fsm = StateMachine(name='TaskExecutorStateMachine')
self.input_sub = rospy.Subscriber('/suii_task_executor/input', SuiiTaskList, self.input_callback)
self.action_list = ActionList()
# States
self.fsm.add_state(StateName.IDLE_STATE, IdleState(self.fsm))
self.fsm.add_state(StateName.CONVERT_DATA_STATE, ConvertDataState(self.fsm))
self.fsm.add_state(StateName.TASK_SELECT, TaskSelectState(self.fsm))
self.fsm.add_state(StateName.TASK_EXECUTION, TaskExecutionState(self.fsm))
self.fsm.add_state(StateName.EMERGENCY, EmergencyState(self.fsm))
self.fsm.add_state(StateName.ERROR, ErrorState(self.fsm))
# Transitions
self.fsm.add_transition(TransitionName.IDLE_STATE, Transition(StateName.IDLE_STATE))
self.fsm.add_transition(TransitionName.CONVERT_DATA_STATE, Transition(StateName.CONVERT_DATA_STATE))
self.fsm.add_transition(TransitionName.TASK_SELECT, Transition(StateName.TASK_SELECT))
self.fsm.add_transition(TransitionName.TASK_EXECUTION, Transition(StateName.TASK_EXECUTION))
self.fsm.add_transition(TransitionName.EMERGENCY, Transition(StateName.EMERGENCY))
self.fsm.add_transition(TransitionName.ERROR, Transition(StateName.ERROR))
# set default state
self.fsm.set_state(StateName.IDLE_STATE)
rospy.loginfo('Handler initialized!')
def convert_task_to_actions(self, task):
actions = ActionList()
if (task.type == int(TaskType.NAVIGATION)):
TaskFormatter.format_drive(task.destination, actions.task_list)
elif (task.type == int(TaskType.TRANSPORTATION)):
TaskFormatter.format_drive(task.source, actions.task_list)
if (task.destination_str == LocationIdentifierType.PP.fullname): # PPT
TaskFormatter.format_pick_task(task, actions.task_list)
TaskFormatter.format_place_to_robot(task, actions.task_list)
TaskFormatter.format_drive(task.destination, actions.task_list)
TaskFormatter.format_find_hole(task, actions.task_list)
TaskFormatter.format_pick_from_robot(task, actions.task_list)
TaskFormatter.format_place_task(task, actions.task_list)
else:
if (task.is_dest_same_as_src()):
TaskFormatter.format_pick_task(task, actions.task_list)
TaskFormatter.format_place_task(task, actions.task_list)
else:
TaskFormatter.format_pick_task(task, actions.task_list)
TaskFormatter.format_place_to_robot(task, actions.task_list)
TaskFormatter.format_drive(task.destination, actions.task_list)
TaskFormatter.format_pick_from_robot(task, actions.task_list)
TaskFormatter.format_place_task(task, actions.task_list)
return actions
def __init__(self, fsm):
super(ErrorState, self).__init__(fsm)
self.list = ActionList()
self.error_code = -1 # not in use for now, will need later
self.error_index = -1 # index of the error'd item
self.error_pub = rospy.Publisher('/suii_task_executor/replan',
SuiiTaskList,
queue_size=10,
latch=True)
self.error_list = SuiiTaskList
def __init__(self, holding_capacity=MAX_HOLDING_CAPACITY, yaml_path="", verbose=False):
# Basic config
self.holding_capacity = holding_capacity
self.yaml_path = yaml_path
self.verbose = verbose
self.MAX_REPLAN_ATTEMPTS = 10
# Here come the lists
self.task_list = TaskList() # The current one
self.task_list_last_updated = TaskList() # Last replanned task list
self.replan_list = ActionList() # Action list from mux
self.error_index = -1 # Error index
# Output
self.output_list = ActionList() # The output list to send to mux
self.task_list_type = -1 # Type of the list (TRANSPORTATION/NAVIGATION)
# Helpers
self.transport_optimizer = TaskTransportOptimizer(holding_capacity, yaml_path, verbose)
self.navigation_optimizer = TaskNavigationOptimizer(yaml_path, verbose)
self.task_replanner = TaskReplanner(yaml_path, verbose)
def __init__(self, fsm):
super(TaskSelectState, self).__init__(fsm)
self.list = ActionList()
self.index = 0
self.error = False
self.done = False
def not_done_tasks(self, todo_list, done_list):
not_done = ActionList()
for item in todo_list:
if not (item in done_list):
not_done.task_list.append(item)
return not_done
def __init__(self, yaml_path, verbose=False):
self.input_list = TaskList()
self.output_list = ActionList()
self.task_sorter = TaskSorter(yaml_path)
self.verbose = verbose
def __init__(self, holding_capacity, yaml_path, verbose=False):
self.holding_capacity = holding_capacity
self.input_list = TaskList()
self.output_list = ActionList()
self.task_sorter = TaskSorter(yaml_path)
self.verbose = verbose
def ros_to_action_list(ros_msg):
result = ActionList()
for item in ros_msg.tasks:
result.task_list.append(MuxConverter.ros_to_action(item))
return result