def __init__(self, robot, arm_designator, drop_bag_pose): """ :param robot: the robot with which to execute this state machine :param arm_designator: ArmDesignator resolving to Arm holding the bag to drop """ smach.StateMachine.__init__(self, outcomes=['done']) check_type(arm_designator, Arm) with self: smach.StateMachine.add('DROP_POSE', states.ArmToJointConfig( robot, arm_designator, drop_bag_pose), transitions={ 'succeeded': 'OPEN_AFTER_DROP', 'failed': 'OPEN_AFTER_DROP' }) smach.StateMachine.add('OPEN_AFTER_DROP', states.SetGripper( robot, arm_designator, gripperstate=GripperState.OPEN), transitions={ 'succeeded': 'RESET_ARM', 'failed': 'RESET_ARM' }) smach.StateMachine.add('RESET_ARM', states.ResetArms(robot), transitions={'done': 'done'})
def __init__(self, robot, arm_designator): """ :param robot: the robot with which to execute this state machine :param arm_designator: ArmDesignator resolving to Arm holding the bag to drop """ smach.StateMachine.__init__(self, outcomes=['succeeded','failed']) check_type(arm_designator, Arm) with self: smach.StateMachine.add( 'OPEN_BEFORE_DROP', states.SetGripper(robot, arm_designator, gripperstate='open'), transitions={'succeeded' : 'DROP_POSE', 'failed' : 'DROP_POSE'}) smach.StateMachine.add("DROP_POSE", states.ArmToJointConfig(robot, arm_designator, "drop_bag_pose"), transitions={'succeeded' :'RESET_ARM_OK', 'failed' :'RESET_ARM_FAIL'}) smach.StateMachine.add( 'RESET_ARM_OK', states.ResetArms(robot), transitions={'done' : 'succeeded'}) smach.StateMachine.add( 'RESET_ARM_FAIL', states.ResetArms(robot), transitions={'done' : 'failed'})
def __init__(self, robot): # type: (Robot) -> str """ Initialization method :param robot: robot api object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) # Designators bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des') room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des') objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des') unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des') hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm') with self: smach.StateMachine.add("INITIALIZE", states.Initialize(robot=robot), transitions={"initialized": "INITIAL_POSE", "abort": "aborted"}) smach.StateMachine.add("INITIAL_POSE", states.SetInitialPose(robot, challenge_knowledge.starting_point), transitions={"done": "INSPECT_BAR", "preempted": "aborted", "error": "INSPECT_BAR"}) # Inspect bar and store the list of available drinks smach.StateMachine.add("INSPECT_BAR", states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of", objectIDsDes=objects_list_des), transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019 "failed": "INSPECT_FALLBACK"}) smach.StateMachine.add("CHECK_INSPECT_RESULT", CheckInspect(objects_list_des, [ClassificationResult]), transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK", "false": "INSPECT_FALLBACK"}) smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK", IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects, classification_list_designator=objects_list_des, unavailable_drink_designator=unav_drink_des.writeable, max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS), transitions={"done": "NAVIGATE_TO_ROOM", "failed": "INSPECT_FALLBACK"}) # Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink smach.StateMachine.add("INSPECT_FALLBACK", AskAvailability(robot=robot, unavailable_drink_designator=unav_drink_des.writeable, objects=common_knowledge.objects), transitions={"succeeded": "RESET_ROBOT", "failed": "RESET_ROBOT"}) smach.StateMachine.add("RESET_ROBOT", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "NAVIGATE_TO_ROOM", 'failed': "NAVIGATE_TO_ROOM"}) # Navigate to the predefined room smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "SAY_HI", "unreachable": "SAY_HI", "goal_not_defined": "aborted"}) smach.StateMachine.add("SAY_HI", states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)), transitions={"spoken": "SERVE_DRINK_1"}) # Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state # is constructed every time for idx in range(1, challenge_knowledge.NR_DRINKS + 1): next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE" smach.StateMachine.add("SERVE_DRINK_{}".format(idx), ServeOneDrink(robot=robot, bar_designator=bar_designator, room_id=challenge_knowledge.room_id, room_designator=room_designator, objects_list_des=objects_list_des, unav_drink_des=unav_drink_des, name_options=common_knowledge.names, objects=common_knowledge.objects), transitions={"succeeded": next_state, "failed": next_state, "aborted": next_state}) smach.StateMachine.add("SAY_DONE", states.Say(robot, "My job here is done. Enjoy your day and see you next time"), transitions={"spoken": "succeeded"})
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
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, bar_designator, room_id, room_designator, objects_list_des, unav_drink_des, name_options, objects): """ Initialization method :param robot: robot api object :param bar_designator: (EntityDesignator) in which the bar location is stored :param room_id: room ID from challenge knowledge :param room_designator: (EntityDesignator) in which the room location is stored :param objects_list_des: (VariableDesignator) in which the available drinks are stored :param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored :param name_options: Names from common knowledge :param objects: Objects from common knowledge """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) # Designators arm_designator = ds.UnoccupiedArmDesignator(robot=robot, arm_properties={}, name='arm_des').lockable() drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des') drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des') operator_name = ds.VariableDesignator(resolve_type=str, name='name_des') operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des') learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des') hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2') with self: # Lock the arm_designator smach.StateMachine.add("LOCK_ARM", states.LockDesignator(arm_designator), transitions={'locked': "GET_ORDER"}) # Get order smach.StateMachine.add("GET_ORDER", GetOrder(robot=robot, operator_name=operator_name, drink_designator=drink_str_designator, available_drinks_designator=objects_list_des, unavailable_drink_designator=unav_drink_des, name_options=name_options, objects=objects, learn_check_designator=learn_check_designator.writeable, target_room_designator=room_designator), transitions={"succeeded": "INSPECT_BAR", "failed": "failed", "aborted": "aborted"}) # Inspect bar smach.StateMachine.add("INSPECT_BAR", states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"), transitions={"done": "GRASP_DRINK", "failed": "FALLBACK_BAR"}) # Grasp drink smach.StateMachine.add("GRASP_DRINK", states.Grab(robot=robot, item=drink_designator, arm=arm_designator), transitions={"done": "FIND_OPERATOR", "failed": "FALLBACK_BAR"}) # Inspect or grasp fallback - ask for assistance smach.StateMachine.add("FALLBACK_BAR", states.Say(robot=robot, sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "HANDOVER_FROM_HUMAN"}) # Handover from human fallback smach.StateMachine.add("HANDOVER_FROM_HUMAN", states.HandoverFromHuman(robot=robot, arm_designator=arm_designator, grabbed_entity_designator=drink_designator), transitions={"succeeded": "RESET_ROBOT_2", "failed": "RESET_ROBOT_2", "timeout": "RESET_ROBOT_2"}) smach.StateMachine.add("RESET_ROBOT_2", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "CHECK_LEARN_OPERATOR", 'failed': "CHECK_LEARN_OPERATOR"}) smach.StateMachine.add("CHECK_LEARN_OPERATOR", states.CheckBool(learn_check_designator), transitions={"true": "FIND_OPERATOR", "false": "GO_TO_ROOM"}) smach.StateMachine.add("GO_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "SAY_NOT_FOUND", "unreachable": "failed", "goal_not_defined": "aborted"}) # Find operator smach.StateMachine.add("FIND_OPERATOR", states.FindPersonInRoom(robot=robot, area=room_id, name=operator_name, discard_other_labels=True, found_entity_designator=operator_designator.writeable), transitions={"found": "GOTO_OPERATOR", "not_found": "SAY_NOT_FOUND"}) # Move to this person smach.StateMachine.add("GOTO_OPERATOR", states.NavigateToObserve(robot=robot, entity_designator=operator_designator), transitions={"arrived": "SAY_THE_NAME", "unreachable": "SAY_NOT_FOUND", "goal_not_defined": "SAY_NOT_FOUND"}) # Say not found smach.StateMachine.add("SAY_NOT_FOUND", states.Say(robot=robot, sentence=DescriptionStrDesignator("not_found_operator", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "RISE_FOR_HMI_2"}) # Say the name smach.StateMachine.add("SAY_THE_NAME", states.Say(robot=robot, sentence=DescriptionStrDesignator("found_operator", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "RISE_FOR_HMI_2"}) smach.StateMachine.add("RISE_FOR_HMI_2", states.RiseForHMI(robot=robot), transitions={"succeeded": "HAND_OVER", "failed": "HAND_OVER"}) # Hand over the drink to the operator smach.StateMachine.add("HAND_OVER", states.HandoverToHuman(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "UNLOCK_ARM", "failed": "UNLOCK_ARM"}) smach.StateMachine.add("UNLOCK_ARM", states.UnlockDesignator(arm_designator), transitions={'unlocked': "RESET_ROBOT_3"}) smach.StateMachine.add("RESET_ROBOT_3", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "RETURN_TO_ROOM", 'failed': "RETURN_TO_ROOM"}) smach.StateMachine.add("RETURN_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "succeeded", "unreachable": "failed", "goal_not_defined": "aborted"})
def __init__(self, robot, operator_name, drink_designator, available_drinks_designator, unavailable_drink_designator, name_options, objects, learn_check_designator, target_room_designator): # type: (Robot, str, VariableDesignator) -> None """ Initialization method :param robot: robot api object :param operator_name: name with which the operator will be stored in image recognition module :param drink_designator: (VariableDesignator) in which the drink to fetch is stored :param available_drinks_designator: (VariableDesignator) in which the available drinks are stored :param unavailable_drink_designator: (VariableDesignator) in which the unavailable drink is stored :param name_options: Names from common knowledge :param objects: Objects from common knowledge :param learn_check_designator: (VariableDesignator) Bool flag indicating whether the operator was learnt successfully :param target_room_designator: (EdEntityDesignator) Entity specifying the target room where the operator needs to be searched for getting an order """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_3') with self: # Operator id caller_id = "operator" caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name="caller_des", none_resolve=True) smach.StateMachine.add("RESET_ROBOT_GET_ORDER", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "SAY_PEOPLE_WITHOUT_DRINKS", 'failed': "SAY_PEOPLE_WITHOUT_DRINKS"}) # Detect - people holding drinks and people without drinks smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS", states.Say(robot=robot, sentence="Trying to find people without a drink", look_at_standing_person=True, block=True), transitions={"spoken": "FIND_PERSON_WITHOUT_DRINK"}) # TODO: Change DummyState to actual state smach.StateMachine.add("FIND_PERSON_WITHOUT_DRINK", states.SetPoseFirstFoundPersonToEntity(robot=robot, properties={'tags': ['LNotHolding', 'RNotHolding']}, strict=True, dst_entity_designator=caller_id, query_entity_designator=target_room_designator), transitions={"done": "SAY_I_HAVE_SEEN", "failed": "SAY_PEOPLE_WITHOUT_DRINKS_FAILED"}) # Detect fallback - detect waving people smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS_FAILED", states.Say(robot=robot, sentence="Could not detect people without drinks", look_at_standing_person=True, block=True), transitions={"spoken": "ASK_FOR_WAVING"}) smach.StateMachine.add("ASK_FOR_WAVING", states.Say(robot=robot, sentence="Please raise your arm completely and wave, if you want me to bring you something", look_at_standing_person=True, block=True), transitions={"spoken": "WAIT_FOR_WAVING"}) # Change to WAIT_FOR_WAVING smach.StateMachine.add("WAIT_FOR_WAVING", states.SetPoseFirstFoundPersonToEntity(robot=robot, properties={'tags': ['LWave', 'RWave']}, strict=False, dst_entity_designator=caller_id, query_entity_designator=target_room_designator), transitions={"done": "SAY_I_HAVE_SEEN", "failed": "SAY_COULD_NOT_FIND_WAVING"}) # Navigate to person who wants to place an order smach.StateMachine.add("SAY_COULD_NOT_FIND_WAVING", states.Say(robot=robot, sentence="I did not find any waving person.", look_at_standing_person=True, block=True), transitions={"spoken": "ASK_STEP_IN_FRONT"}) smach.StateMachine.add("SAY_I_HAVE_SEEN", states.Say(robot=robot, sentence="Found person who might want to place an order. I will be there shortly!", look_at_standing_person=True, block=True), transitions={"spoken": "NAVIGATE_TO_PERSON"}) # Navigate to waving people smach.StateMachine.add("NAVIGATE_TO_PERSON", states.NavigateToObserve(robot=robot, entity_designator=caller_designator, radius=1.1), transitions={"arrived": "LEARN_NAME", "unreachable": "SAY_COULD_NOT_NAVIGATE", "goal_not_defined": "SAY_PEOPLE_WITHOUT_DRINKS"}) # Detect waving people fallback - ask operator in front smach.StateMachine.add("SAY_COULD_NOT_NAVIGATE", states.Say(robot=robot, sentence="Sorry! I could not navigate to you.", look_at_standing_person=True), transitions={"spoken": "ASK_STEP_IN_FRONT"}) smach.StateMachine.add("ASK_STEP_IN_FRONT", states.Say(robot=robot, sentence="Please step in front of me to give your order", look_at_standing_person=True, block=True), transitions={"spoken": "LEARN_NAME"}) # Ask operator for his name smach.StateMachine.add("LEARN_NAME", states.AskPersonName(robot=robot, person_name_des=operator_name.writeable, name_options=name_options, default_name="john", nr_tries=2), transitions={"succeeded": "LEARN_OPERATOR", "failed": "LEARN_NAME_FALLBACK", "timeout": "LEARN_NAME_FALLBACK"}) # Ask operator for his name fallback smach.StateMachine.add("LEARN_NAME_FALLBACK", states.Say(robot=robot, sentence="Sorry, I did not get your name, I'll just call you john", look_at_standing_person=True), transitions={"spoken": "LEARN_OPERATOR"}) # Learn operator smach.StateMachine.add("LEARN_OPERATOR", states.LearnPerson(robot=robot, name_designator=operator_name, nr_tries=5), transitions={"succeeded": "ASK_DRINK", "failed": "LEARN_OPERATOR_FALLBACK"}) # Learn operator fallback smach.StateMachine.add("LEARN_OPERATOR_FALLBACK", states.Say(robot=robot, sentence="I will call you by your name when I'm back", look_at_standing_person=True, block=True), transitions={"spoken": "LEARN_OPERATOR_FLAG_TOGGLE"}) smach.StateMachine.add("LEARN_OPERATOR_FLAG_TOGGLE", states.ToggleBool(learn_check_designator), transitions={"done": "ASK_DRINK"}) # Ask for preferred beverage smach.StateMachine.add("ASK_DRINK", AskDrink(robot=robot, operator_name=operator_name, drink_designator=drink_designator.writeable, available_drinks_designator=available_drinks_designator, unavailable_drink_designator=unavailable_drink_designator, objects=objects), transitions={"succeeded": "succeeded", "failed": "failed", "aborted": "aborted"})
def __init__(self, robot, trashbin_designator, arm_designator): """ :param robot: robot object :param trashbin_designator: EdEntityDesignator designating the trashbin :param arm_designator: arm designator resolving to the arm with which to grab """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) place_pose_designator = EmptySpotDesignator(robot, trashbin_designator, arm_designator) with self: # @cb_interface(outcomes=['done']) # def _joint_goal(_): # arm = arm_designator.resolve() # if not arm: # rospy.logerr("Could not resolve arm") # return "failed" # # Send to grab trash pose # arm.send_joint_goal('grab_trash_bag') # arm.wait_for_motion_done() # return 'done' # # smach.StateMachine.add("JOINT_GOAL", # CBState(_joint_goal), # transitions={'done': 'GO_BIN'}) smach.StateMachine.add("GO_BIN", states.NavigateToPlace(robot=robot, place_pose_designator=place_pose_designator, arm_designator=arm_designator), transitions={"arrived": "GET_BIN_POSITION", "goal_not_defined": "aborted", "unreachable": "failed"}) smach.StateMachine.add("GET_BIN_POSITION", GetTrashBin(robot=robot, trashbin=trashbin_designator), transitions={"succeeded": "JOINT_PATH", "failed": "failed"}) @cb_interface(outcomes=['done']) def _joint_path(_): robot.head.cancel_goal() arm = arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" # ToDo: fix # Send to grab trash pose arm._arm._send_joint_trajectory( [[0.01, 0.0, -1.57, -1.57, 0.0], [0.69, 0.0, -1.57, -1.57, 0.0], [0.65, -2.2, -1.57, -1.57, 0.], [0.65, -2.2, 0.0, -0.85, 0.] ] ) arm.wait_for_motion_done() return 'done' smach.StateMachine.add("JOINT_PATH", CBState(_joint_path), transitions={'done': 'GO_TO_NEW_BIN'}) smach.StateMachine.add("GO_TO_NEW_BIN", ControlToTrashBin(robot=robot, trashbin_id=trashbin_designator.id, radius=0.4, yaw_offset=-0.2), transitions={"done": "PREPARE_AND_GRAB"}) smach.StateMachine.add("PREPARE_AND_GRAB", GrabTrash(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "succeeded", "failed": "ANNOUNCE_PICKUP_FAIL"}) smach.StateMachine.add("ANNOUNCE_PICKUP_FAIL", states.Say(robot, "Unfortunately I could not pick up the trash myself, let's go to" "plan B!", block=False), transitions={'spoken': 'ASK_HANDOVER'}) # Ask human to handover the trash bag smach.StateMachine.add("ASK_HANDOVER", HandoverFromHumanFigure(robot=robot, arm_designator=arm_designator, grabbed_entity_label='thrash'), transitions={"succeeded": "LOWER_ARM", "failed": "failed", "timeout": "TIMEOUT"}) arm_occupied_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={"required_goals": ["reset" ]}) smach.StateMachine.add("LOWER_ARM", states.ArmToJointConfig(robot=robot, arm_designator=arm_occupied_designator, configuration="reset"), transitions={"succeeded": "RECEIVED_TRASH_BAG", "failed": "RECEIVED_TRASH_BAG"}) smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw" " it away, please move away.", block=True), transitions={'spoken': 'succeeded'}) smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue", block=False), transitions={'spoken': "failed"})
def __init__(self, robot, selected_entity_designator, room_des): smach.StateMachine.__init__(self, outcomes=['done', 'failed']) store_entity_id_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") store_entity_des = ds.EdEntityDesignator(robot, id_designator=store_entity_id_des) selected_entity_type_des = ds.AttrDesignator(selected_entity_designator, "type", resolve_type=str) store_area_name_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6, "drop_pose") category_des = ds.VariableDesignator(resolve_type=str, name="category_des") with self: smach.StateMachine.add("SPEAK", robot_smach_states.Say(robot, ["I will pick-up the {object}", "Let's move the {object}"], object=selected_entity_type_des, block=True), transitions={"spoken": "GRAB"}) smach.StateMachine.add("GRAB", robot_smach_states.Grab(robot, selected_entity_designator, ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator")), transitions={"done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET"}) smach.StateMachine.add("ARM_RESET", robot_smach_states.ArmToJointConfig(robot, ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator"), "reset"), transitions={"succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED"}) smach.StateMachine.add('SAY_GRAB_SUCCESS', robot_smach_states.Say(robot, ["Now I am going to move this item", "Let's clean up this object", "Away with this one", "Everything will be cleaned"], block=False), transitions={"spoken": "GET_CATEGORY"}) smach.StateMachine.add('SAY_GRAB_FAILED', robot_smach_states.Say(robot, ["I could not grab the item.", "I failed to grasp the item", "I cannot reach the item", "Item grab failed"], block=False), transitions={"spoken": "failed"}) smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={"yes": "done", "no": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={"yes": "GET_CATEGORY", "no": "done"}) # # ROBOT # smach.StateMachine.add('GET_CATEGORY', # EntityToCategory(robot, selected_entity_designator, category_des.writeable), # transitions={"done": "DETERMINE_PLACE_LOCATION", # "failed": "NAVIGATE_TO_TRASH"}) # OPERATOR smach.StateMachine.add('GET_CATEGORY', OperatorToCategory(robot, category_des.writeable, room_des), transitions={"done": "DETERMINE_PLACE_LOCATION", "failed": "NAVIGATE_TO_TRASH"}) smach.StateMachine.add('DETERMINE_PLACE_LOCATION', CategoryToLocation(category_des, store_entity_id_des.writeable, store_area_name_des.writeable), transitions={"trashbin": "INSPECT_TRASH", "other": "PLACE_TO_STORE", "failed": "NAVIGATE_TO_TRASH"}) smach.StateMachine.add('NAVIGATE_TO_TRASH', robot_smach_states.NavigateToPlace(robot, trash_place_pose, ds.OccupiedArmDesignator(robot, {}, name="occupied_arm_designator")), transitions={"arrived": "PLACE_IN_TRASH", "unreachable": "SAY_PLACE_FAILED", "goal_not_defined": "SAY_PLACE_FAILED"}) smach.StateMachine.add('INSPECT_TRASH', robot_smach_states.Inspect(robot, store_entity_des), transitions={"done": "PLACE_IN_TRASH", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('PLACE_IN_TRASH', robot_smach_states.Place(robot, selected_entity_designator, trash_place_pose, ds.OccupiedArmDesignator(robot, {}, name="occupied_arm_designator")), transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('PLACE_TO_STORE', robot_smach_states.Place(robot, selected_entity_designator, store_entity_des, ds.OccupiedArmDesignator(robot, {}, name= "occupied_arm_designator"), "on_top_of"), transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('SAY_PLACE_SUCCESS', robot_smach_states.Say(robot, ["Bye bye!", "Yeah!", "Successfully disposed the item", "Another score for HERO"], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add('SAY_PLACE_FAILED', robot_smach_states.Say(robot, ["I could not cleanup the item.", "I cannot put the item in the trashbin", "Item cleanup failed"], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"})