예제 #1
0
def main(args):
    rospy.init_node("dryer_demo_node")
    global _listener
    _listener = tf.TransformListener()
    global _cloth_tracker
    _cloth_tracker = ClothTracker(disable_services=False)
    DryerNavigationUtils.loadLocations()
    sm = OuterStateMachine(DEFAULT_OUTCOMES)

    START_STATE = 'Go_To_Dryer'
    with sm:
        OuterStateMachine.add('Initialize', Initialize(), {
            SUCCESS: START_STATE,
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Go_To_Dryer', GoToDryer(), {
            SUCCESS: 'Open_Dryer',
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Open_Dryer', OpenDryer(), {
            SUCCESS: 'Find_Cloth',
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Find_Cloth', FindCloth(), {
            SUCCESS: 'Take_Out_Cloth',
            FAILURE: 'Final_State'
        })
        OuterStateMachine.add('Take_Out_Cloth', TakeOutCloth(), {
            SUCCESS: 'Go_To_Folding_Station',
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Go_To_Folding_Station', GoToFoldingStation(), {
            SUCCESS: 'Unfold_Cloth',
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Unfold_Cloth', UnfoldCloth(), {
            SUCCESS: 'Fold_Cloth',
            FAILURE: 'Unfold_Cloth'
        })
        OuterStateMachine.add('Fold_Cloth', FoldCloth(), {
            SUCCESS: SUCCESS,
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Go_To_Stacking_Station', GoToStackingStation(),
                              {
                                  SUCCESS: 'Stack_Cloth',
                                  FAILURE: FAILURE
                              })
        OuterStateMachine.add('Stack_Cloth', StackCloth(), {
            SUCCESS: 'Go_To_Dryer',
            FAILURE: FAILURE
        })
        OuterStateMachine.add('Final_State', FinalState(), {
            SUCCESS: SUCCESS,
            FAILURE: FAILURE
        })

    outcome = sm.execute()
예제 #2
0
def go_to_folding_station():
    print "Going to folding station"
    DryerNavigationUtils.dryerToTable1()
    GripUtils.go_to(arm="l",
                    x=0.5,
                    y=0,
                    z=0.35,
                    roll=0,
                    yaw=0,
                    pitch=pi / 2,
                    grip=True,
                    frame="table_height")
    GripUtils.open_gripper("l")
    return True
예제 #3
0
def main(args):
    rospy.init_node("dryer_demo_node")
    global _listener
    _listener = tf.TransformListener()
    global _cloth_tracker
    _cloth_tracker = ClothTracker(disable_services=False)
    DryerNavigationUtils.loadLocations()
    sm = OuterStateMachine(DEFAULT_OUTCOMES)

    START_STATE = 'Go_To_Dryer'
    with sm:
         OuterStateMachine.add('Initialize',                Initialize(),           {SUCCESS:START_STATE,FAILURE:FAILURE})
         OuterStateMachine.add('Go_To_Dryer',               GoToDryer(),            {SUCCESS:'Open_Dryer',FAILURE:FAILURE})
         OuterStateMachine.add('Open_Dryer',                OpenDryer(),            {SUCCESS:'Find_Cloth',FAILURE:FAILURE})
         OuterStateMachine.add('Find_Cloth',                FindCloth(),            {SUCCESS:'Take_Out_Cloth',FAILURE:'Final_State'})
         OuterStateMachine.add('Take_Out_Cloth',            TakeOutCloth(),         {SUCCESS:'Go_To_Folding_Station',FAILURE:FAILURE})
         OuterStateMachine.add('Go_To_Folding_Station',     GoToFoldingStation(),   {SUCCESS:'Unfold_Cloth',FAILURE:FAILURE})
         OuterStateMachine.add('Unfold_Cloth',              UnfoldCloth(),          {SUCCESS:'Fold_Cloth',FAILURE:'Unfold_Cloth'})
         OuterStateMachine.add('Fold_Cloth',                FoldCloth(),            {SUCCESS:SUCCESS,FAILURE:FAILURE})
         OuterStateMachine.add('Go_To_Stacking_Station',    GoToStackingStation(),  {SUCCESS:'Stack_Cloth',FAILURE:FAILURE})
         OuterStateMachine.add('Stack_Cloth',               StackCloth(),           {SUCCESS:'Go_To_Dryer',FAILURE:FAILURE})
         OuterStateMachine.add('Final_State',               FinalState(),           {SUCCESS:SUCCESS,FAILURE:FAILURE})
    
    outcome = sm.execute()
예제 #4
0
def take_out_cloth():
    print "Taking out cloth"
    RosUtils.call_service("move_torso",MoveTorso,height=0.01)
    DryerNavigationUtils.goToPosition("enter_dryer")
    GripUtils.go_to(x=-0.1,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=False,arm="l",frame="dryer")
    DryerNavigationUtils.goToPosition("into_dryer")
    GripUtils.go_to(x=0.15,y=-0.33,z=-0.65,roll=0,pitch=pi/2,yaw=0,grip=False,arm="l",frame="dryer")
    GripUtils.close_gripper("l")
    GripUtils.go_to(x=0.15,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=True,arm="l",frame="dryer")
    DryerNavigationUtils.goToPosition("enter_dryer")
    RosUtils.call_service("move_torso",MoveTorso,height=0.3)
    return True
예제 #5
0
def take_out_cloth():
    print "Taking out cloth"
    RosUtils.call_service("move_torso", MoveTorso, height=0.01)
    DryerNavigationUtils.goToPosition("enter_dryer")
    GripUtils.go_to(x=-0.1,
                    y=-0.33,
                    z=-0.4,
                    roll=0,
                    pitch=0,
                    yaw=0,
                    grip=False,
                    arm="l",
                    frame="dryer")
    DryerNavigationUtils.goToPosition("into_dryer")
    GripUtils.go_to(x=0.15,
                    y=-0.33,
                    z=-0.65,
                    roll=0,
                    pitch=pi / 2,
                    yaw=0,
                    grip=False,
                    arm="l",
                    frame="dryer")
    GripUtils.close_gripper("l")
    GripUtils.go_to(x=0.15,
                    y=-0.33,
                    z=-0.4,
                    roll=0,
                    pitch=0,
                    yaw=0,
                    grip=True,
                    arm="l",
                    frame="dryer")
    DryerNavigationUtils.goToPosition("enter_dryer")
    RosUtils.call_service("move_torso", MoveTorso, height=0.3)
    return True
예제 #6
0
def go_to_folding_station():
    print "Going to folding station"
    DryerNavigationUtils.dryerToTable1()
    GripUtils.go_to(arm="l",x=0.5,y=0,z=0.35,roll=0,yaw=0,pitch=pi/2,grip=True,frame="table_height")
    GripUtils.open_gripper("l")
    return True
예제 #7
0
def go_to_dryer():
    print "Going to dryer"
    DryerNavigationUtils.initToDryer()
    return True
예제 #8
0
def go_to_stacking_station():
    print "Going to the stacking station"
    DryerNavigationUtils.table1ToTable2(0.6)
    return True
예제 #9
0
def go_to_stacking_station():
    print "Going to the stacking station"
    DryerNavigationUtils.table1ToTable2(0.6)
    return True
예제 #10
0
def go_to_dryer():
    print "Going to dryer"
    DryerNavigationUtils.initToDryer()
    return True