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 = [ '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
})
return sm
if __name__ == '__main__':
import sm_rfid_explore
rospy.init_node('smach_rfid_explore')
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':'succeeded'})
transitions={'succeeded': 'BEST_VANTAGE'})
sm_vantage = sm_best_vantage()
smach.StateMachine.add(
'BEST_VANTAGE', # outcomes: succeeded, aborted, preempted
sm_vantage,
return sm
if __name__ == '__main__':
import sm_rfid_explore
rospy.init_node('smach_rfid_explore')
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':'succeeded'})
transitions={'succeeded':'BEST_VANTAGE'})
sm_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
