def full_delivery():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'tagid', 'explore_radius' ])
# Open the container
with sm:
sm_search = sm_rfid_explore.sm_search()
smach.StateMachine.add(
'RFID_SEARCH', # outcomes: succeded, aborted, preempted
sm_search,
remapping = { 'tagid' : 'tagid', # input
'explore_radius' : 'explore_radius', # input
'explore_rfid_reads' : 'explore_rfid_reads' }, # output
transitions={'succeeded':'BEST_VANTAGE'})
sm_vantage = sm_next_best_vantage.sm_best_vantage()
smach.StateMachine.add(
'BEST_VANTAGE', # outcomes: succeeded, aborted, preempted
sm_vantage,
remapping = { 'tagid' : 'tagid', # input
'rfid_reads' : 'explore_rfid_reads' }, # input
transitions = {'succeeded':'DELIVERY'})
sm_delivery = sm_rfid_delivery.sm_delivery()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping = { 'tagid' : 'tagid'}, #input
transitions = { 'succeeded': 'succeeded' })
# Execute SMACH plan
return sm
def full_delivery():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tagid', 'explore_radius'])
# Open the container
with sm:
sm_search = sm_rfid_explore.sm_search()
smach.StateMachine.add(
'RFID_SEARCH', # outcomes: succeded, aborted, preempted
sm_search,
remapping={
'tagid': 'tagid', # input
'explore_radius': 'explore_radius', # input
'explore_rfid_reads': 'explore_rfid_reads'
}, # output
transitions={'succeeded': 'BEST_VANTAGE'})
sm_vantage = sm_next_best_vantage.sm_best_vantage()
smach.StateMachine.add(
'BEST_VANTAGE', # outcomes: succeeded, aborted, preempted
sm_vantage,
remapping={
'tagid': 'tagid', # input
'rfid_reads': 'explore_rfid_reads'
}, # input
transitions={'succeeded': 'DELIVERY'})
sm_delivery = sm_rfid_delivery.sm_delivery()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping={'tagid': 'tagid'}, #input
transitions={'succeeded': 'succeeded'})
# Execute SMACH plan
return sm
def cousins_demo():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'person_id' ])
with sm:
# Just a precation
tgoal = SingleJointPositionGoal()
tgoal.position = 0.040 # all the way up is 0.200, mostly down is 0.040
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': 'INIT_ARM_POSE' })
smach.StateMachine.add(
'INIT_ARM_POSE',
ServiceState( '/traj_playback/hfa_untuck',
TrajPlaybackSrv,
request = TrajPlaybackSrvRequest( False )),
transitions = { 'succeeded':'MOVE_GET_OBJ' })
go = MoveBaseGoal()
go.target_pose.header.frame_id = '/map'
go.target_pose.header.stamp = rospy.Time(0)
go.target_pose.pose.position.x = -5.07
go.target_pose.pose.position.y = 8.725
go.target_pose.pose.orientation.z = 0.926
go.target_pose.pose.orientation.w = 0.377
smach.StateMachine.add(
'MOVE_GET_OBJ',
SimpleActionState( '/move_base',
MoveBaseAction,
goal = go ),
transitions = {'succeeded':'READY_HANDOFF_INIT'})
smach.StateMachine.add(
'READY_HANDOFF_INIT',
PrintStr('Hand me an object [ENTER]'),
transitions = { 'succeeded':'HANDOFF_INIT' })
smach.StateMachine.add(
'HANDOFF_INIT',
ServiceState( 'rfid_handoff/initialize',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'CONFIRM_HANDOFF' })
smach.StateMachine.add(
'CONFIRM_HANDOFF',
ConfirmObj('Does the robot have the object [\'yes\' to proceed]?'),
transitions = { 'succeeded':'ROTATE_AFTER_HANDOFF',
'aborted':'READY_HANDOFF_INIT'} )
smach.StateMachine.add(
'ROTATE_AFTER_HANDOFF',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest( 3.14, 0.0)), # Full 180-deg spin.
transitions = { 'succeeded':'MOVE_DELIVERY' })
gd = MoveBaseGoal()
gd.target_pose.header.frame_id = '/map'
gd.target_pose.header.stamp = rospy.Time(0)
gd.target_pose.pose.position.x = 2.956
gd.target_pose.pose.position.y = 3.047
gd.target_pose.pose.orientation.z = 0.349
gd.target_pose.pose.orientation.w = 0.937
smach.StateMachine.add(
'MOVE_DELIVERY',
SimpleActionState( '/move_base',
MoveBaseAction,
goal = gd ),
transitions = {'succeeded':'DELIVERY'})
sm_delivery = sm_rfid_delivery.sm_delivery()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping = { 'tagid' : 'person_id'}, #input
transitions = { 'succeeded': 'TUCKL_SUCC',
'aborted': 'TUCKL_ABOR'})
smach.StateMachine.add(
'TUCKL_SUCC',
ServiceState( '/robotis/servo_left_pan_moveangle',
MoveAng,
request = MoveAngRequest( 1.350, 0.3, 0)),
transitions = { 'succeeded':'TUCKR_SUCC' })
smach.StateMachine.add(
'TUCKR_SUCC',
ServiceState( '/robotis/servo_right_pan_moveangle',
MoveAng,
request = MoveAngRequest( -1.350, 0.3, 0)),
transitions = { 'succeeded':'BACKUP_AFTER_DELIVERY' })
smach.StateMachine.add(
'TUCKL_ABOR',
ServiceState( '/robotis/servo_left_pan_moveangle',
MoveAng,
request = MoveAngRequest( 1.350, 0.3, 0)),
transitions = { 'succeeded':'TUCKR_ABOR' })
smach.StateMachine.add(
'TUCKR_ABOR',
ServiceState( '/robotis/servo_right_pan_moveangle',
MoveAng,
request = MoveAngRequest( -1.350, 0.3, 0)),
transitions = { 'succeeded':'aborted' })
smach.StateMachine.add(
'BACKUP_AFTER_DELIVERY',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest(0.0, -0.50)),
transitions = { 'succeeded':'ROTATE_AFTER_DELIVERY' })
smach.StateMachine.add(
'ROTATE_AFTER_DELIVERY',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest( 3.14, 0.0)),
transitions = { 'succeeded':'MOVE_FINAL' })
# Kitchen
# gk = MoveBaseGoal()
# gk.target_pose.header.frame_id = '/map'
# gk.target_pose.header.stamp = rospy.Time(0)
# gk.target_pose.pose.position.x = -1.61
# gk.target_pose.pose.position.y = 0.88
# gk.target_pose.pose.orientation.z = 0.91
# gk.target_pose.pose.orientation.w = 0.40
smach.StateMachine.add(
'MOVE_FINAL',
SimpleActionState( '/move_base',
MoveBaseAction,
goal = go ), # Back down the hallway
transitions = {'succeeded':'succeeded'})
return sm
def cousins_demo():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
input_keys = [ 'explore_id', # should be ''
'obj_id',
'person_id',
'explore_radius' ])
with sm:
sm_search = sm_rfid_explore.sm_search()
smach.StateMachine.add(
'RFID_SEARCH', # outcomes: succeded, aborted, preempted
sm_search,
remapping = { 'tagid' : 'explore_id', # input
'explore_radius' : 'explore_radius' },
transitions={'succeeded':'BEST_VANTAGE_OBJ'})
# Get best vantage for obj.
# The NextBestVantage was initialized in the search.
smach.StateMachine.add(
'BEST_VANTAGE_OBJ',
ServiceState( '/rfid_recorder/best_vantage',
NextBestVantage,
request_slots = ['tagid'],
response_slots = ['best_vantage']),
remapping = {'best_vantage':'best_vantage_obj', # output
'tagid':'obj_id'}, # input
transitions = {'succeeded':'MOVE_VANTAGE_OBJ'})
smach.StateMachine.add(
'MOVE_VANTAGE_OBJ',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'best_vantage_obj' }, # input
transitions = {'aborted':'BEST_VANTAGE_OBJ',
'preempted':'aborted',
'succeeded':'FETCH'})
# # FETCH
# smach.StateMachine.add('FETCH',PrintStr( 'Fetching object' ),
# transitions = { 'succeeded' : 'BEST_VANTAGE_PERSON' })
# Fetch
smach.StateMachine.add(
'FETCH',
sm_fetch.sm_fetch_object(),
remapping = {'tagid':'obj_id'},
transitions = {'succeeded':'POINT_HEAD',
'aborted':'BACKUP'})
# Backup (60cm)
smach.StateMachine.add(
'BACKUP',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request = RotateBackupSrvRequest(0.0, -0.50)),
transitions = { 'succeeded':'PRE_STOW' })
smach.StateMachine.add(
'PRE_STOW',
ServiceState( 'rfid_handoff/stow_grasp',
HandoffSrv,
request = HandoffSrvRequest()),
transitions = { 'succeeded':'POINT_HEAD' })
# Point Head Down (eventaully roll this and perceive table into own sm?)
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = 0.50
pgoal.target.point.z = 0.30
pgoal.min_duration = rospy.Duration(0.6)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
'POINT_HEAD',
SimpleActionState( '/head_traj_controller/point_head_action',
PointHeadAction,
goal = pgoal ),
transitions = { 'succeeded' : 'BEST_VANTAGE_PERSON' })
# Get best vantage for obj.
# The NextBestVantage was initialized in the search.
smach.StateMachine.add(
'BEST_VANTAGE_PERSON',
ServiceState( '/rfid_recorder/best_vantage',
NextBestVantage,
request_slots = ['tagid'],
response_slots = ['best_vantage']),
remapping = {'best_vantage':'best_vantage_person', # output
'tagid':'person_id'}, # input
transitions = {'succeeded':'MOVE_VANTAGE_PERSON'})
smach.StateMachine.add(
'MOVE_VANTAGE_PERSON',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'best_vantage_person' }, # input
transitions = {'aborted':'BEST_VANTAGE_PERSON',
'preempted':'aborted',
'succeeded':'DELIVERY'})
sm_delivery = sm_rfid_delivery.sm_delivery()
smach.StateMachine.add(
'DELIVERY', # outcomes: succeeded, aborted, preempted
sm_delivery,
remapping = { 'tagid' : 'person_id'}, #input
transitions = { 'succeeded': 'succeeded' })
return sm
