def __init__(self, robot, table_id, table_navigation_area, table_close_navigation_area, placement_height=0.7): StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=["item_picked"]) table = EdEntityDesignator(robot=robot, id=table_id) with self: # StateMachine.add("NAVIGATE_TO_TABLE", # NavigateToSymbolic(robot, {table: table_navigation_area}, table), # transitions={'arrived': 'NAVIGATE_TO_TABLE_CLOSE', # 'unreachable': 'failed', # 'goal_not_defined': 'failed'}) StateMachine.add( "NAVIGATE_TO_TABLE_CLOSE", NavigateToSymbolic(robot, {table: table_close_navigation_area}, table), transitions={ 'arrived': 'PLACE_ITEM_ON_TABLE', 'unreachable': 'SAY_PICK_AWAY_THE_CHAIR', 'goal_not_defined': 'failed' }) StateMachine.add("FORCE_DRIVE", ForceDrive(robot, -0.1, 0, 0, 5.0), transitions={'done': 'SAY_PICK_AWAY_THE_CHAIR'}) StateMachine.add( "SAY_PICK_AWAY_THE_CHAIR", Say( robot, "Please pick away the chair, I cannot get close enough to the {}" .format(table_id)), transitions={'spoken': 'WAIT_FOR_PICK_AWAY_CHAIR'}) StateMachine.add('WAIT_FOR_PICK_AWAY_CHAIR', WaitTime(robot, 5), transitions={ 'waited': 'SAY_THANKS', 'preempted': 'failed' }) StateMachine.add('SAY_THANKS', Say(robot, "Thank you darling"), transitions={'spoken': 'NAVIGATE_TO_TABLE_CLOSE'}) StateMachine.add("PLACE_ITEM_ON_TABLE", PlaceItemOnTable(robot, table_id, placement_height), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
def __init__(self, robot, arm_designator, timeout=10): smach.StateMachine.__init__(self, outcomes=['succeeded','failed']) #A designator can resolve to a different item every time its resolved. We don't want that here, so lock check_type(arm_designator, Arm) locked_arm = LockingDesignator(arm_designator) with self: smach.StateMachine.add("LOCK_ARM", LockDesignator(locked_arm), transitions={'locked' :'SPINDLE_MEDIUM'}) smach.StateMachine.add("SPINDLE_MEDIUM", ResetPart(robot, robot.torso), transitions={'done' :'MOVE_HUMAN_HANDOVER_JOINT_GOAL'}) smach.StateMachine.add("MOVE_HUMAN_HANDOVER_JOINT_GOAL", ArmToJointConfig(robot, locked_arm, 'handover_to_human'), transitions={ 'succeeded' :'SAY_OPEN_GRIPPER', 'failed' :'SAY_OPEN_GRIPPER'}) smach.StateMachine.add("SAY_OPEN_GRIPPER", Say(robot, [ "Watch out, I will open my gripper in one second. Please take it from me."]), transitions={ 'spoken' :'OPEN_GRIPPER_HANDOVER'}) smach.StateMachine.add('OPEN_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.OPEN, timeout=2.0), transitions={'succeeded' : 'SAY_CLOSE_NOW_GRIPPER', 'failed' : 'SAY_CLOSE_NOW_GRIPPER'}) smach.StateMachine.add("SAY_CLOSE_NOW_GRIPPER", Say(robot, [ "I will close my gripper now"]), transitions={ 'spoken' :'CLOSE_GRIPPER_HANDOVER'}) # smach.StateMachine.add('OPEN_GRIPPER_ON_HANDOVER', OpenGripperOnHandoverToHuman(robot, locked_arm, timeout=timeout), # transitions={'succeeded' : 'CLOSE_GRIPPER_HANDOVER', # 'failed' : 'SAY_I_WILL_KEEP_IT'}) smach.StateMachine.add('CLOSE_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.CLOSE, timeout=0.0), transitions={'succeeded' : 'RESET_ARM', 'failed' : 'RESET_ARM'}) # smach.StateMachine.add("SAY_I_WILL_KEEP_IT", # Say(robot, [ "If you don't want it, I will keep it"]), # transitions={ 'spoken' :'RESET_ARM'}) smach.StateMachine.add('RESET_ARM', ArmToJointConfig(robot, locked_arm, 'reset'), transitions={'succeeded' :'RESET_TORSO', 'failed' :'RESET_TORSO' }) smach.StateMachine.add('RESET_TORSO', ResetPart(robot, robot.torso), transitions={'done':'UNLOCK_ARM'}) smach.StateMachine.add("UNLOCK_ARM", UnlockDesignator(locked_arm), transitions={'unlocked' :'succeeded'})
def __init__(self, robot, category_grammar, categoryw): smach.StateMachine.__init__(self, outcomes=["done"]) hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult, name="hmi_result_des") category_des = ds.FuncDesignator(ds.AttrDesignator( hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str) @smach.cb_interface(outcomes=['done']) def write_category(ud, des_read, des_write): # type: (object, ds.Designator, ds.Designator) -> str assert (ds.is_writeable(des_write)) assert (des_write.resolve_type == des_read.resolve_type) des_write.write(des_read.resolve()) return 'done' with self: smach.StateMachine.add( "ASK_WHERE_TO_DROP", Say(robot, "Please look at the object in my gripper and tell me" "which category it is. If it should be thrown away," "call it trash", block=True), transitions={"spoken": "HEAR_LOCATION"}) smach.StateMachine.add("HEAR_LOCATION", HearOptionsExtra( robot, category_grammar, ds.writeable(hmi_result_des)), transitions={ "heard": "SAY_HEARD_CORRECT", "no_result": "ASK_WHERE_TO_DROP" }) smach.StateMachine.add( "SAY_HEARD_CORRECT", Say(robot, "I understood that the object is of category {category}, is this correct?", category=category_des, block=True), transitions={"spoken": "HEAR_CORRECT"}) smach.StateMachine.add("HEAR_CORRECT", AskYesNo(robot), transitions={ "yes": "WRITE_CATEGORY", "no": "ASK_WHERE_TO_DROP", "no_result": "ASK_WHERE_TO_DROP" }) smach.StateMachine.add('WRITE_CATEGORY', smach.CBState( write_category, cb_args=[category_des, categoryw]), transitions={'done': 'done'})
def __init__(self, robot, object_category_des, room_des): smach.StateMachine.__init__(self, outcomes=["done", "failed"]) room_id_des = ds.AttrDesignator(room_des, "id", resolve_type=str) with self: smach.StateMachine.add("LOOK_INTO_ROOM", NavigateToRoom(robot, room_des, room_des), transitions={ "arrived": "SAY_COME_TO_ME", "unreachable": "SAY_COME_TO_ME", "goal_not_defined": "SAY_COME_TO_ME" }) smach.StateMachine.add( "SAY_COME_TO_ME", Say(robot, "Operator, please come to me in the {room}", room=room_id_des, block=True), transitions={"spoken": "WAIT_FOR_OPERATOR"}) smach.StateMachine.add("WAIT_FOR_OPERATOR", WaitTime(4), transitions={ "waited": "ASK_WHICH_CATERGORY", "preempted": "ASK_WHICH_CATERGORY" }) smach.StateMachine.add( "ASK_WHICH_CATERGORY", AskWhichCategory( robot, ds.Designator(challenge_knowledge.category_grammar), object_category_des), transitions={"done": "done"})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10, arm_configuration="handover_to_human"): """ Hold up hand to accept an object and close hand once something is inserted :param robot: Robot with which to execute this behavior :param arm_designator: ArmDesignator resolving to arm accept item into :param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied :param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy :param timeout: How long to hold hand over before closing without anything :param arm_configuration: Which pose to put arm in when holding hand up for the item. """ smach.StateMachine.__init__(self, outcomes=['succeeded','failed','timeout']) check_type(arm_designator, Arm) if not grabbed_entity_designator and grabbed_entity_label == "": rospy.logerr("No grabbed entity label or grabbed entity designator given") with self: smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, arm_configuration), transitions={'succeeded':'OPEN_BEFORE_INSERT','failed':'OPEN_BEFORE_INSERT'}) smach.StateMachine.add( 'OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate=GripperState.OPEN), transitions={'succeeded' : 'SAY1', 'failed' : 'SAY1'}) smach.StateMachine.add("SAY1", Say(robot,'Please hand over the object by pushing it gently in my gripper'), transitions={'spoken':'CLOSE_AFTER_INSERT'}) smach.StateMachine.add( 'CLOSE_AFTER_INSERT', CloseGripperOnHandoverToRobot(robot, arm_designator, grabbed_entity_label=grabbed_entity_label, grabbed_entity_designator=grabbed_entity_designator, timeout=timeout), transitions={'succeeded' : 'succeeded', 'timeout' : 'timeout', 'failed' : 'failed'})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) check_type(arm_designator, Arm) if not grabbed_entity_designator and grabbed_entity_label == "": rospy.logerr( "No grabbed entity label or grabbed entity designator given") with self: smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, "handover_to_human"), transitions={ 'succeeded': 'OPEN_BEFORE_INSERT', 'failed': 'OPEN_BEFORE_INSERT' }) smach.StateMachine.add('OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate='open'), transitions={ 'succeeded': 'SAY1', 'failed': 'SAY1' }) smach.StateMachine.add( "SAY1", Say( robot, 'Please hand over the object by pushing it gently in my gripper' ), transitions={'spoken': 'CLOSE_AFTER_INSERT'}) smach.StateMachine.add( 'CLOSE_AFTER_INSERT', CloseGripperOnHandoverToRobot( robot, arm_designator, grabbed_entity_label=grabbed_entity_label, grabbed_entity_designator=grabbed_entity_designator, timeout=timeout), transitions={ 'succeeded': 'succeeded', 'failed': '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, 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 setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted']) with sm: smach.StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT), transitions={ 'Done': 'GO_TO_SEARCH_POSE', 'Aborted': 'aborted', 'Failed': 'GO_TO_SEARCH_POSE' }) smach.StateMachine.add('SAY_START', Say(robot, "Finding your mates, here we go!", block=False), transitions={'spoken': 'GO_TO_SEARCH_POSE'}) smach.StateMachine.add('GO_TO_SEARCH_POSE', NavigateToWaypoint(robot, ds.EntityByIdDesignator( robot, id=SEARCH_POINT), radius=0.375), transitions={ 'arrived': 'RISE_FOR_THE_PEOPLE', 'goal_not_defined': 'failed', 'unreachable': 'WAIT_SEARCH_POSE' }) smach.StateMachine.add('WAIT_SEARCH_POSE', WaitTime(robot, 5), transitions={ 'preempted': 'aborted', 'waited': 'GO_TO_SEARCH_POSE' }) # noinspection PyUnusedLocal @smach.cb_interface(outcomes=["done"]) def _rise_for_the_people(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ robot.arms.values()[0]._send_joint_trajectory( [[0.70, -1.9, 0.0, -1.57, 0.0]]) robot.speech.speak( "Hi there. My Name is Hero. I'm looking for the mates of my operator", block=False) return "done" smach.StateMachine.add("RISE_FOR_THE_PEOPLE", smach.CBState(_rise_for_the_people), transitions={"done": "LOCATE_PEOPLE"}) # locate all four people smach.StateMachine.add('LOCATE_PEOPLE', LocatePeople(robot, room_id=ROOM_ID), transitions={ 'done': 'RESET_FOR_DRIVING', }) # noinspection PyUnusedLocal @smach.cb_interface(outcomes=["done"]) def _reset_for_driving(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ robot.speech.speak("Thank you for your attention", block=False) robot.arms.values()[0]._send_joint_trajectory([ [0.01, -1.9, 0.0, -1.57, 0.0], # Inspect with q0 low [0.01, 0.0, -1.57, -1.57, 0.0] ]) # Reset return "done" smach.StateMachine.add("RESET_FOR_DRIVING", smach.CBState(_reset_for_driving), transitions={"done": "GO_BACK_TO_OPERATOR"}) # drive back to the operator to describe the mates smach.StateMachine.add('GO_BACK_TO_OPERATOR', NavigateToWaypoint( robot, ds.EntityByIdDesignator(robot, id=OPERATOR_POINT), radius=0.7, look_at_designator=ds.EntityByIdDesignator( robot, id=OPERATOR_POINT)), transitions={ 'arrived': 'REPORT_PEOPLE', 'goal_not_defined': 'REPORT_PEOPLE', 'unreachable': 'WAIT_GO_BACK' }) smach.StateMachine.add('WAIT_GO_BACK', WaitTime(robot, 5), transitions={ 'preempted': 'aborted', 'waited': 'GO_BACK_TO_OPERATOR' }) # check how to uniquely define them # ToDo: make this more interesting smach.StateMachine.add('REPORT_PEOPLE', ReportPeople(robot), transitions={'done': 'done'}) return sm
#ROS import rospy import argparse # TU/e Robotics from robot_skills.get_robot import get_robot # Robot Smach States from robot_smach_states.human_interaction import Say if __name__ == "__main__": parser = argparse.ArgumentParser( description="Test the say state with a sentence") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node('test_say') robot = get_robot(args.robot) sentence = 'I have said something useful' say_state = Say(robot, sentence) say_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 __init__(self, robot, arm_designator, timeout=10): """ State to hand over the object in the arm to a human operator :param robot: robot to execute state with :param arm_designator: designator that resolves to arm holding the object :param timeout: float amount of time the procedure may take """ smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) # A designator can resolve to a different item every time its resolved. We don't want that here, so lock check_type(arm_designator, PublicArm) locked_arm = LockingDesignator(arm_designator) with self: smach.StateMachine.add("LOCK_ARM", LockDesignator(locked_arm), transitions={'locked': 'SPINDLE_MEDIUM'}) smach.StateMachine.add( "SPINDLE_MEDIUM", ResetPart(robot, robot.torso), transitions={'done': 'MOVE_HUMAN_HANDOVER_JOINT_GOAL'}) smach.StateMachine.add("MOVE_HUMAN_HANDOVER_JOINT_GOAL", ArmToJointConfig(robot, locked_arm, 'handover_to_human'), transitions={ 'succeeded': 'SAY_DETECT_HANDOVER', 'failed': 'SAY_DETECT_HANDOVER' }) smach.StateMachine.add("SAY_DETECT_HANDOVER", Say(robot, [ "I will handover the object now" "Please take it from my gripper." ]), transitions={'spoken': 'DETECT_HANDOVER'}) smach.StateMachine.add("DETECT_HANDOVER", HandOverTo(robot, locked_arm), transitions={ 'succeeded': 'SAY_CLOSE_NOW_GRIPPER', 'failed': 'SAY_CLOSE_NOW_GRIPPER' }) smach.StateMachine.add( "SAY_CLOSE_NOW_GRIPPER", Say(robot, ["I will close my gripper now"]), transitions={'spoken': 'CLOSE_GRIPPER_HANDOVER'}) smach.StateMachine.add('CLOSE_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.CLOSE, timeout=0), transitions={ 'succeeded': 'RESET_ARM', 'failed': 'RESET_ARM' }) smach.StateMachine.add('RESET_ARM', ArmToJointConfig(robot, locked_arm, 'reset'), transitions={ 'succeeded': 'UNLOCK_ARM', 'failed': 'UNLOCK_ARM' }) smach.StateMachine.add("UNLOCK_ARM", UnlockDesignator(locked_arm), transitions={'unlocked': 'succeeded'})
def setup_statemachine(robot): state_machine = StateMachine(outcomes=['done']) state_machine.userdata['item_picked'] = None with state_machine: # Intro # StateMachine.add('START_CHALLENGE_ROBUST', # Initialize(robot), # transitions={'initialized': 'SAY_START', 'abort': 'done'}) StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"), # ToDo: in knowledge transitions={'Done': 'SAY_START', 'Aborted': 'done', 'Failed': 'SAY_START'}) StateMachine.add('SAY_START', Say(robot, "Let's set the table baby! If there are any chairs near the kitchen_table, please remove them", block=False), transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'}) # The pre-work StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD', NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"), transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER', 'failed': 'SAY_OPEN_FAILED'}) StateMachine.add('SAY_OPEN_FAILED', Say(robot, "I failed to open the cupboard drawer"), transitions={'spoken': 'WAIT_OPEN'}) StateMachine.add('WAIT_OPEN', WaitTime(robot, 5), transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'}) StateMachine.add('SAY_OPEN_THANKS', Say(robot, "Thank you darling"), transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'}) # The loop StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER', NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of", required_items), transitions={'succeeded': 'PLACE_ITEM_ON_TABLE', 'failed': 'CHECK_IF_WE_HAVE_IT_ALL'}) StateMachine.add('PLACE_ITEM_ON_TABLE', NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"), transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL', 'failed': 'WAIT'}) StateMachine.add('WAIT', WaitTime(robot, 2), transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'}) StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL', CBState(check_if_we_have_it_all, cb_args=[robot]), transitions={'we_have_it_all': 'SAY_END_CHALLENGE', 'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'}) # Outro StateMachine.add('SAY_END_CHALLENGE', Say(robot, "That was all folks, have a nice meal!"), transitions={'spoken': 'done'}) StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD', NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"), transitions={'succeeded': 'done', 'failed': 'done'}) return state_machine
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", StartChallengeRobust(robot, STARTING_POINT), transitions={ "Done": "GO_TO_INTERMEDIATE_WAYPOINT", "Aborted": "GO_TO_INTERMEDIATE_WAYPOINT", "Failed": "GO_TO_INTERMEDIATE_WAYPOINT" }) # There is no transition to Failed in StartChallengeRobust (28 May) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT', NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_2), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_3), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'ASK_CONTINUE', 'goal_not_defined': 'ASK_CONTINUE' }) smach.StateMachine.add("ASK_CONTINUE", AskContinue(robot, 30), transitions={ 'continue': 'SAY_CONTINUEING', 'no_response': 'SAY_CONTINUEING' }) smach.StateMachine.add( 'SAY_CONTINUEING', Say(robot, [ "I heard continue, so I will move to the exit now. See you guys later!" ], block=False), transitions={'spoken': 'GO_TO_EXIT'}) # Amigo goes to the exit (waypoint stated in knowledge base) smach.StateMachine.add('GO_TO_EXIT', NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_1), radius=0.7), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_2', 'goal_not_defined': 'GO_TO_EXIT_2' }) smach.StateMachine.add('GO_TO_EXIT_2', NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_2), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_3', 'goal_not_defined': 'GO_TO_EXIT_3' }) smach.StateMachine.add('GO_TO_EXIT_3', NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_3), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_TARGET', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('RESET_ED_TARGET', ResetED(robot), transitions={'done': 'GO_TO_EXIT'}) # Finally amigo will stop and says 'goodbye' to show that he's done. smach.StateMachine.add('AT_END', Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "rips") return sm
def __init__(self, robot, operator_distance=1.0, operator_radius=0.5): # type: (Robot, float, float) -> None """ Base Smach state to guide an operator to a designated position :param robot: (Robot) robot api object :param operator_distance: (float) check for the operator to be within this range of the robot :param operator_radius: (float) from the point behind the robot defined by `distance`, the person must be within this radius """ smach.StateMachine.__init__( self, outcomes=["arrived", "unreachable", "goal_not_defined", "lost_operator", "preempted"]) self.robot = robot self.operator_distance = operator_distance self.operator_radius = operator_radius self.execute_plan = ExecutePlanGuidance(robot=self.robot, operator_distance=self.operator_distance, operator_radius=self.operator_radius) with self: @smach.cb_interface(outcomes=["done"]) def _reset_mentioned_entities(userdata=None): """ Resets the entities that have been mentioned so that the robot will mention all entities to all operators """ self.execute_plan.reset_tourguide() return "done" smach.StateMachine.add("RESET_MENTIONED_ENTITIES", smach.CBState(_reset_mentioned_entities), transitions={"done": "SAY_BEHIND"}) smach.StateMachine.add("SAY_BEHIND", Say(robot, "Please stand behind me and look at me", block=True), transitions={"spoken": "WAIT"}) smach.StateMachine.add("WAIT", WaitTime(robot, waittime=3.0), transitions={"waited": "GET_PLAN", "preempted": "preempted"}) smach.StateMachine.add("GET_PLAN", navigation.getPlan(self.robot, self.generate_constraint), transitions={"unreachable": "unreachable", "goal_not_defined": "goal_not_defined", "goal_ok": "EXECUTE_PLAN"}) smach.StateMachine.add("EXECUTE_PLAN", self.execute_plan, transitions={"arrived": "arrived", "blocked": "PLAN_BLOCKED", "preempted": "preempted", "lost_operator": "WAIT_FOR_OPERATOR"}) smach.StateMachine.add("WAIT_FOR_OPERATOR", WaitForOperator(robot=self.robot, distance=self.operator_distance, radius=self.operator_radius), transitions={"is_following": "GET_PLAN", "is_lost": "lost_operator"}) smach.StateMachine.add("PLAN_BLOCKED", navigation.planBlocked(self.robot), transitions={"blocked": "GET_PLAN", "free": "EXECUTE_PLAN"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"]) hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult) information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator( hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str) information_point_designator = ds.EdEntityDesignator( robot, id_designator=information_point_id_designator) with self: single_item = InformMachine(robot) if START_ROBUST: smach.StateMachine.add("START_CHALLENGE", StartChallengeRobust( robot, INITIAL_POSE_ID), transitions={ "Done": "ASK_WHERE_TO_GO", "Aborted": "Aborted", "Failed": "Aborted" }) smach.StateMachine.add( "ASK_WHERE_TO_GO", Say( robot, "Near which furniture object should I go to start guiding people?" ), transitions={"spoken": "WAIT_WHERE_TO_GO"}) smach.StateMachine.add( "WAIT_WHERE_TO_GO", HearOptionsExtra( robot=robot, spec_designator=ds.Designator( initial_value=START_GRAMMAR), speech_result_designator=hmi_result_des.writeable), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "ASK_WHERE_TO_GO" }) # ToDo: add fallbacks #option: STORE_STARTING_POSE smach.StateMachine.add( "ASK_CONFIRMATION", Say(robot, [ "I hear that you would like me to start the tours at" " the {place}, is this correct?" ], place=information_point_id_designator, block=True), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", HearOptions(robot=robot, options=["yes", "no"]), transitions={ "yes": "MOVE_OUT_OF_MY_WAY", "no": "ASK_WHERE_TO_GO", "no_result": "ASK_WHERE_TO_GO" }) smach.StateMachine.add( "MOVE_OUT_OF_MY_WAY", Say(robot, "Please move your ass so I can get going!"), transitions={"spoken": "TC_MOVE_TIME"}) smach.StateMachine.add("TC_MOVE_TIME", WaitTime(robot=robot, waittime=3), transitions={ "waited": "NAV_TO_START", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START", NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "in_front_of" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "WAIT_NAV_BACKUP", "goal_not_defined": "Aborted" }) # If this happens: never mind smach.StateMachine.add("WAIT_NAV_BACKUP", WaitTime(robot, 3.0), transitions={ "waited": "NAV_TO_START_BACKUP", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START_BACKUP", NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "near" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "SAY_CANNOT_REACH_WAYPOINT", # Current pose backup "goal_not_defined": "Aborted" }) # If this happens: never mind @smach.cb_interface(outcomes=["done"]) def _turn_around(userdata=None): """ Turns the robot approximately 180 degrees around """ v_th = 0.5 robot.base.force_drive(vx=0.0, vy=0.0, vth=v_th, timeout=math.pi / v_th) return "done" smach.StateMachine.add( "TURN_AROUND", smach.CBState(_turn_around), transitions={"done": "STORE_STARTING_POSE"}) smach.StateMachine.add( "SAY_CANNOT_REACH_WAYPOINT", Say( robot, "I am not able to reach the starting point." "I'll use this as starting point"), transitions={"spoken": "STORE_STARTING_POSE"}) else: smach.StateMachine.add("INITIALIZE", Initialize(robot), transitions={ "initialized": "STORE_STARTING_POSE", "abort": "Aborted" }) # This is purely for a back up scenario until the range iterator @smach.cb_interface(outcomes=["succeeded"]) def store_pose(userdata=None): base_loc = robot.base.get_location() base_pose = base_loc.frame location_id = INFORMATION_POINT_ID robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped( base_pose, "/map"), type="waypoint") return "succeeded" smach.StateMachine.add("STORE_STARTING_POSE", smach.CBState(store_pose), transitions={"succeeded": "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(1000), 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", Say(robot, "Goodbye"), transitions={"spoken": "Done"})
def __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: self.spec_des = ds.Designator(knowledge.location_grammar) self.answer_des = ds.VariableDesignator(resolve_type=HMIResult) self.entity_des = EntityFromHmiResults(robot, self.answer_des) self._location_hmi_attempt = 0 self._max_hmi_attempts = 3 # ToDo: parameterize? @smach.cb_interface(outcomes=["reset"]) def _reset_location_hmi_attempt(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ self._location_hmi_attempt = 0 return "reset" smach.StateMachine.add("RESET_HMI_ATTEMPT", smach.CBState(_reset_location_hmi_attempt), transitions={"reset": "ANNOUNCE_ITEM"}) if WAIT_MODE == WaitMode.SPEECH: smach.StateMachine.add( "ANNOUNCE_ITEM", Say(robot, "Hello, my name is {}. Please call me by my name. " "Talk loudly into my microphone and wait for the ping". format(robot.robot_name), block=True), transitions={"spoken": "WAIT_TO_BE_CALLED"}) smach.StateMachine.add( "WAIT_TO_BE_CALLED", HearOptions(robot, ["{}".format(robot.robot_name)], timeout=10), transitions={ "{}".format(robot.robot_name): "INSTRUCT", "no_result": "ANNOUNCE_ITEM" }) elif WAIT_MODE == WaitMode.VISUAL: smach.StateMachine.add( "ANNOUNCE_ITEM", Say(robot, "Hello, my name is {}. Please step in front of me.". format(robot.robot_name), block=True), transitions={"spoken": "WAIT_TO_BE_CALLED"}) smach.StateMachine.add("WAIT_TO_BE_CALLED", WaitForPersonInFront( robot, attempts=10, sleep_interval=1.0), transitions={ "success": "INSTRUCT", "failed": "SAY_NOT_DETECTED" }) smach.StateMachine.add( "SAY_NOT_DETECTED", Say(robot, "I did not see you but will try to continue anyway.". format(robot.robot_name), block=True), transitions={"spoken": "INSTRUCT"}) smach.StateMachine.add( "INSTRUCT", Say(robot, [ "Please tell me where you would like to go. " "Talk loudly into my microphone and wait for the ping" ], block=True), transitions={"spoken": "LISTEN_FOR_LOCATION"}) smach.StateMachine.add("LISTEN_FOR_LOCATION", HearOptionsExtra(robot, self.spec_des, self.answer_des.writeable, 6), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "HANDLE_FAILED_HMI" }) smach.StateMachine.add( "ASK_CONFIRMATION", Say(robot, [ "I hear that you would like to go to the {place}, is this correct?" ], place=ds.AttrDesignator(self.entity_des, "id", resolve_type=str)), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", HearOptions(robot=robot, options=["yes", "no"]), transitions={ "yes": "INSTRUCT_FOR_WAIT", "no": "INSTRUCT", "no_result": "HANDLE_FAILED_HMI" }) @smach.cb_interface(outcomes=["retry", "fallback", "failed"]) def _handle_failed_hmi(userdata=None): """ Handle failed HMI queries so we can try up to x times """ self._location_hmi_attempt += 1 # Increment if self._location_hmi_attempt == self._max_hmi_attempts: rospy.logwarn( "HMI failed for the {} time, returning 'failed'". format(self._max_hmi_attempts)) if not BACKUP_SCENARIOS: rospy.logwarn( "No fallback scenario's available anymore") return "failed" backup = BACKUP_SCENARIOS.pop(0) robot.speech.speak("I am sorry but I did not hear you", mood="sad", block=False) robot.speech.speak(backup.sentence, block=False) self.answer_des.writeable.write( HMIResult("", backup.entity_id)) return "fallback" rospy.loginfo( "HMI failed for the {} time out of {}, retrying".format( self._location_hmi_attempt, self._max_hmi_attempts)) return "retry" smach.StateMachine.add("HANDLE_FAILED_HMI", smach.CBState(_handle_failed_hmi), transitions={ "retry": "INSTRUCT", "fallback": "INSTRUCT_FOR_WAIT", "failed": "failed" }) smach.StateMachine.add( "INSTRUCT_FOR_WAIT", Say(robot, [ "Let me think how to get to the {entity_id}", "I will now determine the best route to the {entity_id}" ], entity_id=ds.AttrDesignator(self.entity_des, "id", resolve_type=str)), transitions={"spoken": "GIVE_DIRECTIONS"}) smach.StateMachine.add("GIVE_DIRECTIONS", GiveDirections(robot, self.entity_des), transitions={ "succeeded": "INSTRUCT_FOLLOW", "failed": "failed" }) smach.StateMachine.add("INSTRUCT_FOLLOW", Say(robot, ["Please follow me"], block=True), transitions={"spoken": "GUIDE_OPERATOR"}) smach.StateMachine.add("GUIDE_OPERATOR", GuideToRoomOrObject(robot, self.entity_des), transitions={ "arrived": "SUCCESS", "unreachable": "SAY_CANNOT_REACH", "goal_not_defined": "SAY_CANNOT_REACH", "lost_operator": "SAY_LOST_OPERATOR", "preempted": "failed" }) smach.StateMachine.add( "SUCCESS", Say(robot, ["We have arrived"], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add( "SAY_CANNOT_REACH", Say(robot, ["I am sorry but I cannot reach the destination."], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add( "SAY_LOST_OPERATOR", Say(robot, ["Oops I have lost you completely."], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add("RETURN_TO_INFORMATION_POINT", NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, INFORMATION_POINT_ID)), transitions={ "arrived": "succeeded", "unreachable": "failed", "goal_not_defined": "failed" })
def __init__(self, robot, location_des): """ Visit all selected locations from the list, and handle the found objects :param location_des: is a designator resolving to a dictionary with fields ... TODO """ smach.StateMachine.__init__(self, outcomes=['done']) segment_areas = ValueByKeyDesignator(location_des, "segment_areas", [str], name='segment_areas') segment_area = VariableDesignator(resolve_type=str, name='segment_area') navigation_area_des = ValueByKeyDesignator(location_des, key='navigation_area', resolve_type=str, name='navigation_area') location_id_des = ValueByKeyDesignator(location_des, key='name', resolve_type=str, name='location_id') room_des = EdEntityDesignator(robot, id_designator=ValueByKeyDesignator( location_des, key="room", resolve_type=str)) # Set up the designators for this machine e_classifications_des = VariableDesignator( [], resolve_type=[ClassificationResult], name='e_classifications_des') e_des = EdEntityDesignator(robot, id_designator=location_id_des, name='e_des') with self: smach.StateMachine.add('ITERATE_NEXT_AREA', IterateDesignator(segment_areas, segment_area.writeable), transitions={ "next": 'INSPECT', "stop_iteration": "done" }) # Segment the area and handle segmented objects for the specified navigation area # for i, segment_area in enumerate(segment_areas): smach.StateMachine.add( 'INSPECT', NavigateToSymbolic(robot, {e_des: navigation_area_des}, e_des), transitions={ 'arrived': 'SEGMENT_SAY', 'unreachable': "SAY_UNREACHABLE", 'goal_not_defined': "SAY_UNREACHABLE" }) smach.StateMachine.add("SEGMENT_SAY", Say(robot, "Looking {area} the {entity}", area=segment_area, entity=location_id_des, block=False), transitions={"spoken": "RISE"}) smach.StateMachine.add('RISE', RiseForInspect(robot, e_des, segment_area), transitions={ 'succeeded': 'SEGMENT', 'failed': 'SEGMENT' }) smach.StateMachine.add( 'SEGMENT', SegmentObjects(robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES"}) smach.StateMachine.add("SAY_UNREACHABLE", Say(robot, "I failed to inspect the {furn}", furn=location_id_des, block=True), transitions={"spoken": "done"}) smach.StateMachine.add("HANDLE_DETECTED_ENTITIES", HandleDetectedEntities( robot, e_classifications_des, location_id_des, segment_area, room_des), transitions={"done": "ITERATE_NEXT_AREA"})
def __init__(self, robot, entity_des, operator_distance=1.5, operator_radius=0.5): """ Constructor :param robot: robot object :param entity_des: designator resolving to a room or a piece of furniture :param operator_distance: (float) check for the operator to be within this range of the robot :param operator_radius: (float) from the point behind the robot defined by `distance`, the person must be within this radius """ smach.StateMachine.__init__(self, outcomes=[ "arrived", "unreachable", "goal_not_defined", "lost_operator", "preempted" ]) self.operator_distance = operator_distance self.operator_radius = operator_radius with self: @smach.cb_interface(outcomes=["room", "object"]) def determine_type(userdata=None): entity = entity_des.resolve() entity_type = entity.type if entity_type == "room": return "room" else: return "object" smach.StateMachine.add("DETERMINE_TYPE", smach.CBState(determine_type), transitions={ "room": "GUIDE_TO_ROOM", "object": "GUIDE_TO_FURNITURE" }) smach.StateMachine.add( "GUIDE_TO_ROOM", guidance.GuideToSymbolic( robot, {entity_des: "in"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "WAIT_ROOM_BACKUP", "goal_not_defined": "goal_not_defined", "lost_operator": "ROOM_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add("WAIT_ROOM_BACKUP", WaitTime(robot, 3.0), transitions={ "waited": "GUIDE_TO_ROOM_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_TO_ROOM_BACKUP", guidance.GuideToSymbolic( robot, {entity_des: "in"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", "lost_operator": "ROOM_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_TO_FURNITURE", guidance.GuideToSymbolic( robot, {entity_des: "in_front_of"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "WAIT_FURNITURE_BACKUP", # Something is blocking "goal_not_defined": "GUIDE_NEAR_FURNITURE", # in_front_of not defined "lost_operator": "FURNITURE_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_NEAR_FURNITURE", guidance.GuideToSymbolic( robot, {entity_des: "near"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "WAIT_FURNITURE_BACKUP", "goal_not_defined": "goal_not_defined", "lost_operator": "FURNITURE_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add("WAIT_FURNITURE_BACKUP", WaitTime(robot, 3.0), transitions={ "waited": "GUIDE_NEAR_FURNITURE_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_NEAR_FURNITURE_BACKUP", guidance.GuideToSymbolic( robot, {entity_des: "near"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", "lost_operator": "FURNITURE_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add("ROOM_NAV_BACKUP", NavigateToSymbolic(robot, {entity_des: "in"}, entity_des), transitions={ "arrived": "SAY_ARRIVED", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", }) smach.StateMachine.add("FURNITURE_NAV_BACKUP", NavigateToSymbolic(robot, {entity_des: "near"}, entity_des), transitions={ "arrived": "SAY_ARRIVED", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", }) smach.StateMachine.add( "SAY_ARRIVED", Say(robot, "We have arrived. I'll go back to the meeting point"), transitions={"spoken": "arrived"})
def __init__(self, robot, arm_designator, grab_entity_designator): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) check_type(arm_designator, Arm) self.arm_designator = arm_designator self.robot = robot self.grab_entity_designator = grab_entity_designator '''check check input and output keys''' with self: smach.StateMachine.add('CLOSE_GRIPPER', SetGripper(robot, arm_designator, gripperstate=ArmState.CLOSE), transitions={ 'succeeded': 'PRE_POINT', 'failed': 'PRE_POINT' }) smach.StateMachine.add('PRE_POINT', ArmToQueryPoint(robot, arm_designator, grab_entity_designator, time_out=20, pre_grasp=True, first_joint_pos_only=True), transitions={ 'succeeded': 'POINT', 'failed': 'RESET_ARM_FAILED' }) smach.StateMachine.add('POINT', Point_at_object(arm_designator, robot, grab_entity_designator), transitions={ 'point_succeeded': 'SAY_POINTED', 'point_failed': 'RETRACT', 'target_lost': 'RESET_ARM_FAILED' }) smach.StateMachine.add( 'SAY_POINTED', Say(robot, "I hope this is the object you were looking for."), transitions={'spoken': 'RETRACT'}) smach.StateMachine.add('RETRACT', ArmToJointConfig(robot, arm_designator, 'retract'), transitions={ 'succeeded': 'RESET_ARM_SUCCEEDED', 'failed': 'RESET_ARM_SUCCEEDED' }) smach.StateMachine.add('RESET_ARM_SUCCEEDED', ArmToJointConfig(robot, arm_designator, 'reset'), transitions={ 'done': 'succeeded', 'failed': 'succeeded' }) smach.StateMachine.add('RESET_ARM_FAILED', ArmToJointConfig(robot, arm_designator, 'reset'), transitions={ 'done': 'failed', 'failed': 'failed' })
def __init__(self, robot, furniture_designator, entity_designator): # type: (Robot, object) -> None """ Drives to the designated furniture object, inspects this and selects the entity that will be pointed to :param robot: (Robot) robot API object :param furniture_designator: (EdEntityDesignator) designates the furniture object that was pointed to. :param entity_designator: (EdEntityDesignator) writeable EdEntityDesignator """ # ToDo: we need to add userdata smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=["laser_dot"]) assert ds.is_writeable( entity_designator ), "Entity designator must be writeable for this purpose" object_ids_des = ds.VariableDesignator( [], resolve_type=[ClassificationResult]) with self: smach.StateMachine.add("SAY_GO", Say(robot, "Let's go to the {furniture_object}", furniture_object=ds.AttrDesignator( furniture_designator, "id", resolve_type=str)), transitions={"spoken": "INSPECT_FURNITURE"}) smach.StateMachine.add("INSPECT_FURNITURE", Inspect(robot=robot, entityDes=furniture_designator, objectIDsDes=object_ids_des, navigation_area="in_front_of"), transitions={ "done": "SELECT_ENTITY", "failed": "SAY_INSPECTION_FAILED" }) # ToDo: fallback? smach.StateMachine.add( "SAY_INSPECTION_FAILED", Say(robot, "I am sorry but I was not able to reach the {furniture_object}", furniture_object=ds.AttrDesignator(furniture_designator, "id", resolve_type=str)), transitions={"spoken": "failed"}) @smach.cb_interface(outcomes=["succeeded", "no_entities"], input_keys=["laser_dot"]) def select_entity(userdata): """ Selects the entity that the robot believes the operator has pointed to and that the robot will identify later on. Userdata contains key 'laser_dot' with value geometry_msgs.msg.PointStamped where the operator pointed at. :param userdata: (dict) :return: (str) outcome """ assert userdata.laser_dot.header.frame_id.endswith( "map"), "Provide your laser dot in map frame" # Extract classification results entity_ids = [cr.id for cr in object_ids_des.resolve()] rospy.loginfo("Segmented entities: {}".format(entity_ids)) # Obtain all corresponding entities all_entities = robot.ed.get_entities() segmented_entities = [ e for e in all_entities if e.id in entity_ids ] # Filter out 'unprobable' entities candidates = [] for entity in segmented_entities: # type: Entity # The following filtering has been 'copied' from the cleanup challenge # It can be considered a first step but does not take the orientation of the convex hull into # account shape = entity.shape size_x = max(shape.x_max - shape.x_min, 0.001) size_y = max(shape.y_max - shape.y_min, 0.001) if size_x > SIZE_LIMIT or size_y > SIZE_LIMIT: continue if not 1 / min(RATIO_LIMIT, 1000) <= size_x / size_y <= min( RATIO_LIMIT, 1000): continue candidates.append(entity) # If no entities left: don't bother continuing if not candidates: rospy.logwarn("No 'probable' entities left") return "no_entities" # Select entity closest to the point where the operator pointed at (i.e., closest in 2D) closest_tuple = (None, None) x_ref = userdata.laser_dot.point.x y_ref = userdata.laser_dot.point.y # ToDo: use sorting for this... for e in candidates: # type: Entity x_e = e.pose.frame.p.x() y_e = e.pose.frame.p.y() distance_2d = math.hypot(x_ref - x_e, y_ref - y_e) rospy.loginfo("Entity {} at {}, {}: distance = {}".format( e.id, x_e, y_e, distance_2d)) if closest_tuple[0] is None or distance_2d < closest_tuple[ 1]: closest_tuple = (e, distance_2d) rospy.loginfo("Best entity: {} at {}".format( closest_tuple[0].id, closest_tuple[1])) entity_designator.write(closest_tuple[0]) return "succeeded" smach.StateMachine.add("SELECT_ENTITY", smach.CBState(select_entity), transitions={ "succeeded": "succeeded", "no_entities": "failed" })