def execute(self, userdata=None): # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.OccupiedArmDesignator( self._robot, arm_properties={ "required_trajectories": ["prepare_place"], "required_goals": ["reset", "handover_to_human"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) resolved_arm = arm_designator.resolve() if resolved_arm is None: rospy.logwarn("No arm holding an entity") return "failed" sm = states.Place(robot=self._robot, item_to_place=item, place_pose=self._place_designator, arm=arm_designator) result = sm.execute() # If failed, do handover to human in order to continue if result != "done": sm = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) sm.execute() return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None): # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.OccupiedArmDesignator( self._robot, { "required_goals": ["reset", "handover_to_human"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) resolved_arm = arm_designator.resolve() if resolved_arm is None: rospy.logwarn("No arm holding an entity") return "failed" place = states.Place(robot=self._robot, item_to_place=item, place_pose=self.place_designator, arm=arm_designator) result = place.execute() # If failed, do handover to human in order to continue if result != "done": rospy.loginfo("{place} resulted in {out}".format(place=place, out=result)) handover = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) handover.execute() return "succeeded" if result == "done" else "failed"
def setUp(self): entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0) self.robot.arms["leftArm"].occupied_by = entity self.arm_ds = ds.OccupiedArmDesignator( self.robot, { "required_goals": ['handover_to_human', 'reset'], "required_gripper_types": [arms.GripperTypes.GRASPING] })
def __init__(self, robot, drop_zone_id): """ :param robot: robot object :param drop_designator: EdEntityDesignator designating the collection zone """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={}) with self: smach.StateMachine.add("GO_TO_COLLECTION_ZONE", states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id), radius=0.5), transitions={"arrived": "DROP_TRASH", "goal_not_defined": "aborted", "unreachable": "OPEN_DOOR_PLEASE"}) smach.StateMachine.add("OPEN_DOOR_PLEASE", states.Say(robot, "Can you please open the door for me? It seems blocked!"), transitions={"spoken": "WAIT_FOR_DOOR_OPEN"}) smach.StateMachine.add("WAIT_FOR_DOOR_OPEN", states.WaitTime(robot=robot, waittime=5), transitions={"waited": "GO_TO_COLLECTION_ZONE2", "preempted": "GO_TO_COLLECTION_ZONE2"}) smach.StateMachine.add("GO_TO_COLLECTION_ZONE2", states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id), radius=0.5), transitions={"arrived": "DROP_TRASH", "goal_not_defined": "aborted", "unreachable": "failed"}) smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "succeeded", "failed": "HANDOVER"}) smach.StateMachine.add("HANDOVER", states.HandoverToHuman(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "succeeded", "failed": "failed"})
def execute(self, userdata=None): # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.OccupiedArmDesignator(self._robot) resolved_arm = arm_designator.resolve() if resolved_arm is None: rospy.logwarn("No arm holding an entity") return "failed" sm = states.Place(robot=self._robot, item_to_place=item, place_pose=self._place_designator, arm=arm_designator) result = sm.execute() # If failed, do handover to human in order to continue if result != "done": sm = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) sm.execute() return "succeeded" if result == "done" else "failed"
def __init__(self, dummy_robot): smach.StateMachine.__init__(self, outcomes=[ "succeeded", "failed", ]) # Create designators dummy_trashbin_designator = ds.EdEntityDesignator( dummy_robot, id=CHALLENGE_KNOWLEDGE.trashbin_id2, name='trashbin_designator') dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {}) dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {}) with self: smach.StateMachine.add("PICK_UP_TRASH", PickUpTrash(dummy_robot, dummy_trashbin_designator, dummy_arm_designator_un), transitions={ "succeeded": "TURN_BASE", "failed": "TURN_BASE", "aborted": "failed" }) smach.StateMachine.add("TURN_BASE", states.ForceDrive(dummy_robot, 0, 0, 0.5, 3), transitions={"done": "DROP_TRASH"}) smach.StateMachine.add("DROP_TRASH", DropTrash(dummy_robot, dummy_arm_designator_oc), transitions={ "succeeded": "succeeded", "failed": "failed" })
def __init__(self, robot, selected_entity_designator, room_des): smach.StateMachine.__init__(self, outcomes=['done', 'failed']) store_entity_id_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") store_entity_des = ds.EdEntityDesignator( robot, id_designator=store_entity_id_des) selected_entity_type_des = ds.AttrDesignator( selected_entity_designator, "type", resolve_type=str) store_area_name_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6, "drop_pose") category_des = ds.VariableDesignator(resolve_type=str, name="category_des") with self: smach.StateMachine.add( "SPEAK", Say(robot, ["I will pick-up the {object}", "Let's move the {object}"], object=selected_entity_type_des, block=True), transitions={"spoken": "GRAB"}) smach.StateMachine.add("GRAB", Grab( robot, selected_entity_designator, ds.UnoccupiedArmDesignator( robot, arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] }, name="empty_arm_designator")), transitions={ "done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET" }) smach.StateMachine.add( "ARM_RESET", ArmToJointConfig( robot, ds.UnoccupiedArmDesignator( robot, arm_properties={"required_goals": ["reset"]}, name="empty_arm_designator"), "reset"), transitions={ "succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED" }) smach.StateMachine.add( 'SAY_GRAB_SUCCESS', Say(robot, [ "Now I am going to move this item", "Let's clean up this object", "Away with this one", "Everything will be cleaned" ], block=False), transitions={"spoken": "GET_CATEGORY"}) smach.StateMachine.add( 'SAY_GRAB_FAILED', Say(robot, [ "I could not grab the item.", "I failed to grasp the item", "I cannot reach the item", "Item grab failed" ], block=False), transitions={"spoken": "failed"}) smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={ "yes": "done", "no": "CHECK_ARM_OCCUPIED" }) smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={ "yes": "GET_CATEGORY", "no": "done" }) # # ROBOT # smach.StateMachine.add('GET_CATEGORY', # EntityToCategory(robot, selected_entity_designator, category_des.writeable), # transitions={"done": "DETERMINE_PLACE_LOCATION", # "failed": "NAVIGATE_TO_TRASH"}) # OPERATOR smach.StateMachine.add('GET_CATEGORY', OperatorToCategory(robot, category_des.writeable, room_des), transitions={ "done": "DETERMINE_PLACE_LOCATION", "failed": "NAVIGATE_TO_TRASH" }) smach.StateMachine.add( 'DETERMINE_PLACE_LOCATION', CategoryToLocation(category_des, store_entity_id_des.writeable, store_area_name_des.writeable), transitions={ "trashbin": "INSPECT_TRASH", "other": "PLACE_TO_STORE", "failed": "NAVIGATE_TO_TRASH" }) smach.StateMachine.add('NAVIGATE_TO_TRASH', NavigateToPlace( robot, trash_place_pose, ds.OccupiedArmDesignator( robot, {}, name="occupied_arm_designator")), transitions={ "arrived": "PLACE_IN_TRASH", "unreachable": "SAY_PLACE_FAILED", "goal_not_defined": "SAY_PLACE_FAILED" }) smach.StateMachine.add('INSPECT_TRASH', Inspect(robot, store_entity_des), transitions={ "done": "PLACE_IN_TRASH", "failed": "SAY_PLACE_FAILED" }) arm_properties_place = { "required_trajectories": ["prepare_place"], "required_gripper_types": [arms.GripperTypes.GRASPING] } arm_designator_place = ds.OccupiedArmDesignator( robot, arm_properties_place, name="occupied_arm_designator") smach.StateMachine.add('PLACE_IN_TRASH', Place(robot, selected_entity_designator, trash_place_pose, arm_designator_place), transitions={ "done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED" }) arm_designator_place_store = ds.OccupiedArmDesignator( robot, arm_properties_place, name="occupied_arm_designator") smach.StateMachine.add('PLACE_TO_STORE', Place(robot, selected_entity_designator, store_entity_des, arm_designator_place_store, "on_top_of"), transitions={ "done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED" }) smach.StateMachine.add( 'SAY_PLACE_SUCCESS', Say(robot, [ "Bye bye!", "Yeah!", "Successfully disposed the item", "Another score for {}".format(robot.robot_name) ], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add( 'SAY_PLACE_FAILED', Say(robot, [ "I could not cleanup the item.", "I cannot put the item in the trashbin", "Item cleanup failed" ], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"})
def execute(self, userdata=None): d = ds.OccupiedArmDesignator(self._robot, {}, name="empty_arm_designator2") if d.resolve(): return "yes" return "no"
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 = ds.EmptySpotDesignator(robot, trashbin_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={}) 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"})