def execute(self, userdata=None):
arm = None
# See if there's an arm holding something
for k, v in self._robot.arms.iteritems():
if v.occupied_by is not None:
arm = v
break
if arm is None:
return "failed"
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.ArmDesignator(all_arms={arm.side: arm},
preferred_arm=arm)
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(
self._robot,
arm_properties={
"required_trajectories": ["prepare_place"],
"required_goals": ["reset", "handover_to_human"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(
self._robot, {
"required_goals": ["reset", "handover_to_human"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
place = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self.place_designator,
arm=arm_designator)
result = place.execute()
# If failed, do handover to human in order to continue
if result != "done":
rospy.loginfo("{place} resulted in {out}".format(place=place,
out=result))
handover = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
handover.execute()
return "succeeded" if result == "done" else "failed"
def test_handover_to_human(self):
state = states.HandoverToHuman(self.robot, self.arm_ds)
state.check_consistency()
self.assertEqual(state.execute(), "succeeded")
self.robot.arms["rightArm"].send_joint_goal.assert_not_called()
self.robot.arms["rightArm"].send_gripper_goal.assert_not_called()
self.robot.arms["leftArm"].send_joint_goal.assert_any_call(
'handover_to_human', max_joint_vel=mock.ANY, timeout=mock.ANY)
self.robot.arms["leftArm"].send_joint_goal.assert_any_call(
'reset', max_joint_vel=mock.ANY, timeout=mock.ANY)
self.robot.arms["leftArm"].send_gripper_goal.assert_any_call(
'open', mock.ANY, max_torque=mock.ANY)
self.assertIsNone(self.robot.arms["leftArm"].occupied_by)
def __init__(self, robot, drop_zone_id):
"""
:param robot: robot object
:param drop_designator: EdEntityDesignator designating the collection zone
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
with self:
smach.StateMachine.add("GO_TO_COLLECTION_ZONE",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "OPEN_DOOR_PLEASE"})
smach.StateMachine.add("OPEN_DOOR_PLEASE",
states.Say(robot, "Can you please open the door for me? It seems blocked!"),
transitions={"spoken": "WAIT_FOR_DOOR_OPEN"})
smach.StateMachine.add("WAIT_FOR_DOOR_OPEN",
states.WaitTime(robot=robot, waittime=5),
transitions={"waited": "GO_TO_COLLECTION_ZONE2",
"preempted": "GO_TO_COLLECTION_ZONE2"})
smach.StateMachine.add("GO_TO_COLLECTION_ZONE2",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "failed"})
smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "HANDOVER"})
smach.StateMachine.add("HANDOVER",
states.HandoverToHuman(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "failed"})
def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(self._robot)
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def __init__(self, robot, manipulated_items):
"""@param manipulated_items is VariableDesignator that will be a list of items manipulated by the robot."""
self.manipulated_items = manipulated_items
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.cabinet = ds.EntityByIdDesignator(robot,
id=CABINET,
name="pick_shelf")
# self.place_shelf = ds.EntityByIdDesignator(robot, id=PLACE_SHELF, name="place_shelf")
not_manipulated = lambda entity: not entity in self.manipulated_items.resolve(
)
def entity_z_pos(entity):
"""
Checks if the entity is between the minimum and maximum grasp height
:param entity:
:return:
"""
if not entity._pose:
return False
return MIN_GRASP_HEIGHT < entity._pose.p.z() < MAX_GRASP_HEIGHT
# select the entity closest in x direction to the robot in base_link frame
def weight_function(entity):
# TODO: return x coordinate of entity.center_point in base_link frame
epose = entity.pose.projectToFrame(
robot.robot_name + "/base_link",
robot.tf_listener) # Get position in base_link
p = epose.frame.p
return p.x()**2
self.current_item = ds.LockingDesignator(ds.EdEntityDesignator(
robot,
criteriafuncs=[
not_ignored, size, not_manipulated, min_entity_height,
entity_z_pos, max_width
],
weight_function=weight_function,
debug=False,
name="item"),
name="current_item")
#This makes that the empty spot is resolved only once, even when the robot moves. This is important because the sort is based on distance between robot and constraint-area
# self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position")
self.place_position = ds.LockingDesignator(EmptyShelfDesignator(
robot, self.cabinet, name="placement", area=PLACE_SHELF),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {
'required_trajectories': ['prepare_grasp'],
'required_goals': ['carrying_pose'],
'required_gripper_types': [arms.GripperTypes.GRASPING],
'required_arm_name': PREFERRED_ARM
},
name="empty_arm_designator")
self.arm_with_item_designator = ds.ArmHoldingEntityDesignator(
robot, {'required_objects': [self.current_item]}, {
"required_trajectories": ["prepare_place"],
"required_goals": ["reset", "handover_to_human"],
'required_gripper_types': [arms.GripperTypes.GRASPING]
},
name="arm_with_item_designator")
# print "{0} = pick_shelf".format(self.pick_shelf)
# print "{0} = current_item".format(self.current_item)
# print "{0} = place_position".format(self.place_position)
# print "{0} = empty_arm_designator".format(self.empty_arm_designator)
# print "{0} = arm_with_item_designator".format(self.arm_with_item_designator)
with self:
# smach.StateMachine.add( "NAV_TO_OBSERVE_PICK_SHELF",
# #states.NavigateToObserve(robot, self.pick_shelf),
# states.NavigateToSymbolic(robot, {self.pick_shelf:"in_front_of", EntityByIdDesignator(robot, id=ROOM):"in"}, self.pick_shelf),
# transitions={ 'arrived' :'LOOKAT_PICK_SHELF',
# 'unreachable' :'LOOKAT_PICK_SHELF',
# 'goal_not_defined' :'LOOKAT_PICK_SHELF'})
smach.StateMachine.add("REMOVE_ENTITIES",
RemoveSegmentedEntities(robot=robot),
transitions={'done': 'LOOKAT_PICK_SHELF'})
smach.StateMachine.add("LOOKAT_PICK_SHELF",
states.LookAtArea(robot,
self.cabinet,
area=PICK_SHELF),
transitions={'succeeded': 'SEGMENT_SHELF'})
smach.StateMachine.add("SEGMENT_SHELF",
SegmentShelf(robot,
entity_id=CABINET,
area_id=PICK_SHELF),
transitions={'done': 'LOCK_ITEM'})
@smach.cb_interface(outcomes=['locked'])
def lock(userdata=None):
self.current_item.lock(
) #This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
if self.current_item.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(
self.current_item.resolve().id))
self.place_position.lock(
) #This determines that self.place_position will lock/cache its result after its resolved the first time.
return 'locked'
smach.StateMachine.add('LOCK_ITEM',
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot,
EntityDescriptionDesignator(
self.current_item,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
Grab(robot, self.current_item,
self.empty_arm_designator),
transitions={
'done': 'STORE_ITEM',
'failed': 'SAY_GRAB_FAILED'
})
smach.StateMachine.add(
"SAY_GRAB_FAILED",
states.Say(robot, ["I couldn't grab this thing"], mood="sad"),
transitions={'spoken': 'UNLOCK_ITEM_AFTER_FAILED_GRAB'}
) # Not sure whether to fail or keep looping with NAV_TO_OBSERVE_PICK_SHELF
@smach.cb_interface(outcomes=['unlocked'])
def unlock_and_ignore(userdata=None):
global ignore_ids
# import ipdb; ipdb.set_trace()
if self.current_item.resolve():
ignore_ids += [self.current_item.resolve().id]
rospy.loginfo("Current_item WAS now locked to {0}".format(
self.current_item.resolve().id))
self.current_item.unlock(
) #This determines that self.current_item can now resolve to a new value on the next call
self.place_position.unlock(
) #This determines that self.place_position can now resolve to a new position on the next call
return 'unlocked'
smach.StateMachine.add('UNLOCK_ITEM_AFTER_FAILED_GRAB',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'failed'})
@smach.cb_interface(outcomes=['stored'])
def store_as_manipulated(userdata=None):
# manipulated_items.current += [self.current_item.current]
item_list = manipulated_items.resolve()
item_list += [self.current_item.resolve()]
w = ds.VariableWriter(manipulated_items)
w.write(item_list)
return 'stored'
smach.StateMachine.add(
'STORE_ITEM',
smach.CBState(store_as_manipulated),
transitions={'stored': 'LOOKAT_PLACE_SHELF'})
smach.StateMachine.add("LOOKAT_PLACE_SHELF",
states.LookAtArea(robot,
self.cabinet,
area=PLACE_SHELF),
transitions={'succeeded': 'PLACE_ITEM'})
smach.StateMachine.add("PLACE_ITEM",
Place(robot, self.current_item,
self.place_position,
self.arm_with_item_designator),
transitions={
'done': 'RESET_HEAD_PLACE',
'failed': 'RESET_HEAD_HUMAN'
})
smach.StateMachine.add(
"RESET_HEAD_PLACE",
states.CancelHead(robot),
transitions={'done': 'UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE'})
smach.StateMachine.add(
"RESET_HEAD_HUMAN",
states.CancelHead(robot),
transitions={'done': 'SAY_HANDOVER_TO_HUMAN'})
smach.StateMachine.add('UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add(
"SAY_HANDOVER_TO_HUMAN",
states.Say(robot, [
"I'm can't get rid of this item myself, can somebody help me maybe?"
]),
transitions={'spoken': 'HANDOVER_TO_HUMAN'})
smach.StateMachine.add('HANDOVER_TO_HUMAN',
states.HandoverToHuman(
robot, self.arm_with_item_designator),
transitions={
'succeeded': 'UNLOCK_AFTER_HANDOVER',
'failed': 'UNLOCK_AFTER_HANDOVER'
})
smach.StateMachine.add('UNLOCK_AFTER_HANDOVER',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': '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"})
