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()
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
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()
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
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
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
def go_to_dryer():
print "Going to dryer"
DryerNavigationUtils.initToDryer()
return True
def go_to_stacking_station():
print "Going to the stacking station"
DryerNavigationUtils.table1ToTable2(0.6)
return True
def go_to_stacking_station():
print "Going to the stacking station"
DryerNavigationUtils.table1ToTable2(0.6)
return True
def go_to_dryer():
print "Going to dryer"
DryerNavigationUtils.initToDryer()
return True