def __init__(self, robot): smach.StateMachine.__init__( self, outcomes=['succeeded', 'failed', 'aborted']) self.door_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_door['id']) self.livingroom_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_livingroom['id']) self.operator_designator = ds.VariableDesignator( resolve_type=Entity) self.guest1_entity_des = ds.VariableDesignator( resolve_type=Entity, name='guest1_entity') self.guest1_name_des = ds.VariableDesignator('guest 1', name='guest1_name') self.guest1_drink_des = ds.VariableDesignator( resolve_type=HMIResult, name='guest1_drink') self.guest1_drinkname_des = ds.FieldOfHMIResult( self.guest1_drink_des, semantics_field='drink', name='guest1_drinkname') with self: smach.StateMachine.add('LEARN_GUEST', LearnGuest(robot, self.door_waypoint, self.guest1_entity_des, self.guest1_name_des, self.guest1_drink_des), transitions={ 'succeeded': 'succeeded', 'failed': 'failed', 'aborted': 'aborted' })
def __init__(self, robot, area, name): """ Constructor :param robot: robot object :param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate to this waypoint. Else, it will navigate to the room called "<area>" :param name: (str) Name of the person to look for """ smach.StateMachine.__init__(self, outcomes=["found", "not_found"]) waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=area + "_waypoint") room_designator = ds.EntityByIdDesignator(robot=robot, id=area) with self: smach.StateMachine.add("DECIDE_NAVIGATE_STATE", _DecideNavigateState( robot=robot, waypoint_designator=waypoint_designator, room_designator=room_designator), transitions={ "waypoint": "NAVIGATE_TO_WAYPOINT", "room": "NAVIGATE_TO_ROOM", "none": "not_found" }) smach.StateMachine.add("NAVIGATE_TO_WAYPOINT", states.NavigateToWaypoint( robot=robot, waypoint_designator=waypoint_designator, radius=0.15), transitions={ "arrived": "FIND_PERSON", "unreachable": "not_found", "goal_not_defined": "not_found" }) smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom( robot=robot, entity_designator_room=room_designator), transitions={ "arrived": "FIND_PERSON", "unreachable": "not_found", "goal_not_defined": "not_found" }) # Wait for the operator to appear and detect what he's pointing at smach.StateMachine.add("FIND_PERSON", FindPerson(robot=robot, person_label=name), transitions={ "found": "found", "failed": "not_found" })
def __init__(self, robot, room, found_people_designator, look_range=(-np.pi/2, np.pi/2), look_steps=8): """ Constructor :param robot: robot object :param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate to this waypoint. Else, it will navigate to the room called "<area>" :param name: (str) Name of the person to look for :param discard_other_labels: (bool) Whether or not to discard faces based on label :param found_person_designator: (Designator) A designator that will resolve to the found object """ smach.StateMachine.__init__(self, outcomes=["found", "not_found"]) waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=room + "_waypoint") room_designator = ds.EntityByIdDesignator(robot=robot, id=room) ds.check_type(found_people_designator, [Entity]) ds.is_writeable(found_people_designator) with self: smach.StateMachine.add("DECIDE_NAVIGATE_STATE", _DecideNavigateState(robot=robot, waypoint_designator=waypoint_designator, room_designator=room_designator), transitions={"waypoint": "NAVIGATE_TO_WAYPOINT", "room": "NAVIGATE_TO_ROOM", "none": "not_found"}) smach.StateMachine.add("NAVIGATE_TO_WAYPOINT", states.NavigateToWaypoint(robot=robot, waypoint_designator=waypoint_designator, radius=0.15), transitions={"arrived": "FIND_PEOPLE", "unreachable": "not_found", "goal_not_defined": "not_found"}) smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "FIND_PEOPLE", "unreachable": "not_found", "goal_not_defined": "not_found"}) # Wait for the operator to appear and detect what he's pointing at smach.StateMachine.add("FIND_PEOPLE", FindPeople(robot=robot, query_entity_designator=room_designator, found_people_designator=found_people_designator, speak=True, look_range=look_range, look_steps=look_steps), transitions={"found": "found", "failed": "not_found"})
def __init__(self, robot, cabinet_id, cabinet_navigate_area, cabinet_inspect_area): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) self.cabinet = ds.EntityByIdDesignator(robot=robot, id=cabinet_id) with self: smach.StateMachine.add("NAVIGATE_TO_CABINET", states.NavigateToSymbolic( robot, {self.cabinet: cabinet_navigate_area}, self.cabinet), transitions={ 'arrived': 'UPDATE_CABINET_POSE', 'unreachable': 'failed', 'goal_not_defined': 'failed' }) smach.StateMachine.add("UPDATE_CABINET_POSE", UpdateCabinetPose(robot, self.cabinet, cabinet_inspect_area), transitions={ 'succeeded': 'OPEN_DOOR', 'failed': 'failed' }) smach.StateMachine.add("OPEN_DOOR", OpenDoor(robot, self.cabinet), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
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 arm_left_grab(pre): global arm_left_object_in_arm global task_object_grabbed global left_grab_exec_object_in_WS classification_result_designator = ds.VariableDesignator([], resolve_type=[ClassificationResult]) # Inspect the waypoint and classify objects on it segmSM = SegmentObjects(robot, classification_result_designator.writeable, wp) status = segmSM.execute() grab_obj = ds.VariableDesignator(resolve_type=Entity) # take the first seen coke for obj in classification_result_designator.resolve(): if obj.type == 'coke': grab_obj = ds.EntityByIdDesignator(robot, type=obj.id) break grabSM = Grab(robot, grab_obj, leftarm) status = grabSM.execute() if status == 'done': arm_left_object_in_arm = True task_object_grabbed = True # left_grab_exec_object_in_WS = 0 else: arm_left_object_in_arm = False # RETRY task_object_grabbed = False Amigo_AssignInputVariables()
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted']) runs = ds.Designator([0, 1]) run = ds.VariableDesignator(resolve_type=int) 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': 'HANDLE_GUEST_1', "preempted": 'aborted', 'error': 'HANDLE_GUEST_1' }) smach.StateMachine.add('HANDLE_GUEST_1', HandleSingleGuest(robot, assume_john=True), transitions={ 'succeeded': 'HANDLE_GUEST_2', 'aborted': 'HANDLE_GUEST_2' }) smach.StateMachine.add('HANDLE_GUEST_2', HandleSingleGuest(robot, assume_john=False), transitions={ 'succeeded': 'SAY_DONE', 'aborted': 'SAY_DONE' }) smach.StateMachine.add( 'SAY_DONE', states.Say(robot, ["That's all folks, my job is done, bye bye!"], block=False), transitions={'spoken': 'GO_BACK'}) smach.StateMachine.add( 'GO_BACK', states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_door['id']), challenge_knowledge.waypoint_door['radius']), transitions={ 'arrived': 'succeeded', 'unreachable': 'succeeded', 'goal_not_defined': 'succeeded' })
def setup_statemachine(robot): entity = ds.EntityByIdDesignator(robot, id='hallway_couch') sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: smach.StateMachine.add('TEST', LookAtEntity(robot, entity), transitions={'succeeded': 'Done'}) return sm
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): target_waypoint = challenge_knowledge.waypoints[userdata.target_room]['id'] target_radius = challenge_knowledge.waypoints[userdata.target_room]['radius'] navigateToWaypoint = states.NavigateToWaypoint(self._robot, ds.EntityByIdDesignator(self._robot, id=target_waypoint), target_radius) return navigateToWaypoint.execute()
def move(p, robot): entities = parse_object(p, robot) if not entities: say("No such entities!") return machine = NavigateToObserve(robot, entity_designator=ds.EntityByIdDesignator(robot, id=entities[0].id), radius=.5) stop() global ACTION ACTION = SmachAction(machine) ACTION.start()
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done']) with sm: # Start challenge via StartChallengeRobust, skipped atm smach.StateMachine.add( "RAYTRACEDEMO", RayTraceSelector(robot, waypoint="final_challenge", furniture_designator=ds.EntityByIdDesignator( robot, id="temp")), transitions={ outcome: "Done" for outcome in ["waypoint", "furniture", "grasp", "done"] }) return sm
def __init__(self, robot): """ Drives to the lightsaber pose and starts the lightsaber state. """ smach.StateMachine.__init__(self, outcomes=["done"]) with self: smach.StateMachine.add( "NAVIGATE_TO_START", states.NavigateToWaypoint( robot=robot, waypoint_designator=ds.EntityByIdDesignator( robot, id=LIGHTSABER_WAYPOINT_ID), ), transitions={ "arrived": "SWORDFIGHT", "unreachable": "SWORDFIGHT", # Just take it from here "goal_not_defined": "SWORDFIGHT" }) # Just take it from here smach.StateMachine.add("SWORDFIGHT", LightSaber(robot), transitions={"done": "done"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") pdf_writer = WritePdf(robot=robot) with self: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SAY_UNABLE_TO_OPEN_DOOR', 'abort': 'Aborted' }) smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR', states.Say( robot, "I am unable to open the shelf door, " "can you please open it for me?"), transitions={'spoken': 'AWAIT_START'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None, manipulate_machine=None): """ Moves the entities for this challenge to the correct poses""" # Determine where to perform the challenge # Apparently, the indices here come from: # (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area) robot_pose = robot.base.get_location() WORKSPACES.sort( key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p - robot_pose.frame.p).Norm()) closest_workspace = WORKSPACES[0] rospy.loginfo( "Closest workspace: grasp from '{grasp}' and place on '{place}'" .format( grasp=closest_workspace.grasp_entity_conf.entity_id, place=closest_workspace.place_entity_conf.entity_id)) cabinet_id = closest_workspace.place_entity_conf.entity_id table_id = closest_workspace.grasp_entity_conf.entity_id # Update the world model by fitting the entities to the frame_stamped's given below. robot.ed.update_entity(id=cabinet_id, frame_stamped=closest_workspace. place_entity_conf.pose_estimate) robot.ed.update_entity(id=table_id, frame_stamped=closest_workspace. grasp_entity_conf.pose_estimate) # Update designators cabinet.id_ = closest_workspace.place_entity_conf.entity_id # Update manipulate machine manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[ 0] manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table, cb_args=[single_item]), transitions={'done': 'NAV_TO_START'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'INSPECT_SHELVES', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, cabinet), transitions={ 'succeeded': 'WRITE_PDF_SHELVES', 'nothing_found': 'WRITE_PDF_SHELVES', 'failed': 'WRITE_PDF_SHELVES' }) smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded') with range_iterator: smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "manipulation")
# TU/e Robotics from robot_smach_states.human_interaction import give_directions import robot_smach_states.util.designators as ds from robot_skills.get_robot import get_robot if __name__ == "__main__": parser = argparse.ArgumentParser( description= "Starting from the current robot position and orientation give " "directions to the desired target object") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") parser.add_argument("target_entity_id", type=str, help="Entity id of the target object") args = parser.parse_args() # Create node rospy.init_node("test_give_directions") robot = get_robot(args.robot) e_id = args.target_entity_id # Instantiate GuideToSymbolic machine state = give_directions.GiveDirections(r, ds.EntityByIdDesignator(r, id=e_id)) # Execute the state state.execute()
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") pdf_writer = WritePdf(robot=robot) with self: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SAY_UNABLE_TO_OPEN_DOOR', 'abort': 'Aborted' }) smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR', states.Say( robot, "I am unable to open the shelf door, " "can you please open it for me?"), transitions={'spoken': 'AWAIT_START'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None, manipulate_machine=None): """ Moves the entities for this challenge to the correct poses""" # Determine where to perform the challenge robot_pose = robot.base.get_location() ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm()) cabinet_id = ENTITY_POSES[0][2] table_id = ENTITY_POSES[0][3] # Update the world model robot.ed.update_entity(id="balcony_shelf", frame_stamped=FrameStamped( kdl.Frame(kdl.Rotation(), kdl.Vector(0.0, 3.0, 0.0)), frame_id="map")) robot.ed.update_entity(id=cabinet_id, frame_stamped=ENTITY_POSES[0][0]) robot.ed.update_entity(id=table_id, frame_stamped=ENTITY_POSES[0][1]) # Update designators cabinet.id_ = ENTITY_POSES[0][2] room.id_ = ENTITY_POSES[0][4] # Update manipulate machine manipulate_machine.place_entity_designator.id_ = cabinet_id manipulate_machine.place_designator._area = ENTITY_POSES[0][5] manipulate_machine.place_designator.place_location_designator.id = cabinet_id manipulate_machine.table_designator.id_ = table_id manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2] return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table, cb_args=[single_item]), transitions={'done': 'NAV_TO_START'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'INSPECT_SHELVES', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, cabinet), transitions={ 'succeeded': 'WRITE_PDF_SHELVES', 'nothing_found': 'WRITE_PDF_SHELVES', 'failed': 'WRITE_PDF_SHELVES' }) smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded') with range_iterator: smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "manipulation")
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 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
from robot_smach_states.manipulation.place import EmptySpotDesignator import robot_smach_states.util.designators as ds from robot_skills import get_robot if __name__ == "__main__": rospy.init_node("testdesignator") parser = argparse.ArgumentParser( description="Test the empty spot designator") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() robot = get_robot(args.robot) furniture_designator = ds.EntityByIdDesignator(robot, id="dinner_table") arm_designator = ds.UnoccupiedArmDesignator(robot, {}) def with_area(): esd = EmptySpotDesignator( robot=robot, place_location_designator=furniture_designator, arm_designator=arm_designator, name="with_area", area="on_top_of") print(esd.resolve()) def without_area(): esd = EmptySpotDesignator( robot=robot, place_location_designator=furniture_designator,
robot_name = sys.argv[1] if robot_name == 'amigo': from robot_skills.amigo import Amigo as Robot elif robot_name == 'sergio': from robot_skills.sergio import Sergio as Robot elif robot_name == 'hero': from robot_skills.hero import Hero as Robot else: rospy.logerr("unknown robot") sys.exit() robot = Robot() if len(sys.argv) > 2: furniture_id = sys.argv[2] else: furniture_id = "bed" rospy.loginfo("Waiting for tf cache to be filled") rospy.sleep(2) # wait for tf cache to be filled rospy.loginfo("Testing the 'get_room' method") _test_rooms(robot) rospy.loginfo("Starting giving directions to {}".format(furniture_id)) state = GiveDirections(robot=robot, entity_designator=ds.EntityByIdDesignator( robot=robot, id=furniture_id)) state.execute()
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") placed_items = [] with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add("INIT_WM", InitializeWorldModel(robot), transitions={'done': 'AWAIT_START'}) # smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR", # states.Say(robot, ["Hi there, I will now wait until you remove the cup", # "I'm waiting for you to remove the cup"], block=False), # transitions={"spoken": "WAIT_FOR_DOOR"}) # # smach.StateMachine.add("WAIT_FOR_DOOR", # states.WaitForDoorOpen(robot, timeout=10), # transitions={"closed": "DOOR_CLOSED", # "open": "AWAIT_START"}) # # smach.StateMachine.add("DOOR_CLOSED", # states.Say(robot, ["I am waiting for you to remove the cup", # "I'd start, if you remove the cup from my laser"]), # transitions={"spoken": "WAIT_FOR_DOOR"}) if USE_SLAM: drive_state = "RESET_ED_SLAM" else: drive_state = "NAV_TO_START" smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': drive_state, 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) if USE_SLAM: # vth = 1.0 # smach.StateMachine.add("NAV_TO_FIT_POSE", # ForceDrive(robot, 0, 0, vth, 3.14/vth), # transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("RESET_ED_SLAM", states.ResetED(robot), transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("FIT_ENTITY", FitEntity(robot, CABINET), transitions={ 'succeeded': 'NAV_TO_START', 'failed': 'SAY_FITTING_FAILED' }) smach.StateMachine.add( "SAY_FITTING_FAILED", states.Say(robot, [ "Fitting the {0} failed, I will stop now.".format(CABINET) ], mood="sad"), transitions={'spoken': 'Aborted'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'FORCE_ROTATE', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("FORCE_ROTATE", ForceRotate(robot, 0.5, 2.0, 30.0), transitions={ 'done': "NAV_TO_START", 'timedout': "INSPECT_SHELVES" }) # smach.StateMachine.add("RESET_ED", # states.ResetED(robot), # transitions={'done' :'INSPECT_SHELVES'}) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, OBJECT_SHELVES), transitions={ 'succeeded': 'EXPORT_PDF', 'nothing_found': 'EXPORT_PDF', 'failed': 'EXPORT_PDF' }) @smach.cb_interface(outcomes=["exported"]) def export_to_pdf(userdata=None): global DETECTED_OBJECTS_WITH_PROBS entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS] # Export images (Only best MAX_NUM_ENTITIES_IN_PDF) # pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge") return "exported" smach.StateMachine.add('EXPORT_PDF', smach.CBState(export_to_pdf), transitions={'exported': 'RANGE_ITERATOR'}) # Begin setup iterator range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], #Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded' ) #The exhausted argument should be set to the preffered state machine outcome with range_iterator: single_item = ManipRecogSingleItem( robot, ds.VariableDesignator(placed_items, [Entity], name="placed_items")) smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(sm, "manipulation") return sm
parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") parser.add_argument( "entity", help="Entity name of the object to guide to, for example dinner_table") args = parser.parse_args() # Create node, robot and toggle interface rospy.init_node("test_guidance") r = get_robot(args.robot) e_id = args.entity # Instantiate GuideToSymbolic machine s = guidance.GuideToSymbolic( r, {ds.EntityByIdDesignator(r, id=e_id): "in_front_of"}, ds.EntityByIdDesignator(r, id=e_id)) # In simulation, mock the _check_operator method if os.getenv("ROBOT_REAL", "false").lower() != "true": # noinspection PyUnusedLocal def _mock_detect_operator(robot, distance=1.0, radius=0.5): return OPERATOR_AVAILABLE guidance._detect_operator_behind_robot = _mock_detect_operator rospy.Service("toggle_operator", std_srvs.srv.Empty, toggle_operator) rospy.loginfo( '\n\nCall:\nrosservice call /toggle_operator "{}"\n\nto toggle operator availability\n' .format('{}')) # Execute the state
def __init__(self, robot, room_id=ROOM_ID): """ Finds the people in the living room and takes pictures of them. Put then in a data struct and put this in output keys. Output keys: * detected_people: same datastruct as was used in find my mates. Ask Rein for a pickled example :param robot: (Robot) api object :param room_id: (str) identifies the room in which the people are detected """ smach.StateMachine.__init__(self, outcomes=["done"], output_keys=["detected_people"]) with self: # Move to start location smach.StateMachine.add("NAVIGATE_TO_START", states.NavigateToWaypoint( robot=robot, waypoint_designator=ds.EntityByIdDesignator(robot, WAYPOINT_ID), radius=0.3), transitions={"arrived": "DETECT_PEOPLE", "unreachable": "DETECT_PEOPLE", "goal_not_defined": "DETECT_PEOPLE"}) @smach.cb_interface(outcomes=["done"], output_keys=["raw_detections"]) def detect_people(user_data): person_detections = [] #look_angles = np.linspace(-np.pi / 2, np.pi / 2, 8) # From -pi/2 to +pi/2 to scan 180 degrees wide look_angles = np.linspace(-np.pi / 4, np.pi / 4, 4) # From -pi/2 to +pi/2 to scan 180 degrees wide head_goals = [kdl_conversions.VectorStamped(x=100 * math.cos(angle), y=100 * math.sin(angle), z=1.5, frame_id="/%s/base_link" % robot.robot_name) for angle in look_angles] sentences = deque([ "Hi there mates, where are you, please look at me!", "I am looking for my mates! Dippi dee doo! Pew pew!", "You are all looking great today! Keep looking at my camera.", "I like it when everybody is staring at me!" ]) look_at_me_sentences = deque([ "Please look at me", "Let me check you out", "Your eyes are beautiful", "Try to look pretty, your face will pop up on the screen later!" ]) for _ in range(NUM_LOOKS): sentences.rotate(1) robot.speech.speak(sentences[0], block=False) for head_goal in head_goals: look_at_me_sentences.rotate(1) robot.speech.speak(look_at_me_sentences[0], block=False) robot.head.look_at_point(head_goal) robot.head.wait_for_motion_done() now = time.time() rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo() if rgb: try: persons = robot.perception.detect_person_3d(rgb, depth, depth_info) except Exception as e: rospy.logerr(e) rospy.sleep(2.0) else: for person in persons: if person.face.roi.width > 0 and person.face.roi.height > 0: try: person_detections.append({ "map_ps": robot.tf_listener.transformPoint("map", PointStamped( header=rgb.header, point=person.position )), "person_detection": person, "rgb": rgb }) except Exception as e: rospy.logerr( "Failed to transform valid person detection to map frame: {}".format(e)) rospy.loginfo("Took %.2f, we have %d person detections now", time.time() - now, len(person_detections)) rospy.loginfo("Detected %d persons", len(person_detections)) user_data.raw_detections = person_detections return 'done' smach.StateMachine.add('DETECT_PEOPLE', smach.CBState(detect_people), transitions={"done": "FILTER_AND_CLUSTER"}) # Filter and cluster images @smach.cb_interface( outcomes=["done", "failed"], input_keys=["raw_detections"], output_keys=["detected_people"]) def filter_and_cluster_images(user_data): """ Filters the raw detections so that only people within the room maintain. Next, it clusters the images so that only one image per person remains. This is stored in the user data :param user_data: (smach.UserData) :return: (str) Done """ try: user_data.detected_people = _filter_and_cluster_images( robot, user_data.raw_detections, room_id) return "done" except: return "failed" smach.StateMachine.add('FILTER_AND_CLUSTER', smach.CBState(filter_and_cluster_images), transitions={"done": "done", "failed": "done"}) # ToDo: fallback
rospy.init_node('supervisor_wrapper', log_level=rospy.DEBUG) robot = robot_constructor("amigo") objects_to_grab = ["coke"] objects_to_navigate = ["hallway_table", "dinner_table"] objects_to_navigate_ds = [] robot.base.set_initial_pose(0.2, 5, -1.4) # not nescessary in demo extern mode leftarm = Designator(robot.leftArm) rightarm = Designator(robot.rightArm) objects_to_grab.reverse() # Reverse here, because we are going to use list.pop() objects_to_navigate.reverse() # Reverse here, because we are going to use list.pop() for item in objects_to_navigate: objects_to_navigate_ds.append(ds.EntityByIdDesignator(robot, id=item)) leftarm.resolve().reset() rightarm.resolve().reset() leftarm.resolve().send_gripper_goal('close') rightarm.resolve().send_gripper_goal('close') raw_input('Press enter to continue: ') robot.base.set_initial_pose(0.2, 5, -1.4) # not nescessary in demo extern mode lib.Amigo_EngineFirstStep() while not rospy.is_shutdown(): lib.Amigo_EngineTimeStep(0.5) rospy.sleep(0.5) reset_variables()
description="Test if it can segment objects") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") parser.add_argument("entity", default="dinner_table", help="Name of the entity on which to segment objects") args = parser.parse_args() rospy.init_node("example_segment_objects") robot = get_robot(args.robot) location_name = args.entity rospy.loginfo("Creating location designator") location_designator = ds.EntityByIdDesignator(robot, location_name) rospy.loginfo("Creating found entity designator") segmented_entities_designator = ds.VariableDesignator( [], resolve_type=[ClassificationResult]) rospy.loginfo("Creating segment objects state") sm = SegmentObjects(robot, segmented_entities_designator.writeable, location_designator, "on_top_of") rospy.loginfo("Executing segment objects state") sm.execute() rospy.loginfo("Resulting classification result: \n {}".format( segmented_entities_designator.resolve()))
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, grasp_designator1, grasp_designator2, grasp_designator3, grasp_furniture_id1, grasp_furniture_id3, place_furniture_id): """ Constructor :param robot: robot object :param grasp_designator1: EdEntityDesignator designating the first item to grab. :param grasp_designator2: EdEntityDesignator designating the second item to grab. :param grasp_designator3: EdEntityDesignator designating the third item to grab. :param grasp_furniture_id1: string identifying the location where to grasp objects 1 and 2 :param grasp_furniture_id3: string identifying the location where to grasp object 3 :param place_furniture_id: string identifying the location where to place the objects """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators grasp_furniture_designator1 = ds.EntityByIdDesignator( robot, id=grasp_furniture_id1) grasp_furniture_designator3 = ds.EntityByIdDesignator( robot, id=grasp_furniture_id3) place_furniture_designator = ds.EntityByIdDesignator( robot, id=place_furniture_id) place_designator = ds.EmptySpotDesignator( robot=robot, place_location_designator=place_furniture_designator, area="on_top_of") with self: # Move to the inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE1", states.NavigateToSymbolic( robot, {grasp_furniture_designator1: "in_front_of"}, grasp_furniture_designator1), 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_designator1: "large_in_front_of"}, grasp_furniture_designator1), 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_designator1, 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_designator: "in_front_of"}, place_furniture_designator), 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_designator), 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_designator), 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_designator3: "in_front_of"}, grasp_furniture_designator3), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE2', 'unreachable': 'MOVE_TO_GRASP_SURFACE4', 'goal_not_defined': 'INSPECT_GRASP_SURFACE2' }) # Backup for moving back to the grasp location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE4", states.NavigateToSymbolic( robot, {grasp_furniture_designator3: "large_in_front_of"}, grasp_furniture_designator3), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE2', 'unreachable': 'INSPECT_GRASP_SURFACE2', 'goal_not_defined': 'INSPECT_GRASP_SURFACE2' }) # Inspect grasp furniture smach.StateMachine.add("INSPECT_GRASP_SURFACE2", states.Inspect( robot=robot, entityDes=grasp_furniture_designator3, objectIDsDes=None, searchArea="shelf2", navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_3", "failed": "failed" }) # 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_designator: "in_front_of"}, place_furniture_designator), 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_designator), transitions={ "succeeded": "succeeded", "failed": "succeeded" })
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, grab_designator_1=None, grab_designator_2=None, place_designator=None, pdf_writer=None): """ Constructor :param robot: robot object :param grab_designator_1: EdEntityDesignator designating the item to grab :param grab_designator_2: EdEntityDesignator designating the item to grab :param pdf_writer: WritePDF object to save images of recognized objects to pdf files """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators self._table_designator = ds.EntityByIdDesignator(robot, id=TABLE) if grab_designator_1 is None: grab_designator_1 = DefaultGrabDesignator( robot=robot, surface_designator=self._table_designator, area_description=GRAB_SURFACE) if grab_designator_2 is None: grab_designator_2 = DefaultGrabDesignator( robot=robot, surface_designator=self._table_designator, area_description=GRAB_SURFACE) with self: smach.StateMachine.add("MOVE_TO_TABLE1", states.NavigateToSymbolic( robot, {self._table_designator: "in_front_of"}, self._table_designator), transitions={ 'arrived': 'INSPECT_TABLE', 'unreachable': 'MOVE_TO_TABLE2', 'goal_not_defined': 'INSPECT_TABLE' }) smach.StateMachine.add( "MOVE_TO_TABLE2", states.NavigateToSymbolic( robot, {self._table_designator: "large_in_front_of"}, self._table_designator), transitions={ 'arrived': 'INSPECT_TABLE', 'unreachable': 'INSPECT_TABLE', 'goal_not_defined': 'INSPECT_TABLE' }) if pdf_writer: # Designator to store the classificationresults class_designator = ds.VariableDesignator( [], resolve_type=[ robot_skills.classification_result.ClassificationResult ]) # Add the designator to the pdf writer state pdf_writer.set_designator(class_designator) smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=self._table_designator, objectIDsDes=class_designator, searchArea=GRAB_SURFACE, navigation_area="in_front_of"), transitions={ "done": "WRITE_PDF", "failed": "failed" }) smach.StateMachine.add("WRITE_PDF", pdf_writer, transitions={"done": "GRAB_ITEM_1"}) else: smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=self._table_designator, objectIDsDes=None, searchArea=GRAB_SURFACE, inspection_area="in_front_of"), transitions={ "done": "GRAB_ITEM_1", "failed": "failed" }) smach.StateMachine.add("GRAB_ITEM_1", GrabSingleItem( robot=robot, grab_designator=grab_designator_1), transitions={ "succeeded": "GRAB_ITEM_2", "failed": "GRAB_ITEM_2" }) smach.StateMachine.add("GRAB_ITEM_2", GrabSingleItem( robot=robot, grab_designator=grab_designator_2), transitions={ "succeeded": "MOVE_TO_PLACE", "failed": "MOVE_TO_PLACE" }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) smach.StateMachine.add("MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) smach.StateMachine.add("PLACE_ITEM_1", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) smach.StateMachine.add("PLACE_ITEM_2", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "succeeded", "failed": "failed" })
def __init__(self, robot, assume_john): """ :param robot: :param assume_john: bool indicating that John (the homeowner) is already there. """ smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted']) door_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_door['id']) livingroom_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_livingroom['id']) guest_entity_des = ds.VariableDesignator(resolve_type=Entity, name='guest_entity') guest_name_des = ds.VariableDesignator('guest 1', name='guest_name') guest_drink_des = ds.VariableDesignator(resolve_type=HMIResult, name='guest_drink') guest_drinkname_des = ds.FieldOfHMIResult(guest_drink_des, semantics_field='drink', name='guest_drinkname') with self: smach.StateMachine.add('LEARN_GUEST', LearnGuest(robot, door_waypoint, guest_entity_des, guest_name_des, guest_drink_des), transitions={ 'succeeded': 'SAY_GOTO_OPERATOR', 'aborted': 'SAY_GOTO_OPERATOR', 'failed': 'SAY_GOTO_OPERATOR' }) smach.StateMachine.add( 'SAY_GOTO_OPERATOR', states.SayFormatted(robot, [ "Okidoki, you are {name} and you like {drink}, lets go inside. Please follow me" ], name=guest_name_des, drink=guest_drinkname_des, block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_LIVINGROOM'}) smach.StateMachine.add( 'GOTO_LIVINGROOM', states.NavigateToWaypoint( robot, livingroom_waypoint, challenge_knowledge.waypoint_livingroom['radius']), transitions={ 'arrived': 'INTRODUCE_GUEST', 'unreachable': 'INTRODUCE_GUEST', 'goal_not_defined': 'aborted' }) smach.StateMachine.add('INTRODUCE_GUEST', IntroduceGuest(robot, guest_entity_des, guest_name_des, guest_drinkname_des, assume_john=assume_john), transitions={ 'succeeded': 'FIND_SEAT_FOR_GUEST', 'abort': 'FIND_SEAT_FOR_GUEST' }) smach.StateMachine.add( 'FIND_SEAT_FOR_GUEST', FindEmptySeat(robot, seats_to_inspect=challenge_knowledge.seats, room=ds.EntityByIdDesignator( robot, challenge_knowledge.sitting_room), seat_is_for=guest_name_des), transitions={ 'succeeded': 'succeeded', 'failed': 'aborted' })