def grab(p, robot):
entities = parse_object(p, robot)
if not entities:
say("No such entities!")
return
# Only filter to entities that do not have a shape but do have a convex hull
entities = [
e for e in entities if not e.has_shape and len(e.convex_hull) > 0
]
if not entities:
say("I am sorry, but I cannot grab that object")
return
arm = robot.leftArm
machine = Grab(robot,
arm=ds.UnoccupiedArmDesignator(robot.arms, arm),
item=ds.EntityByIdDesignator(robot, id=entities[0].id))
stop()
global ACTION
ACTION = SmachAction(machine)
ACTION.start()
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 setUp(self):
entity = Entity("123", "dummy", "/map", None, None, {}, None, 0)
self.robot.arms["leftArm"].occupied_by = entity
self.arm_ds = ds.UnoccupiedArmDesignator(
self.robot, {
"required_goals": ['handover_to_human'],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
self.entity = Entity("456", "dummy", "/map", None, None, {}, None, 0)
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 setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
furniture_designator = ds.VariableDesignator(resolve_type=Entity)
entity_designator = ds.VariableDesignator(resolve_type=Entity)
arm_designator = ds.UnoccupiedArmDesignator(robot, {})
with state_machine:
# Intro
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT),
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
# Say we're gonna start
StateMachine.add('SAY_START', Say(robot, "Hand me that it is!", block=False),
transitions={'spoken': 'NAVIGATE_TO_START'})
# Drive to the start location
StateMachine.add('NAVIGATE_TO_START',
NavigateToWaypoint(robot, ds.EdEntityDesignator(robot, id=HOME_LOCATION)),
transitions={'arrived': 'GET_FURNITURE_FROM_OPERATOR_POSE',
'unreachable': 'NAVIGATE_TO_START', # ToDo: other fallback
'goal_not_defined': 'done'}) # I'm not even going to fill this in
# The pre-work
StateMachine.add('GET_FURNITURE_FROM_OPERATOR_POSE',
GetFurnitureFromOperatorPose(robot, furniture_designator.writeable),
transitions={'done': 'INSPECT_FURNITURE'})
# Go to the furniture object that was pointing to see what's there
StateMachine.add('INSPECT_FURNITURE',
InspectFurniture(robot, furniture_designator, entity_designator.writeable),
transitions={"succeeded": "IDENTIFY_OBJECT",
"failed": "NAVIGATE_TO_START"}) # If no entities, try again
# Point at the object
StateMachine.add('IDENTIFY_OBJECT',
IdentifyObject(robot, entity_designator, arm_designator),
transitions={'done': 'NAVIGATE_TO_START', # Just keep on going
'failed': 'NAVIGATE_TO_START'}) # Just keep on going
return state_machine
def __init__(self, dummy_robot):
smach.StateMachine.__init__(self, outcomes=[
"succeeded",
"failed",
])
# Create designators
dummy_trashbin_designator = ds.EdEntityDesignator(
dummy_robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator')
dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {})
dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {})
with self:
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(dummy_robot,
dummy_trashbin_designator,
dummy_arm_designator_un),
transitions={
"succeeded": "TURN_BASE",
"failed": "TURN_BASE",
"aborted": "failed"
})
smach.StateMachine.add("TURN_BASE",
states.ForceDrive(dummy_robot, 0, 0, 0.5,
3),
transitions={"done": "DROP_TRASH"})
smach.StateMachine.add("DROP_TRASH",
DropTrash(dummy_robot,
dummy_arm_designator_oc),
transitions={
"succeeded": "succeeded",
"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",
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",
Grab(
robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals":
["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")),
transitions={
"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"
})
smach.StateMachine.add(
"ARM_RESET",
ArmToJointConfig(
robot,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={"required_goals": ["reset"]},
name="empty_arm_designator"), "reset"),
transitions={
"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"
})
smach.StateMachine.add(
'SAY_GRAB_SUCCESS',
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',
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',
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',
Inspect(robot, store_entity_des),
transitions={
"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"
})
arm_properties_place = {
"required_trajectories": ["prepare_place"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
}
arm_designator_place = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_IN_TRASH',
Place(robot, selected_entity_designator,
trash_place_pose,
arm_designator_place),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
arm_designator_place_store = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_TO_STORE',
Place(robot, selected_entity_designator,
store_entity_des,
arm_designator_place_store,
"on_top_of"),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
smach.StateMachine.add(
'SAY_PLACE_SUCCESS',
Say(robot, [
"Bye bye!", "Yeah!", "Successfully disposed the item",
"Another score for {}".format(robot.robot_name)
],
block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add(
'SAY_PLACE_FAILED',
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 execute(self, userdata=None):
d = ds.UnoccupiedArmDesignator(self._robot, {},
name="empty_arm_designator2")
if d.resolve():
return "yes"
return "no"
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'})
return 'succeeded'
if __name__ == "__main__":
from robot_skills import get_robot
if len(sys.argv) > 1:
robot_name = sys.argv[1]
point_at = sys.argv[2]
look_at = sys.argv[3]
rospy.init_node('test_follow_operator')
robot = get_robot(robot_name)
sm = PointAt(
robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=ds.EdEntityDesignator(robot,
id=point_at,
name='point_at_des'),
look_at_designator=ds.EdEntityDesignator(robot,
id=look_at,
name='look_at_des'))
sm.execute()
else:
print(
"Please provide robot name, point_at ID and look_at ID as argument."
)
exit(1)
smach.StateMachine.add("LOWER_ARM", states.ArmToJointConfig(robot=robot,
arm_designator=arm_occupied_designator,
configuration="reset"),
transitions={"succeeded": "RECEIVED_TRASH_BAG",
"failed": "RECEIVED_TRASH_BAG"})
smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw"
" it away, please move away.", block=True),
transitions={'spoken': 'succeeded'})
smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue",
block=False),
transitions={'spoken': "failed"})
if __name__ == '__main__':
import os
import robot_smach_states.util.designators as ds
from robot_skills import Hero
rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
hero = Hero()
hero.reset()
arm = ds.UnoccupiedArmDesignator(hero, {})
# sm = HandoverFromHumanFigure(hero, arm, grabbed_entity_label='trash')
# sm.execute()
GrabTrash(hero, arm, 100, 2).execute()
import robot_smach_states.util.designators as ds
from robot_smach_states import Grab
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Put an imaginary object in the world model and grasp it using the "
"'Grab' smach state")
parser.add_argument("x", type=float, help="x-coordinate (in map) of the imaginary object")
parser.add_argument("y", type=float, help="y-coordinate (in map) of the imaginary object")
parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object")
parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_grasping")
robot = get_robot(args.robot)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
item = ds.EdEntityDesignator(robot, id=entity_id)
arm = ds.UnoccupiedArmDesignator(robot, {})
grab_state = Grab(robot, item, arm)
grab_state.execute()
arm_designator=arm_occupied_designator,
configuration="reset"),
transitions={"succeeded": "RECEIVED_TRASH_BAG",
"failed": "RECEIVED_TRASH_BAG"})
smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw"
" it away, please move away.", block=True),
transitions={'spoken': 'succeeded'})
smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue",
block=False),
transitions={'spoken': "failed"})
if __name__ == '__main__':
import os
import robot_smach_states.util.designators as ds
from robot_skills import Hero, arms
rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
hero = Hero()
hero.reset()
arm = ds.UnoccupiedArmDesignator(hero, {"required_goals": ["reset", "handover"], "force_sensor_required": True,
"required_gripper_types": [arms.GripperTypes.GRASPING]})
# sm = HandoverFromHumanFigure(hero, arm, grabbed_entity_label='trash')
# sm.execute()
GrabTrash(hero, arm, 100, 2).execute()
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
msg = "\n".join([
"==============================================",
"== CHALLENGE HELP ME CARRY ==",
"=============================================="
])
rospy.loginfo("\n" + msg)
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_goals": [
"handover_to_human", "reset",
challenge_knowledge.driving_bag_pose,
challenge_knowledge.drop_bag_pose
],
"required_gripper_types": [GripperTypes.GRASPING]
},
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
self.place_position = ds.LockingDesignator(EmptySpotDesignator(
robot,
self.target_destination,
arm_designator=self.bag_arm_designator,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={
'stopped': 'ASK_FOR_TASK',
'lost_operator': 'ASK_FOR_TASK',
'no_operator': 'ASK_FOR_TASK'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add(
'WAIT_FOR_TASK',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.commands.keys(),
commands_as_outcomes=True),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'
})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
states.HandoverFromHuman(
robot,
self.bag_arm_designator,
"current_item",
self.current_item,
arm_configuration=challenge_knowledge.carrying_bag_pose),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER',
# For now in simulation timeout is considered a success.
'failed': 'BACKUP_CLOSE_GRIPPER'
})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(
robot,
self.bag_arm_designator,
gripperstate=GripperState.CLOSE),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'
})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(
robot, self.bag_arm_designator,
challenge_knowledge.driving_bag_pose),
transitions={
'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'
})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.default_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.backup_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
hmc_states.DropBagOnGround(
robot, self.bag_arm_designator,
challenge_knowledge.drop_bag_pose),
transitions={'done': 'ASKING_FOR_HELP'})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, grasp_furniture_id, place_furniture_id1,
place_furniture_id2):
"""
Constructor
:param robot: robot object
:param grasp_furniture_id: string identifying the furniture object where to grasp the objects
:param place_furniture_id1: string identifying the furniture object where to place objects 1 and 2
:param place_furniture_id2: string identifying the furniture object where to place object 3
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
grasp_furniture_designator = ds.EntityByIdDesignator(
robot, id=grasp_furniture_id)
grasp_designator1 = DefaultGrabDesignator(
robot=robot,
surface_designator=grasp_furniture_designator,
area_description="on_top_of")
grasp_designator2 = DefaultGrabDesignator(
robot=robot,
surface_designator=grasp_furniture_designator,
area_description="on_top_of")
grasp_designator3 = DefaultGrabDesignator(
robot=robot,
surface_designator=grasp_furniture_designator,
area_description="on_top_of")
# TODO use the same designator that is used for setting the table
arm_designator = ds.UnoccupiedArmDesignator(robot, {})
place_furniture_designator1 = ds.EntityByIdDesignator(
robot, id=place_furniture_id1)
place_designator1 = EmptySpotDesignator(
robot=robot,
place_location_designator=place_furniture_designator1,
arm_designator=arm_designator,
area="on_top_of")
place_furniture_designator3 = ds.EntityByIdDesignator(
robot, id=place_furniture_id2)
place_designator3 = EmptySpotDesignator(
robot=robot,
place_location_designator=place_furniture_designator3,
arm_designator=arm_designator,
area="on_top_of")
with self:
# Move to the inspect location
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE1",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator: "in_front_of"},
grasp_furniture_designator),
transitions={
'arrived': 'INSPECT_GRASP_SURFACE',
'unreachable': 'MOVE_TO_GRASP_SURFACE2',
'goal_not_defined': 'INSPECT_GRASP_SURFACE'
})
# Backup for moving to inspect location
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE2",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator: "large_in_front_of"},
grasp_furniture_designator),
transitions={
'arrived': 'INSPECT_GRASP_SURFACE',
'unreachable': 'INSPECT_GRASP_SURFACE',
'goal_not_defined': 'INSPECT_GRASP_SURFACE'
})
# Inspect grasp furniture
smach.StateMachine.add("INSPECT_GRASP_SURFACE",
states.Inspect(
robot=robot,
entityDes=grasp_furniture_designator,
objectIDsDes=None,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={
"done": "GRAB_ITEM_1",
"failed": "failed"
})
# Grasp the first item
smach.StateMachine.add("GRAB_ITEM_1",
GrabSingleItem(
robot=robot,
grab_designator=grasp_designator1),
transitions={
"succeeded": "GRAB_ITEM_2",
"failed": "GRAB_ITEM_2"
})
# Grasp the second item
smach.StateMachine.add("GRAB_ITEM_2",
GrabSingleItem(
robot=robot,
grab_designator=grasp_designator2),
transitions={
"succeeded": "MOVE_TO_PLACE",
"failed": "MOVE_TO_PLACE"
})
# Move to the place location
smach.StateMachine.add(
"MOVE_TO_PLACE",
states.NavigateToSymbolic(
robot, {place_furniture_designator1: "in_front_of"},
place_furniture_designator1),
transitions={
'arrived': 'PLACE_ITEM_1',
'unreachable': 'PLACE_ITEM_1',
'goal_not_defined': 'PLACE_ITEM_1'
})
# Place the first item
smach.StateMachine.add("PLACE_ITEM_1",
PlaceSingleItem(
robot=robot,
place_designator=place_designator1),
transitions={
"succeeded": "PLACE_ITEM_2",
"failed": "PLACE_ITEM_2"
})
# Place the second item
smach.StateMachine.add("PLACE_ITEM_2",
PlaceSingleItem(
robot=robot,
place_designator=place_designator1),
transitions={
"succeeded": "MOVE_TO_GRASP_SURFACE3",
"failed": "MOVE_TO_GRASP_SURFACE3"
})
# Move back to the grasp surface to grasp the third item
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE3",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator: "in_front_of"},
grasp_furniture_designator),
transitions={
'arrived': 'GRAB_ITEM_3',
'unreachable': 'MOVE_TO_GRASP_SURFACE4',
'goal_not_defined': 'GRAB_ITEM_3'
})
# Backup for moving back to the grasp location
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE4",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator: "large_in_front_of"},
grasp_furniture_designator),
transitions={
'arrived': 'GRAB_ITEM_3',
'unreachable': 'GRAB_ITEM_3',
'goal_not_defined': 'GRAB_ITEM_3'
})
# Grasp the third item
smach.StateMachine.add("GRAB_ITEM_3",
GrabSingleItem(
robot=robot,
grab_designator=grasp_designator3),
transitions={
"succeeded": "MOVE_TO_PLACE_3",
"failed": "MOVE_TO_PLACE_3"
})
# Move to the place location
smach.StateMachine.add(
"MOVE_TO_PLACE_3",
states.NavigateToSymbolic(
robot, {place_furniture_designator3: "in_front_of"},
place_furniture_designator3),
transitions={
'arrived': 'PLACE_ITEM_3',
'unreachable': 'PLACE_ITEM_3',
'goal_not_defined': 'PLACE_ITEM_3'
})
# Place the first item
smach.StateMachine.add("PLACE_ITEM_3",
PlaceSingleItem(
robot=robot,
place_designator=place_designator3),
transitions={
"succeeded": "succeeded",
"failed": "succeeded"
})
def __init__(self, robot, seats_to_inspect, room, seat_is_for=None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
seats = SeatsInRoomDesignator(robot, seats_to_inspect, room,
"seats_in_room")
seat_ent_des = ds.VariableDesignator(resolve_type=Entity)
if seat_is_for:
ds.check_type(seat_is_for, str)
else:
seat_is_for = ds.Designator(' ')
with self:
smach.StateMachine.add(
'SAY_LETS_FIND_SEAT',
states.SayFormatted(robot, [
"Let me find a place for {name} to sit. Please be patient while I check out where there's place to sit"
],
name=seat_is_for,
block=False),
transitions={'spoken': 'ITERATE_NEXT_SEAT'})
smach.StateMachine.add('ITERATE_NEXT_SEAT',
states.IterateDesignator(
seats, seat_ent_des.writeable),
transitions={
'next': 'CHECK_SEAT_EMPTY',
'stop_iteration': 'SAY_NO_EMPTY_SEATS'
})
smach.StateMachine.add('CHECK_SEAT_EMPTY',
states.CheckVolumeEmpty(
robot, seat_ent_des, 'on_top_of', 0.2),
transitions={
'occupied': 'ITERATE_NEXT_SEAT',
'empty': 'POINT_AT_EMPTY_SEAT',
'partially_occupied':
'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
'failed': 'ITERATE_NEXT_SEAT'
})
smach.StateMachine.add(
'POINT_AT_EMPTY_SEAT',
states.PointAt(robot=robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=seat_ent_des,
look_at_designator=seat_ent_des),
transitions={
"succeeded": "SAY_SEAT_EMPTY",
"failed": "SAY_SEAT_EMPTY"
})
smach.StateMachine.add(
'SAY_SEAT_EMPTY',
states.SayFormatted(robot,
["Please sit on the {seat}, {name}"],
name=seat_is_for,
seat=ds.AttrDesignator(seat_ent_des,
'id',
resolve_type=str),
block=True),
transitions={'spoken': 'RESET_SUCCESS'})
smach.StateMachine.add(
'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
states.PointAt(robot=robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=seat_ent_des,
look_at_designator=seat_ent_des),
transitions={
"succeeded": "SAY_SEAT_PARTIALLY_OCCUPIED",
"failed": "SAY_SEAT_PARTIALLY_OCCUPIED"
})
smach.StateMachine.add(
'SAY_SEAT_PARTIALLY_OCCUPIED',
states.SayFormatted(robot, [
"I think there's some space left here where you can sit {name}"
],
name=seat_is_for,
block=True),
transitions={'spoken': 'RESET_SUCCESS'})
smach.StateMachine.add(
'SAY_NO_EMPTY_SEATS',
states.SayFormatted(robot, [
"Sorry, there are no empty seats. I guess you just have to stand {name}"
],
name=seat_is_for,
block=True),
transitions={'spoken': 'RESET_FAIL'})
smach.StateMachine.add('RESET_FAIL',
states.ResetArms(robot),
transitions={'done': 'failed'})
smach.StateMachine.add('RESET_SUCCESS',
states.ResetArms(robot),
transitions={'done': 'succeeded'})
import rospy
import argparse
# TU/e Robotics
from robot_skills.get_robot import get_robot
from robot_skills.arms import GripperState, GripperTypes
# Robot Smach States
from robot_smach_states.manipulation import SetGripper
import robot_smach_states.util.designators as ds
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Test the gripper state whether it can open and close")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("Set_gripper_state")
robot = get_robot(args.robot)
arm = ds.UnoccupiedArmDesignator(
robot, {"required_gripper_types": [GripperTypes.GRASPING]})
for gripper_state in [GripperState.CLOSE, GripperState.OPEN]:
set_gripper_state = SetGripper(robot, arm, gripperstate=gripper_state)
outcome = set_gripper_state.execute()
rospy.loginfo("Set gripper state to {}, outcome: {}".format(
gripper_state, outcome))
rospy.sleep(2.5)
robot = get_robot(args.robot)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0),
kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
shape = RightPrism([
kdl.Vector(0, 0, 0),
kdl.Vector(0, 0.05, 0),
kdl.Vector(0.05, 0.05, 0),
kdl.Vector(0.05, 0, 0)
], -0.1, 0.1)
item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape,
None, None, None)
item = ds.Designator(item)
arm = ds.UnoccupiedArmDesignator(robot,
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
})
grab_state = Grab(robot, item, arm)
grab_state.execute()
def single_item(robot,
results_writer,
cls,
support,
waypoint,
inspect_from_area=None,
non_strict_class=False,
search_area='on_top_of'):
"""
Benchmark grasping for a single item. Outputs a record dictionary
:param robot: an instance of Robot
:param results_writer: a csv.DictWriter to which the output record is written
:param cls: class/type of item to grab
:param support: ID of the entity supporting the item-to-grab
:param waypoint: From where should the robot start the grasp
:param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection
:param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object
:param search_area: which area of the support-entity to search/inspect for an item of the given class
:return: a dict with the benchmark result
"""
grasp_cls = ds.Designator(cls, name='grasp_cls')
support_entity = ds.EdEntityDesignator(robot,
id=support,
name='support_entity')
entity_ids = ds.VariableDesignator([],
resolve_type=[ClassificationResult],
name='entity_ids')
waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint')
arm = ds.LockingDesignator(
ds.UnoccupiedArmDesignator(robot,
name='unoccupied-arm',
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
}))
arm.lock()
record = {
'robot': robot.robot_name,
'start_waypoint': waypoint,
'expected_class': cls,
'id': None,
'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
}
try:
say_announce = Say(
robot,
sentence="Please put a {cls} {search_area} the {support}".format(
cls=cls, support=support, search_area=search_area))
say_announce.execute()
nav_to_start = NavigateToWaypoint(robot,
waypoint_designator=waypoint_des,
look_at_designator=support_entity)
assert nav_to_start.execute(
) == 'arrived', "I did not arrive at the start"
inspect = Inspect(robot=robot,
entityDes=support_entity,
objectIDsDes=entity_ids,
navigation_area=inspect_from_area,
searchArea=search_area)
record['inspect_start'] = time.time()
assert inspect.execute(
) == 'done', "I could not inspect the support entity"
record['inspect_end'] = time.time()
record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] -
record['inspect_start'])
inspection_result = entity_ids.resolve(
) # type: List[ClassificationResult]
if inspection_result:
if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class:
rospy.loginfo("Any item will match")
matching_results = inspection_result
else:
matching_results = [
result for result in inspection_result
if result.type == grasp_cls.resolve()
]
rospy.loginfo("Found {} items of class {}".format(
len(matching_results), grasp_cls.resolve()))
if matching_results:
if len(matching_results) > 1:
rospy.logwarn(
"There are multiple items OK to grab, will select the last one"
)
record['observed_class'] = ' '.join(
[result.type for result in matching_results])
else:
record['observed_class'] = matching_results[-1].type
selected_entity_id = matching_results[-1].id
rospy.loginfo("Selected entity {} for grasping".format(
selected_entity_id))
grasp_entity = ds.EdEntityDesignator(robot,
id=selected_entity_id)
record['id'] = selected_entity_id[:6]
entity = grasp_entity.resolve() # type: Entity
if entity:
vector_stamped = entity.pose.extractVectorStamped(
) # type: VectorStamped
record['x'] = '{:.3f}'.format(vector_stamped.vector.x())
record['y'] = '{:.3f}'.format(vector_stamped.vector.y())
record['z'] = '{:.3f}'.format(vector_stamped.vector.z())
grab_state = Grab(robot, grasp_entity, arm)
record['grab_start'] = time.time()
assert grab_state.execute() == 'done', "I couldn't grasp"
record['grab_end'] = time.time()
record['grab_duration'] = '{:3.3f}'.format(
record['grab_end'] - record['grab_start'])
assert nav_to_start.execute(
) == 'arrived', "I could not navigate back to the start"
rospy.logwarn("Robot will turn around to drop the {}".format(
grasp_cls.resolve()))
force_drive = ForceDrive(robot, 0, 0, 1,
3.14) # rotate 180 degs in pi seconds
force_drive.execute()
say_drop = Say(
robot,
sentence="I'm going to drop the item, please hold it!")
say_drop.execute()
drop_it = SetGripper(robot,
arm_designator=arm,
grab_entity_designator=grasp_entity)
drop_it.execute()
force_drive_back = ForceDrive(
robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds
force_drive_back.execute()
nav_to_start.execute()
else:
raise AssertionError("No {} found".format(grasp_cls.resolve()))
else:
rospy.logerr("No entities found at all :-(")
except AssertionError as assertion_err:
say_fail = Say(robot, sentence=assertion_err.message + ", sorry")
say_fail.execute()
finally:
results_writer.writerow(record)
return record
def setup_statemachine(robot):
place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name")
place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot,
place_name,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked
# For this challenge, unlocking is not needed
bag_arm_designator = empty_arm_designator.lockable()
bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
current_item = ds.VariableDesignator(Entity("dummy",
"dummy",
"/{}/base_link".format(robot.robot_name),
kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5),
None,
{},
[],
datetime.datetime.now()),
name="current_item")
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(robot, challenge_knowledge.starting_point),
transitions={'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'})
# TODO: learn operator state needs to be added before follow
# smach.StateMachine.add('WAIT_TO_FOLLOW',
# WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']),
# transitions={'success': 'FOLLOW_OPERATOR',
# 'abort': 'Aborted'})
smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER',
states.Say(robot, ["Are we at the car or should I follow you?"], block=True),
transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER',
WaitForOperatorCommand(robot,
possible_commands=[
"follow",
'follow me',
"here is the car",
"stop following",
"stop following me",
],
commands_as_outcomes=True),
transitions={'follow': 'FOLLOW_OPERATOR',
'follow me': 'FOLLOW_OPERATOR',
'here is the car': 'REMEMBER_CAR_LOCATION',
'stop following': 'REMEMBER_CAR_LOCATION',
'stop following me': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={'stopped': 'ASK_FOLLOW_OR_REMEMBER',
'lost_operator': 'ASK_FOLLOW_OR_REMEMBER',
'no_operator': 'ASK_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
StoreCarWaypoint(robot),
transitions={'success': 'ASK_DESTINATION',
'abort': 'Aborted'})
smach.StateMachine.add('ASK_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"], block=True),
transitions={'spoken': 'WAIT_FOR_DESTINATION'})
smach.StateMachine.add('WAIT_FOR_DESTINATION',
WaitForOperatorCommand(robot,
possible_commands=challenge_knowledge.waypoints.keys(),
commands_as_userdata=True),
transitions={'success': 'GRAB_ITEM',
'abort': 'Aborted'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add('GRAB_ITEM',
GrabItem(robot, bag_arm_designator, current_item),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes.
'failed': 'BACKUP_CLOSE_GRIPPER'},
remapping={'target_room_in': 'command_recognized',
'target_room_out': 'target_room'})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(robot, bag_arm_designator, 'driving_bag_pose'),
transitions={'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add('SAY_GOING_TO_ROOM',
states.Say(robot, ["Let me bring in your groceries",
"Helping you carry stuff",
"I'm going back inside"],
block=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add('GOTO_DESTINATION',
NavigateToRoom(robot),
transitions={'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
DropBagOnGround(robot, bag_arm_designator),
transitions={'succeeded': 'ASKING_FOR_HELP',
'failed': 'ASKING_FOR_HELP'})
smach.StateMachine.add('ASKING_FOR_HELP',
#TODO: look and then face new operator
states.Say(robot, "Please follow me and help me carry groceries into the house"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('GOTO_CAR',
states.NavigateToWaypoint(robot,
ds.EntityByIdDesignator(robot,
id=challenge_knowledge.waypoint_car['id']),
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'})
smach.StateMachine.add('OPEN_DOOR',
#TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('AT_END',
states.Say(robot, ["We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"],
block=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "help_me_carry")
return sm
import robot_smach_states.util.designators as ds
from robot_smach_states.manipulation import HandoverFromHuman, HandoverToHuman
from robot_smach_states import LockDesignator
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Test handover to human state")
parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_handover_stuff")
robot = get_robot(args.robot)
rospy.loginfo("Creating arm designator")
arm_designator = ds.UnoccupiedArmDesignator(robot=robot,
arm_properties={"required_goals": ["handover_to_human", "reset"],
"required_gripper_types": [arms.GripperTypes.GRASPING]},
name='arm_des').lockable()
rospy.loginfo("Creating lock designator state")
s0 = LockDesignator(arm_designator)
rospy.loginfo("Creating handover from human state")
s1 = HandoverFromHuman(robot, arm_designator, grabbed_entity_label="foo")
rospy.loginfo("Creating handover to human state")
s2 = HandoverToHuman(robot, arm_designator)
rospy.loginfo("Executing lock designator state")
s0.execute()
rospy.loginfo("Executing handover from human state")
def __init__(self, robot):
""" Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(
self, outcomes=["succeeded", "failed", "aborted"])
# Create designators
trashbin_designator = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id,
name='trashbin_designator')
# Look if there is a second trash bin present
# trashbin_designator2 = None
if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"):
trashbin_designator2 = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator2')
next_state = "HELPER_WAYPOINT"
rospy.loginfo("There is a second trash bin")
else:
rospy.loginfo("There is no second trash bin")
next_state = "ANNOUNCE_END"
# drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id)
helper_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint)
end_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint)
arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
with self:
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
CHALLENGE_KNOWLEDGE.starting_point),
transitions={
"Done": "SAY_START_CHALLENGE",
"Aborted": "SAY_START_CHALLENGE",
"Failed": "SAY_START_CHALLENGE"
})
smach.StateMachine.add("SAY_START_CHALLENGE",
states.Say(
robot,
"I will start cleaning up the trash",
block=True),
transitions={'spoken': "PICK_UP_TRASH"})
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(
robot=robot,
trashbin_designator=trashbin_designator,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH",
"failed": "HELPER_WAYPOINT",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH",
DropDownTrash(robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK",
states.Say(robot,
"First bag has been dropped at the collection zone",
block=False),
transitions={'spoken': next_state})
if next_state == "HELPER_WAYPOINT":
smach.StateMachine.add(
"HELPER_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=helper_waypoint_designator),
transitions={
"arrived": "PICK_UP_TRASH2",
"goal_not_defined": "PICK_UP_TRASH2",
"unreachable": "PICK_UP_TRASH2"
})
smach.StateMachine.add(
"PICK_UP_TRASH2",
PickUpTrash(robot=robot,
trashbin_designator=trashbin_designator2,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH2",
"failed": "ANNOUNCE_END",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH2",
DropDownTrash(
robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK2",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK2",
states.Say(
robot,
"Second bag has been dropped at the collection zone."
"All the thrash has been taken care of",
block=False),
transitions={'spoken': 'ANNOUNCE_END'})
smach.StateMachine.add("ANNOUNCE_END",
states.Say(
robot,
"I have finished taking out the trash.",
block=False),
transitions={'spoken': 'NAVIGATE_OUT'})
smach.StateMachine.add(
"NAVIGATE_OUT",
states.NavigateToWaypoint(
robot=robot, waypoint_designator=end_waypoint_designator),
transitions={
"arrived": "succeeded",
"goal_not_defined": "succeeded",
"unreachable": "succeeded"
})
from robot_smach_states import LockDesignator
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Test handover to human state")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_handover_stuff")
robot = get_robot(args.robot)
rospy.loginfo("Creating arm designator")
arm_designator = ds.UnoccupiedArmDesignator(robot=robot,
arm_properties={},
name='arm_des').lockable()
rospy.loginfo("Creating lock designator state")
s0 = LockDesignator(arm_designator)
rospy.loginfo("Creating handover from human state")
s1 = HandoverFromHuman(robot, arm_designator, grabbed_entity_label="foo")
rospy.loginfo("Creating handover to human state")
s2 = HandoverToHuman(robot, arm_designator)
rospy.loginfo("Executing lock designator state")
s0.execute()
rospy.loginfo("Executing handover from human state")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(
robot,
self.target_destination,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
'preempted': 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
# smach.StateMachine.add('FOLLOW_OPERATOR',
# states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True),
# transitions={'stopped': 'ASK_FOR_TASK',
# 'lost_operator': 'ASK_FOR_TASK',
# 'no_operator': 'FOLLOW_OPERATOR'})
# Use NEW:
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator2(robot),
transitions={
'Done': 'ASK_FOR_TASK',
'Failed': 'ASK_FOR_TASK',
'Aborted': 'FOLLOW_OPERATOR'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add('WAIT_FOR_TASK',
states.HearOptions(robot, ['yes', 'no']),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'CONFIRM_CAR_LOCATION',
'no_result': 'ASK_FOR_TASK'
})
smach.StateMachine.add(
'CONFIRM_CAR_LOCATION',
states.Say(
robot,
["OK, I will remember this location as the car location."],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'REMEMBER_CAR_LOCATION'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
#
# smach.StateMachine.add('CONFIRM_DESTINATION',
# states.Say(robot, [
# "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)],
# block=True,
# look_at_standing_person=True),
# transitions={'spoken': 'GRAB_ITEM'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
# states.HandoverFromHuman(robot, self.bag_arm_designator, "current_item",
# self.current_item,
# arm_configuration=challenge_knowledge.carrying_bag_pose),
# transitions={'succeeded': 'ARM_DRIVING_POSE',
# 'timeout': 'BACKUP_CLOSE_GRIPPER',
# # For now in simulation timeout is considered a success.
# 'failed': 'BACKUP_CLOSE_GRIPPER'})
states.Say(robot, [
"I can't pick up the groceries since I don't have arms. Please place them in my basket."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'})
smach.StateMachine.add('WAIT_FOR_GRAB_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'SAY_GOING_TO_ROOM',
'preempted': 'Aborted'
})
# smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
# states.SetGripper(robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE),
# transitions={'succeeded': 'ARM_DRIVING_POSE',
# 'failed': 'ARM_DRIVING_POSE'})
#
# smach.StateMachine.add('ARM_DRIVING_POSE',
# states.ArmToJointConfig(robot, self.bag_arm_designator,
# challenge_knowledge.driving_bag_pose),
# transitions={'succeeded': 'SAY_GOING_TO_ROOM',
# 'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'TURN_180_TO_REPLAN',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'TURN_180_TO_REPLAN',
hmc_states.TurnToReplan(robot),
transitions={
'success': 'GOTO_DESTINATION_BACKUP',
'abort': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
#'goal_not_defined': 'Aborted'})
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add(
'PUTDOWN_ITEM',
# hmc_states.DropBagOnGround(robot, self.bag_arm_designator,
# challenge_knowledge.drop_bag_pose),
states.Say(robot, [
"I can't put the groceries down since I have no arms. Please take them from my basket and put it down."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'})
smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'ASKING_FOR_HELP',
'preempted': 'Aborted'
})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'}) #'LEARN_OPERATOR'})
# smach.StateMachine.add('LEARN_OPERATOR',
# hmc_states.LearnOperator(robot),
# transitions={'learned': 'GOTO_CAR',
# 'failed': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
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"})
