def __init__(self, robot, grab_designator=None):
"""
Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {
"required_trajectories": ["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot,
to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(
self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator,
self.empty_arm_designator),
transitions={
'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'
})
@smach.cb_interface(outcomes=["unlocked"])
def unlock(userdata=None):
""" 'Unlocks' a locking designator """
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(unlock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(unlock),
transitions={'unlocked': 'failed'})
def __init__(self, robot, selected_entity_designator, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done','failed'])
place_pose = dropPoseDesignator(robot, 0.6, "drop_pose")
with self:
smach.StateMachine.add("SPEAK", Speak(robot, selected_entity_designator, location_id, segment_area),
transitions={"done": "GRAB"})
smach.StateMachine.add("GRAB",
robot_smach_states.Grab(robot, selected_entity_designator,
UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")),
transitions={"done": "SAY_GRAB_SUCCESS", "failed": "SAY_GRAB_FAILED"})
smach.StateMachine.add('SAY_GRAB_SUCCESS',
robot_smach_states.Say(robot, ["Now I am going to toss the item in the trashbin",
"Let's clean up this object",
"Away with this garbage",
"Everything will be cleaned"], block=False),
transitions={"spoken": "CHECK_ARM_FREE"})
smach.StateMachine.add('SAY_GRAB_FAILED',
robot_smach_states.Say(robot, ["I could not grab the item.",
"I failed to grasp the item",
"I cannot reach the item",
"Item grab failed"], block=False),
transitions={"spoken": "failed"})
smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={"yes": "done", "no": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={"yes": "PLACE", "no": "done"})
smach.StateMachine.add('PLACE',
robot_smach_states.Place(robot,
selected_entity_designator,
place_pose,
OccupiedArmDesignator(robot.arms,
robot.rightArm,
name="occupied_arm_designator")),
transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('SAY_PLACE_SUCCESS',
robot_smach_states.Say(robot, ["Bye bye!",
"Yeah!",
"Successfully disposed the item",
"Another score for AMIGO"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('SAY_PLACE_FAILED',
robot_smach_states.Say(robot, ["I could not cleanup the item.",
"I cannot put the item in the trashbin",
"Item cleanup failed"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
def __init__(self, robot, grab_designator=None):
""" Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.leftArm, name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot, EntityDescriptionDesignator(self.grab_designator,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator, self.empty_arm_designator),
transitions={'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'})
@smach.cb_interface(outcomes=["unlocked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(lock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(lock),
transitions={'unlocked': 'failed'})
def __init__(self, robot, supporting_entity_designator, arm_designator):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'inspect_failed', 'grasp_failed'])
self._robot = robot
self._supporting_entity_designator = supporting_entity_designator
self._arm_designator = arm_designator
self._classification_result_designator = VariableDesignator(
[], resolve_type=[ClassificationResult])
self._grasp_entity_designator = VariableDesignator(resolve_type=Entity)
with self:
smach.StateMachine.add(
'INSPECT',
states.Inspect(
robot=self._robot,
entityDes=self._supporting_entity_designator,
objectIDsDes=self._classification_result_designator,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={
'done': 'SELECT_EASIEST_GRASP_ENTITY',
'failed': 'inspect_failed'
})
smach.StateMachine.add(
'SELECT_EASIEST_GRASP_ENTITY',
SelectEasiestGraspEntity(robot=self._robot,
classification_result_designator=self.
_classification_result_designator,
grasp_entity_designator=self.
_grasp_entity_designator.writeable),
transitions={
'selected': 'GRAB',
'failed': 'inspect_failed'
})
smach.StateMachine.add('GRAB',
states.Grab(
robot=self._robot,
item=self._grasp_entity_designator,
arm=self._arm_designator),
transitions={
'done': 'succeeded',
'failed': 'grasp_failed'
})
def __init__(self, robot, bar_designator, room_id, room_designator,
objects_list_des, unav_drink_des, name_options, objects):
"""
Initialization method
:param robot: robot api object
:param bar_designator: (EntityDesignator) in which the bar location is stored
:param room_id: room ID from challenge knowledge
:param room_designator: (EntityDesignator) in which the room location is stored
:param objects_list_des: (VariableDesignator) in which the available drinks are stored
:param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored
:param name_options: Names from common knowledge
:param objects: Objects from common knowledge
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
arm_designator = ds.UnoccupiedArmDesignator(robot=robot,
arm_properties={},
name='arm_des').lockable()
drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des')
drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des')
operator_name = ds.VariableDesignator(resolve_type=str, name='name_des')
operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des')
learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2')
with self:
# Lock the arm_designator
smach.StateMachine.add("LOCK_ARM",
states.LockDesignator(arm_designator),
transitions={'locked': "GET_ORDER"})
# Get order
smach.StateMachine.add("GET_ORDER",
GetOrder(robot=robot,
operator_name=operator_name,
drink_designator=drink_str_designator,
available_drinks_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des,
name_options=name_options,
objects=objects,
learn_check_designator=learn_check_designator.writeable,
target_room_designator=room_designator),
transitions={"succeeded": "INSPECT_BAR",
"failed": "failed",
"aborted": "aborted"})
# Inspect bar
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"),
transitions={"done": "GRASP_DRINK",
"failed": "FALLBACK_BAR"})
# Grasp drink
smach.StateMachine.add("GRASP_DRINK",
states.Grab(robot=robot, item=drink_designator, arm=arm_designator),
transitions={"done": "FIND_OPERATOR",
"failed": "FALLBACK_BAR"})
# Inspect or grasp fallback - ask for assistance
smach.StateMachine.add("FALLBACK_BAR",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "HANDOVER_FROM_HUMAN"})
# Handover from human fallback
smach.StateMachine.add("HANDOVER_FROM_HUMAN",
states.HandoverFromHuman(robot=robot, arm_designator=arm_designator,
grabbed_entity_designator=drink_designator),
transitions={"succeeded": "RESET_ROBOT_2",
"failed": "RESET_ROBOT_2",
"timeout": "RESET_ROBOT_2"})
smach.StateMachine.add("RESET_ROBOT_2",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "CHECK_LEARN_OPERATOR",
'failed': "CHECK_LEARN_OPERATOR"})
smach.StateMachine.add("CHECK_LEARN_OPERATOR",
states.CheckBool(learn_check_designator),
transitions={"true": "FIND_OPERATOR",
"false": "GO_TO_ROOM"})
smach.StateMachine.add("GO_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "SAY_NOT_FOUND",
"unreachable": "failed",
"goal_not_defined": "aborted"})
# Find operator
smach.StateMachine.add("FIND_OPERATOR",
states.FindPersonInRoom(robot=robot,
area=room_id,
name=operator_name,
discard_other_labels=True,
found_entity_designator=operator_designator.writeable),
transitions={"found": "GOTO_OPERATOR",
"not_found": "SAY_NOT_FOUND"})
# Move to this person
smach.StateMachine.add("GOTO_OPERATOR",
states.NavigateToObserve(robot=robot,
entity_designator=operator_designator),
transitions={"arrived": "SAY_THE_NAME",
"unreachable": "SAY_NOT_FOUND",
"goal_not_defined": "SAY_NOT_FOUND"})
# Say not found
smach.StateMachine.add("SAY_NOT_FOUND",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("not_found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
# Say the name
smach.StateMachine.add("SAY_THE_NAME",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
smach.StateMachine.add("RISE_FOR_HMI_2",
states.RiseForHMI(robot=robot),
transitions={"succeeded": "HAND_OVER",
"failed": "HAND_OVER"})
# Hand over the drink to the operator
smach.StateMachine.add("HAND_OVER",
states.HandoverToHuman(robot=robot,
arm_designator=arm_designator),
transitions={"succeeded": "UNLOCK_ARM",
"failed": "UNLOCK_ARM"})
smach.StateMachine.add("UNLOCK_ARM",
states.UnlockDesignator(arm_designator),
transitions={'unlocked': "RESET_ROBOT_3"})
smach.StateMachine.add("RESET_ROBOT_3",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "RETURN_TO_ROOM",
'failed': "RETURN_TO_ROOM"})
smach.StateMachine.add("RETURN_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "aborted"})
def __init__(self, robot, source_location, source_navArea, target_location, target_navArea, target_placeArea="on_top_of", source_searchArea="on_top_of"):
"""
Let the given robot move to a location and remove all entities from that table one at a time
:param robot: Robot to use
:param source_location: Location which will be cleared
:param target_location: Location where the objects will be placed
:return:
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
# Check types or designator resolve types
#check_type(source_location, Entity)
#check_type(target_location, Entity)
segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult])
selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', )
arm_des = UnoccupiedArmDesignator(robot, {}).lockable()
arm_des.lock()
place_position = states.util.designators.EmptySpotDesignator(robot, EdEntityDesignator(
robot, id=target_location.id),
area="on_top_of"
)
with self:
smach.StateMachine.add('INSPECT_SOURCE_ENTITY',
states.Inspect(robot, source_location, objectIDsDes=segmented_entities_designator,
searchArea=source_searchArea, navigation_area=source_navArea),
transitions={'done': 'DETERMINE_IF_CLEAR',
'failed': 'failed'}
)
#smach.StateMachine.add('DETERMINE_IF_CLEAR',
# isitclear(robot=robot,
# objectIDsDes=segmented_entities_designator),
# transitions={'clear': 'done',
# 'not_clear': 'failed'})
smach.StateMachine.add('DETERMINE_IF_CLEAR',
SelectEntity(robot, segmented_entities_designator, selected_entity_designator),
transitions={'no_entities_left': 'done',
'entity_selected': 'GRAB'}
)
smach.StateMachine.add('GRAB',
states.Grab(robot, selected_entity_designator, arm_des),
transitions={'done': 'INSPECT_TARGET',
'failed': 'failed'}
)
smach.StateMachine.add('INSPECT_TARGET',
states.Inspect(robot, target_location, searchArea=target_placeArea,
navigation_area=target_navArea),
transitions={'done': 'PLACE',
'failed': 'failed'}
)
smach.StateMachine.add('PLACE',
states.Place(robot, selected_entity_designator, place_position, arm_des),
transitions={'done': 'INSPECT_SOURCE_ENTITY',
'failed': 'failed'}
)
def __init__(self, robot, selected_entity_designator, room_des):
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
store_entity_id_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id")
store_entity_des = ds.EdEntityDesignator(robot, id_designator=store_entity_id_des)
selected_entity_type_des = ds.AttrDesignator(selected_entity_designator, "type", resolve_type=str)
store_area_name_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id")
trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6, "drop_pose")
category_des = ds.VariableDesignator(resolve_type=str, name="category_des")
with self:
smach.StateMachine.add("SPEAK", robot_smach_states.Say(robot, ["I will pick-up the {object}",
"Let's move the {object}"],
object=selected_entity_type_des,
block=True),
transitions={"spoken": "GRAB"})
smach.StateMachine.add("GRAB",
robot_smach_states.Grab(robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(robot, {},
name="empty_arm_designator")),
transitions={"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"})
smach.StateMachine.add("ARM_RESET", robot_smach_states.ArmToJointConfig(robot,
ds.UnoccupiedArmDesignator(robot,
{},
name="empty_arm_designator"),
"reset"),
transitions={"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"})
smach.StateMachine.add('SAY_GRAB_SUCCESS',
robot_smach_states.Say(robot, ["Now I am going to move this item",
"Let's clean up this object",
"Away with this one",
"Everything will be cleaned"], block=False),
transitions={"spoken": "GET_CATEGORY"})
smach.StateMachine.add('SAY_GRAB_FAILED',
robot_smach_states.Say(robot, ["I could not grab the item.",
"I failed to grasp the item",
"I cannot reach the item",
"Item grab failed"], block=False),
transitions={"spoken": "failed"})
smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot),
transitions={"yes": "done",
"no": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot),
transitions={"yes": "GET_CATEGORY",
"no": "done"})
# # ROBOT
# smach.StateMachine.add('GET_CATEGORY',
# EntityToCategory(robot, selected_entity_designator, category_des.writeable),
# transitions={"done": "DETERMINE_PLACE_LOCATION",
# "failed": "NAVIGATE_TO_TRASH"})
# OPERATOR
smach.StateMachine.add('GET_CATEGORY', OperatorToCategory(robot, category_des.writeable, room_des),
transitions={"done": "DETERMINE_PLACE_LOCATION",
"failed": "NAVIGATE_TO_TRASH"})
smach.StateMachine.add('DETERMINE_PLACE_LOCATION',
CategoryToLocation(category_des, store_entity_id_des.writeable,
store_area_name_des.writeable),
transitions={"trashbin": "INSPECT_TRASH",
"other": "PLACE_TO_STORE",
"failed": "NAVIGATE_TO_TRASH"})
smach.StateMachine.add('NAVIGATE_TO_TRASH',
robot_smach_states.NavigateToPlace(robot, trash_place_pose,
ds.OccupiedArmDesignator(robot, {},
name="occupied_arm_designator")),
transitions={"arrived": "PLACE_IN_TRASH",
"unreachable": "SAY_PLACE_FAILED",
"goal_not_defined": "SAY_PLACE_FAILED"})
smach.StateMachine.add('INSPECT_TRASH',
robot_smach_states.Inspect(robot,
store_entity_des),
transitions={"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('PLACE_IN_TRASH',
robot_smach_states.Place(robot,
selected_entity_designator,
trash_place_pose,
ds.OccupiedArmDesignator(robot, {},
name="occupied_arm_designator")),
transitions={"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('PLACE_TO_STORE', robot_smach_states.Place(robot, selected_entity_designator,
store_entity_des,
ds.OccupiedArmDesignator(robot, {}, name=
"occupied_arm_designator"),
"on_top_of"),
transitions={"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('SAY_PLACE_SUCCESS',
robot_smach_states.Say(robot, ["Bye bye!",
"Yeah!",
"Successfully disposed the item",
"Another score for HERO"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('SAY_PLACE_FAILED',
robot_smach_states.Say(robot, ["I could not cleanup the item.",
"I cannot put the item in the trashbin",
"Item cleanup failed"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
