Пример #1
0
 def with_area():
     esd = ds.EmptySpotDesignator(
         robot=robot,
         place_location_designator=furniture_designator,
         name="with_area",
         area="on_top_of")
     print(esd.resolve())
Пример #2
0
    def __init__(self, robot, place_designator=None):
        """ Constructor

        :param robot: robot object
        :param place_designator: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator
        """
        smach.State.__init__(self, outcomes=["succeeded", "failed"])

        self._robot = robot
        if place_designator is not None:
            self._place_designator = place_designator
        else:
            place_entity_designator = ds.EdEntityDesignator(
                robot=robot, id=DEFAULT_PLACE_ENTITY)
            self._place_designator = ds.EmptySpotDesignator(
                robot=robot,
                place_location_designator=place_entity_designator,
                area=DEFAULT_PLACE_AREA)
Пример #3
0
    def __init__(self, robot, grasp_designator1, grasp_designator2,
                 grasp_designator3, grasp_furniture_id1, grasp_furniture_id3,
                 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_designator3 = ds.EntityByIdDesignator(
            robot, id=grasp_furniture_id3)

        place_furniture_designator = ds.EntityByIdDesignator(
            robot, id=place_furniture_id)
        place_designator = ds.EmptySpotDesignator(
            robot=robot,
            place_location_designator=place_furniture_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_designator3: "in_front_of"},
                    grasp_furniture_designator3),
                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_designator3: "large_in_front_of"},
                    grasp_furniture_designator3),
                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_designator3,
                                       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):
        smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])

        self.target_destination = ds.EntityByIdDesignator(
            robot, id=challenge_knowledge.default_place)

        self.car_waypoint = ds.EntityByIdDesignator(
            robot, id=challenge_knowledge.waypoint_car['id'])

        self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(
            robot,
            self.target_destination,
            name="placement",
            area=challenge_knowledge.default_area),
                                                   name="place_position")

        self.empty_arm_designator = ds.UnoccupiedArmDesignator(
            robot, {}, 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()

        # 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': 'FOLLOW_OPERATOR'})

            # Use NEW:
            smach.StateMachine.add('FOLLOW_OPERATOR',
                                   states.FollowOperator2(robot),
                                   transitions={
                                       'Done': 'ASK_FOR_TASK',
                                       'Failed': 'ASK_FOR_TASK',
                                       'Aborted': 'FOLLOW_OPERATOR'
                                   })

            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',
                                   states.HearOptions(robot, ['yes', 'no']),
                                   transitions={
                                       'no': 'FOLLOW_OPERATOR',
                                       'yes': 'CONFIRM_CAR_LOCATION',
                                       'no_result': 'ASK_FOR_TASK'
                                   })

            smach.StateMachine.add(
                'CONFIRM_CAR_LOCATION',
                states.Say(
                    robot,
                    ["OK, I will remember this location as the car location."],
                    block=True,
                    look_at_standing_person=True),
                transitions={'spoken': 'REMEMBER_CAR_LOCATION'})

            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'
                })
            #
            # smach.StateMachine.add('CONFIRM_DESTINATION',
            #                        states.Say(robot, [
            #                            "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)],
            #                                   block=True,
            #                                   look_at_standing_person=True),
            #                        transitions={'spoken': 'GRAB_ITEM'})

            # 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'})
                states.Say(robot, [
                    "I can't pick up the groceries since I don't have arms. Please place them in my basket."
                ],
                           block=True,
                           look_at_standing_person=True),
                transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'})

            smach.StateMachine.add('WAIT_FOR_GRAB_ITEM',
                                   states.WaitTime(robot),
                                   transitions={
                                       'waited': 'SAY_GOING_TO_ROOM',
                                       'preempted': 'Aborted'
                                   })

            # 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.NavigateToSymbolic(
                    robot, {self.target_destination: "in_front_of"},
                    self.target_destination),
                transitions={
                    'arrived': 'PUTDOWN_ITEM',
                    'unreachable': 'TURN_180_TO_REPLAN',
                    # implement avoid obstacle behaviour later
                    'goal_not_defined': 'Aborted'
                })

            smach.StateMachine.add(
                'TURN_180_TO_REPLAN',
                hmc_states.TurnToReplan(robot),
                transitions={
                    'success': 'GOTO_DESTINATION_BACKUP',
                    'abort': 'GOTO_DESTINATION_BACKUP',
                    # implement avoid obstacle behaviour later
                    #'goal_not_defined': 'Aborted'})
                })

            smach.StateMachine.add(
                'GOTO_DESTINATION_BACKUP',
                states.NavigateToSymbolic(
                    robot, {self.target_destination: "in_front_of"},
                    self.target_destination),
                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),
                states.Say(robot, [
                    "I can't put the groceries down since I have no arms. Please take them from my basket and put it down."
                ],
                           block=True,
                           look_at_standing_person=True),
                transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'})

            smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM',
                                   states.WaitTime(robot),
                                   transitions={
                                       'waited': 'ASKING_FOR_HELP',
                                       'preempted': 'Aborted'
                                   })

            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'})  #'LEARN_OPERATOR'})

            # smach.StateMachine.add('LEARN_OPERATOR',
            #                        hmc_states.LearnOperator(robot),
            #                        transitions={'learned': 'GOTO_CAR',
            #                                     'failed': '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")
Пример #5
0
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
Пример #6
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.place_designator = ds.EmptySpotDesignator(
            robot=robot,
            place_location_designator=self.place_entity_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: "large_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"
                                   })
 def without_area():
     esd = ds.EmptySpotDesignator(
         robot=robot,
         place_location_designator=furniture_designator,
         name=None)
     print esd.resolve()
Пример #8
0
    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"})