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"
Beispiel #2
0
 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"
Beispiel #3
0
 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)
Beispiel #4
0
 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)
Beispiel #5
0
 def setUp(self):
     self.arm_ds = ds.ArmDesignator(self.robot,
                                    {'required_arm_name': 'leftArm'})
Beispiel #6
0
 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()
Beispiel #8
0
 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"
                                   })
Beispiel #10
0
    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"
                                   })