class SMRegistrationSetup(object):
def __init__(self):
self.sm_nav_approach = SMNavApproach()
self.sm_ell_reg = SMEllipsoidRegistration()
def get_sm(self):
sm = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])
with sm:
smach.StateMachine.add('NAV_APPROACH',
self.sm_nav_approach.get_sm(),
transitions={'succeeded' : 'TORSO_SETUP',
'shutdown' : 'aborted'})
# move torso up
tgoal = SingleJointPositionGoal()
tgoal.position = 0.300 # all the way up is 0.300
tgoal.min_duration = rospy.Duration( 2.0 )
tgoal.max_velocity = 1.0
smach.StateMachine.add(
'TORSO_SETUP',
SimpleActionState( 'torso_controller/position_joint_action',
SingleJointPositionAction,
goal = tgoal),
transitions = { 'succeeded': 'R_UNTUCK' })
# Untucks the right arm
smach.StateMachine.add(
'R_UNTUCK',
ServiceState( 'traj_playback/untuck_r_arm',
TrajPlaybackSrv,
request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
transitions = { 'succeeded' : 'R_ADJUST_MIRROR' })
# Adjusts the mirror
smach.StateMachine.add(
'R_ADJUST_MIRROR',
ServiceState( 'traj_playback/r_adjust_mirror',
TrajPlaybackSrv,
request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
transitions = { 'succeeded' : 'L_UNTUCK' })
# Untucks the left arm
smach.StateMachine.add(
'L_UNTUCK',
ServiceState( 'traj_playback/untuck_l_arm',
TrajPlaybackSrv,
request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
transitions = { 'succeeded' : 'HEAD_REG_ALL' })
smach.StateMachine.add('HEAD_REG_ALL',
self.sm_ell_reg.get_sm(),
transitions = { 'succeeded' : 'SETUP_TASK_CONTROLLER' })
smach.StateMachine.add(
'SETUP_TASK_CONTROLLER',
SetupTaskController())
return sm
def __init__(self):
self.sm_nav_approach = SMNavApproach()
self.sm_ell_reg = SMEllipsoidRegistration()
class SMRegistrationSetup(object):
def __init__(self):
self.sm_nav_approach = SMNavApproach()
self.sm_ell_reg = SMEllipsoidRegistration()
def get_sm(self):
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add('NAV_APPROACH',
self.sm_nav_approach.get_sm(),
transitions={
'succeeded': 'TORSO_SETUP',
'shutdown': 'aborted'
})
# move torso up
tgoal = SingleJointPositionGoal()
tgoal.position = 0.300 # all the way up is 0.300
tgoal.min_duration = rospy.Duration(2.0)
tgoal.max_velocity = 1.0
smach.StateMachine.add(
'TORSO_SETUP',
SimpleActionState('torso_controller/position_joint_action',
SingleJointPositionAction,
goal=tgoal),
transitions={'succeeded': 'R_UNTUCK'})
# Untucks the right arm
smach.StateMachine.add(
'R_UNTUCK',
ServiceState('traj_playback/untuck_r_arm',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(
False)), # if True, reverse trajectory
transitions={'succeeded': 'R_ADJUST_MIRROR'})
# Adjusts the mirror
smach.StateMachine.add(
'R_ADJUST_MIRROR',
ServiceState('traj_playback/r_adjust_mirror',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(
False)), # if True, reverse trajectory
transitions={'succeeded': 'L_UNTUCK'})
# Untucks the left arm
smach.StateMachine.add(
'L_UNTUCK',
ServiceState('traj_playback/untuck_l_arm',
TrajPlaybackSrv,
request=TrajPlaybackSrvRequest(
False)), # if True, reverse trajectory
transitions={'succeeded': 'HEAD_REG_ALL'})
smach.StateMachine.add(
'HEAD_REG_ALL',
self.sm_ell_reg.get_sm(),
transitions={'succeeded': 'SETUP_TASK_CONTROLLER'})
smach.StateMachine.add('SETUP_TASK_CONTROLLER',
SetupTaskController())
return sm
def __init__(self):
self.sm_nav_approach = SMNavApproach()
self.sm_ell_reg = SMEllipsoidRegistration()