def execute(self, userdata=None): arm = None # See if there's an arm holding something for k, v in self._robot.arms.iteritems(): if v.occupied_by is not None: arm = v break if arm is None: return "failed" # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.ArmDesignator(all_arms={arm.side: arm}, preferred_arm=arm) 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 setUp(self): self.arm_ds = ds.ArmDesignator( self.robot, { 'required_arm_name': 'leftArm', "required_gripper_types": [arms.GripperTypes.GRASPING] }) self.entity_label = "entity_label"
def setUp(self): self.arm_ds = ds.ArmDesignator( self.robot, { 'required_arm_name': 'leftArm', "required_gripper_types": [arms.GripperTypes.GRASPING] }) self.entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0) self.entity_ds = ds.VariableDesignator(self.entity)
def setUp(self): self.arm_ds = ds.ArmDesignator(self.robot, {'required_arm_name': 'leftArm'}) self.entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0) self.entity_ds = ds.VariableDesignator(self.entity)
def setUp(self): self.arm_ds = ds.ArmDesignator(self.robot, {'required_arm_name': 'leftArm'})
def test_invalid_arm_designator(self): invalid_arm_ds = ds.ArmDesignator( self.robot, {'required_arm_name': 'there_is_no_arm_with_this_name'}) state = states.SetGripper(self.robot, invalid_arm_ds, 'close') self.assertEqual(state.execute(), "failed")
parser = argparse.ArgumentParser( description= "Place an imaginary object at provided x, y, z coordinates using the " "'Place' smach state") parser.add_argument("x", type=float, help="x-coordinate (in map)") parser.add_argument("y", type=float, help="y-coordinate (in map)") parser.add_argument("z", type=float, help="z-coordinate (in map)") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_placing") robot = get_robot(args.robot) pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") item = Entity("dummy_id", "dummy_type", None, None, None, None, None, None) arm = ds.ArmDesignator(robot, arm_properties={ "required_trajectories": ["prepare_place"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) place_state = Place(robot, ds.Designator(item), ds.Designator(pose), arm) place_state.execute()
def setUp(self): self.arm_ds = ds.ArmDesignator( self.robot, { "required_gripper_types": [arms.GripperTypes.GRASPING], 'required_arm_name': 'leftArm' })
def __init__(self, robot, grasp_designator1, grasp_designator2, grasp_designator3, grasp_furniture_id1, grasp_furniture_id2, place_furniture_id): """ Constructor :param robot: robot object :param grasp_designator1: EdEntityDesignator designating the first item to grab. :param grasp_designator2: EdEntityDesignator designating the second item to grab. :param grasp_designator3: EdEntityDesignator designating the third item to grab. :param grasp_furniture_id1: string identifying the location where to grasp objects 1 and 2 :param grasp_furniture_id3: string identifying the location where to grasp object 3 :param place_furniture_id: string identifying the location where to place the objects """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators grasp_furniture_designator1 = ds.EntityByIdDesignator( robot, id=grasp_furniture_id1) grasp_furniture_designator2 = ds.EntityByIdDesignator( robot, id=grasp_furniture_id2) place_furniture_designator = ds.EntityByIdDesignator( robot, id=place_furniture_id) arm_designator = ds.ArmDesignator(robot, {}) place_designator = EmptySpotDesignator( robot=robot, place_location_designator=place_furniture_designator, arm_designator=arm_designator, area="on_top_of") with self: # Move to the inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE1", states.NavigateToSymbolic( robot, {grasp_furniture_designator1: "in_front_of"}, grasp_furniture_designator1), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE', 'unreachable': 'MOVE_TO_GRASP_SURFACE2', 'goal_not_defined': 'INSPECT_GRASP_SURFACE' }) # Backup for moving to inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE2", states.NavigateToSymbolic( robot, {grasp_furniture_designator1: "large_in_front_of"}, grasp_furniture_designator1), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE', 'unreachable': 'INSPECT_GRASP_SURFACE', 'goal_not_defined': 'INSPECT_GRASP_SURFACE' }) # Inspect grasp furniture smach.StateMachine.add("INSPECT_GRASP_SURFACE", states.Inspect( robot=robot, entityDes=grasp_furniture_designator1, objectIDsDes=None, searchArea="on_top_of", navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_1", "failed": "failed" }) # Grasp the first item smach.StateMachine.add("GRAB_ITEM_1", GrabSingleItem( robot=robot, grab_designator=grasp_designator1), transitions={ "succeeded": "GRAB_ITEM_2", "failed": "GRAB_ITEM_2" }) # Grasp the second item smach.StateMachine.add("GRAB_ITEM_2", GrabSingleItem( robot=robot, grab_designator=grasp_designator2), transitions={ "succeeded": "MOVE_TO_PLACE", "failed": "MOVE_TO_PLACE" }) # Move to the place location smach.StateMachine.add( "MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {place_furniture_designator: "in_front_of"}, place_furniture_designator), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) # Place the first item smach.StateMachine.add("PLACE_ITEM_1", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) # Place the second item smach.StateMachine.add("PLACE_ITEM_2", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "MOVE_TO_GRASP_SURFACE3", "failed": "MOVE_TO_GRASP_SURFACE3" }) # Move back to the grasp surface to grasp the third item smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE3", states.NavigateToSymbolic( robot, {grasp_furniture_designator2: "in_front_of"}, grasp_furniture_designator2), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE2', 'unreachable': 'MOVE_TO_GRASP_SURFACE4', 'goal_not_defined': 'INSPECT_GRASP_SURFACE2' }) # Backup for moving back to the grasp location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE4", states.NavigateToSymbolic( robot, {grasp_furniture_designator2: "large_in_front_of"}, grasp_furniture_designator2), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE2', 'unreachable': 'INSPECT_GRASP_SURFACE2', 'goal_not_defined': 'INSPECT_GRASP_SURFACE2' }) # Inspect grasp furniture smach.StateMachine.add("INSPECT_GRASP_SURFACE2", states.Inspect( robot=robot, entityDes=grasp_furniture_designator2, objectIDsDes=None, searchArea="shelf2", navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_3", "failed": "failed" }) # Grasp the third item smach.StateMachine.add("GRAB_ITEM_3", GrabSingleItem( robot=robot, grab_designator=grasp_designator3), transitions={ "succeeded": "MOVE_TO_PLACE_3", "failed": "MOVE_TO_PLACE_3" }) # Move to the place location smach.StateMachine.add( "MOVE_TO_PLACE_3", states.NavigateToSymbolic( robot, {place_furniture_designator: "in_front_of"}, place_furniture_designator), transitions={ 'arrived': 'PLACE_ITEM_3', 'unreachable': 'PLACE_ITEM_3', 'goal_not_defined': 'PLACE_ITEM_3' }) # Place the first item smach.StateMachine.add("PLACE_ITEM_3", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "succeeded", "failed": "succeeded" })
def __init__(self, robot, grab_designator_1=None, grab_designator_2=None, place_designator=None, pdf_writer=None): """ Constructor :param robot: robot object :param grab_designator_1: EdEntityDesignator designating the item to grab :param grab_designator_2: EdEntityDesignator designating the item to grab :param pdf_writer: WritePDF object to save images of recognized objects to pdf files """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators self.table_designator = ds.EntityByIdDesignator( robot, id="temp") # will be updated later on if grab_designator_1 is None: grab_designator_1 = DefaultGrabDesignator( robot=robot, surface_designator=self.table_designator, area_description=GRAB_SURFACE) if grab_designator_2 is None: grab_designator_2 = DefaultGrabDesignator( robot=robot, surface_designator=self.table_designator, area_description=GRAB_SURFACE) self.cabinet = ds.EntityByIdDesignator( robot, id="temp") # will be updated later on self.place_entity_designator = ds.EdEntityDesignator(robot=robot, id="temp") self.arm_designator = ds.ArmDesignator(robot, {}) self.place_designator = EmptySpotDesignator( robot=robot, place_location_designator=self.place_entity_designator, arm_designator=self.arm_designator, area="temp") self.placeaction1 = PlaceSingleItem( robot=robot, place_designator=self.place_designator) self.placeaction2 = PlaceSingleItem( robot=robot, place_designator=self.place_designator) with self: smach.StateMachine.add("MOVE_TO_TABLE1", states.NavigateToSymbolic( robot, {self.table_designator: "in_front_of"}, self.table_designator), transitions={ 'arrived': 'INSPECT_TABLE', 'unreachable': 'MOVE_TO_TABLE2', 'goal_not_defined': 'INSPECT_TABLE' }) smach.StateMachine.add("MOVE_TO_TABLE2", states.NavigateToSymbolic( robot, {self.table_designator: "in_front_of"}, self.table_designator), transitions={ 'arrived': 'INSPECT_TABLE', 'unreachable': 'INSPECT_TABLE', 'goal_not_defined': 'INSPECT_TABLE' }) if pdf_writer: # Designator to store the classificationresults # The Inspect-state (INSPECT_TABLE) gathers a list of ClassificationResults for Entities on the table # These are passed to the pdf_writer class_designator = ds.VariableDesignator( [], resolve_type=[ robot_skills.classification_result.ClassificationResult ]) # Add the designator to the pdf writer state pdf_writer.set_designator(class_designator) smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=self.table_designator, objectIDsDes=class_designator, searchArea=GRAB_SURFACE, navigation_area="in_front_of"), transitions={ "done": "WRITE_PDF", "failed": "failed" }) smach.StateMachine.add("WRITE_PDF", pdf_writer, transitions={"done": "GRAB_ITEM_1"}) else: smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=self.table_designator, objectIDsDes=None, searchArea=GRAB_SURFACE, navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_1", "failed": "failed" }) smach.StateMachine.add("GRAB_ITEM_1", GrabSingleItem( robot=robot, grab_designator=grab_designator_1), transitions={ "succeeded": "GRAB_ITEM_2", "failed": "GRAB_ITEM_2" }) smach.StateMachine.add("GRAB_ITEM_2", GrabSingleItem( robot=robot, grab_designator=grab_designator_2), transitions={ "succeeded": "MOVE_TO_PLACE", "failed": "MOVE_TO_PLACE" }) smach.StateMachine.add("MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {self.cabinet: "in_front_of"}, self.cabinet), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) smach.StateMachine.add("PLACE_ITEM_1", self.placeaction1, transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) smach.StateMachine.add("PLACE_ITEM_2", self.placeaction2, transitions={ "succeeded": "succeeded", "failed": "failed" })