def cam_capture():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['bagfile_name', 'bagfile_topics'])
with sm:
smach.StateMachine.add('START_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request_slots=['topics', 'dest']),
remapping={
'topics': 'bagfile_topics',
'dest': 'bagfile_name'
},
transitions={'succeeded': 'DELAY'})
smach.StateMachine.add('DELAY',
DelayState(),
transitions={'succeeded': 'STOP_BAG_CAPTURE'})
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'succeeded'})
return sm
def head_capture():
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['bagfile_name', 'bagfile_topics'])
with sm:
def PointAdd(x, y, z, dur, state, res):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration(dur)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': res})
return
PointAdd(0.00, -1.00, -0.60, 5.0, 'PH1', 'START_BAG_CAPTURE')
smach.StateMachine.add('START_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request_slots=['topics', 'dest']),
remapping={
'topics': 'bagfile_topics',
'dest': 'bagfile_name'
},
transitions={'succeeded': 'PH2'})
PointAdd(0.00, 1.00, -0.60, 15.0, 'PH2', 'PH3')
PointAdd(0.00, 1.00, -0.20, 3.0, 'PH3', 'PH4')
PointAdd(0.00, -1.00, -0.20, 15.0, 'PH4', 'PH5')
PointAdd(0.00, -1.00, 0.30, 3.0, 'PH5', 'PH6')
PointAdd(0.00, 1.00, 0.30, 15.0, 'PH6', 'PH7')
PointAdd(1.00, 0.00, 0.00, 7.5, 'PH7', 'STOP_BAG_CAPTURE')
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'succeeded'})
return sm
def sm_rfid_servo_approach( yaml_fname ):
# Create a SMACH state machine
sm = smach.StateMachine( outcomes = ['succeeded','aborted','preempted'])
# Open the container
with sm:
smach.StateMachine.add(
'CAPTURE_MONITOR',
rdut.YAMLproc( yaml_fname ),
remapping = {'next_move_pose':'next_move_pose', # output
'bagfile_name':'bagfile_name', # output
'bagfile_topics':'bagfile_topics', # output
'tagid':'tagid'}, # output
transitions = {'aborted':'succeeded',
'succeeded':'MOVE_POSITION'})
smach.StateMachine.add(
'MOVE_POSITION',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = [ 'target_pose' ]),
remapping = { 'target_pose' : 'next_move_pose' }, # input
transitions = {'aborted':'MANUAL_SKIP',
'preempted':'aborted',
'succeeded':'START_BAG_CAPTURE'})
smach.StateMachine.add(
'MANUAL_SKIP',
rdut.ManualSkip(),
transitions = {'succeeded':'START_BAG_CAPTURE', # We already manually positioned the robot
'aborted':'CAPTURE_MONITOR'}) # skip this position and go to next
smach.StateMachine.add(
'START_BAG_CAPTURE',
ServiceState( '/bag_cap/capture',
BagCapture,
request_slots = ['topics','dest'] ),
remapping = {'topics':'bagfile_topics',
'dest':'bagfile_name'},
transitions = {'succeeded':'SERVO'})
# Servoing is a basic state machine. Success means servoing finished @ obs.
smach.StateMachine.add(
'SERVO',
SimpleActionState( '/rfid_servo/servo_act',
ServoAction,
goal_slots = ['tagid']), #goal = ServoGoal( 'person ' ),
transitions = { 'succeeded': 'STOP_BAG_CAPTURE' },
remapping = {'tagid':'tagid'}) # input
smach.StateMachine.add(
'STOP_BAG_CAPTURE',
ServiceState( '/bag_cap/capture',
BagCapture,
request = BagCaptureRequest('','') ),
transitions = {'succeeded':'TUCK_LEFT'})
# Tuck Left (non-blocking)
smach.StateMachine.add(
'TUCK_LEFT',
ServiceState( 'robotis/servo_left_pan_moveangle',
MoveAng,
request = MoveAngRequest( 1.350, 0.2, 0 )), # ang (float), angvel (float), blocking (bool)
transitions = {'succeeded':'TUCK_RIGHT'})
# Tuck Right (non-blocking)
smach.StateMachine.add(
'TUCK_RIGHT',
ServiceState( 'robotis/servo_right_pan_moveangle',
MoveAng,
request = MoveAngRequest( -1.350, 0.2, 0 )), # ang (float), angvel (float), blocking (bool)
transitions = {'succeeded':'CAPTURE_MONITOR'})
return sm
transitions={'succeeded': 'SEARCH'})
sm_search = sm_rfid_explore.sm_search()
smach.StateMachine.add('SEARCH',
sm_search,
transitions={'succeeded': 'STOP_BAG_CAPTURE'},
remapping={
'tagid': 'tagid',
'explore_radius': 'explore_radius'
})
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'succeeded'})
rospy.init_node('smach_datacap_rfid_explore')
rec = recorder.Recorder(serv_name='temp_recorder',
node_name='temp_recorder_py')
rec.process_service(None) # start recording
sm.userdata.tagid = ''
sm.userdata.explore_radius = opt.radius
sm.userdata.bagfile_name = fname
sm.userdata.bagfile_topics = '/tf /visarr /rfid/ears_reader /rfid/ears_reader_arr /map /robot_pose_ekf/odom_combined'
outcome = sm.execute()
def sm_cap_360(yaml_fname):
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['track_mode'])
# Open the container
with sm:
# Lots of head moving -- abstract into function
def PointAdd(x, y, z, dur, state, res):
pgoal = PointHeadGoal()
pgoal.target.header.frame_id = '/torso_lift_link'
pgoal.target.point.x = x
pgoal.target.point.y = y
pgoal.target.point.z = z
pgoal.min_duration = rospy.Duration(dur)
pgoal.max_velocity = 1.0
smach.StateMachine.add(
state,
SimpleActionState('/head_traj_controller/point_head_action',
PointHeadAction,
goal=pgoal),
transitions={'succeeded': res})
return
PointAdd(1.0, 0.0, 0.35, 5.0, 'INIT_HEAD',
'CAP_START') # Go to safe initial conditions
PointAdd(-1.0, -0.25, 0.35, 7.0, 'CAP_START',
'CAPTURE_POSITIONS') # Prepare for lots of "neck craning"
smach.StateMachine.add(
'CAPTURE_POSITIONS',
rdut.YAMLprocPoses(yaml_fname),
remapping={'next_move_pose': 'next_move_pose'}, # output
transitions={
'aborted': 'succeeded',
'succeeded': 'READY_MOVE'
})
smach.StateMachine.add('READY_MOVE',
rdut.MoveNotify(),
transitions={'succeeded': 'MOVE_POSITION'})
smach.StateMachine.add(
'MOVE_POSITION',
SimpleActionState('/move_base',
MoveBaseAction,
goal_slots=['target_pose']),
remapping={'target_pose': 'next_move_pose'}, # input
transitions={
'aborted': 'MANUAL_SKIP',
'preempted': 'aborted',
'succeeded': 'CAPTURE_TAGS'
})
smach.StateMachine.add(
'MANUAL_SKIP',
rdut.ManualSkip(),
transitions={
'succeeded':
'CAPTURE_TAGS', # We already manually positioned the robot
'aborted': 'CAPTURE_POSITIONS'
}) # skip this position and go to next
# This isn't realy necessary, but it provides a nice way to reuse code.
smach.StateMachine.add(
'CAPTURE_TAGS',
rdut.YAMLprocMultitag(yaml_fname),
remapping={
'bagfile_name': 'bagfile_name', # output
'bagfile_topics': 'bagfile_topics', # output
'panrate': 'panrate',
'tagid': 'tagid',
'tilt_left': 'tilt_left',
'tilt_right': 'tilt_right',
'tilt_rate': 'tilt_rate',
'tilt_block': 'tilt_block'
}, # output
transitions={
'aborted': 'CAPTURE_POSITIONS', # move to next location
'succeeded': 'START_BAG_CAPTURE'
}) # capture bag
smach.StateMachine.add('START_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request_slots=['topics', 'dest']),
remapping={
'topics': 'bagfile_topics',
'dest': 'bagfile_name'
},
transitions={'succeeded': 'RFID_START'})
# Initialize RFID reader!
# rosservice call /rfid/head_mode -- ['track','OrangeMedBot']
# Am having issues with service states with request_cb, so just making my own....
smach.StateMachine.add('RFID_START',
RfidStart('/rfid/head_mode'),
remapping={'tagid': 'tagid'},
transitions={'succeeded': 'LOOK_LEFT'})
PointAdd(-1.0, 0.25, 0.35, 27.0, 'LOOK_LEFT', 'RFID_STOP')
smach.StateMachine.add('RFID_STOP',
RfidStop('/rfid/head_mode'),
transitions={'succeeded': 'STOP_BAG_CAPTURE'})
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'LOOK_RIGHT'})
PointAdd(-1.0, -0.25, 0.35, 8.0, 'LOOK_RIGHT', 'CAPTURE_TAGS')
return sm
def sm_rfid_servo_approach(yaml_fname):
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with sm:
smach.StateMachine.add(
'CAPTURE_POSITIONS',
rdut.YAMLprocPoses(yaml_fname),
remapping={'next_move_pose': 'next_move_pose'}, # output
transitions={
'aborted': 'succeeded',
'succeeded': 'READY_MOVE'
})
smach.StateMachine.add('READY_MOVE',
rdut.MoveNotify(),
transitions={'succeeded': 'MOVE_POSITION'})
smach.StateMachine.add(
'MOVE_POSITION',
SimpleActionState('/move_base',
MoveBaseAction,
goal_slots=['target_pose']),
remapping={'target_pose': 'next_move_pose'}, # input
transitions={
'aborted': 'MANUAL_SKIP',
'preempted': 'aborted',
'succeeded': 'CAPTURE_TAGS'
})
smach.StateMachine.add(
'MANUAL_SKIP',
rdut.ManualSkip(),
transitions={
'succeeded':
'CAPTURE_TAGS', # We already manually positioned the robot
'aborted': 'CAPTURE_POSITIONS'
}) # skip this position and go to next
smach.StateMachine.add(
'CAPTURE_TAGS',
rdut.YAMLprocMultitag(yaml_fname),
remapping={
'bagfile_name': 'bagfile_name', # output
'bagfile_topics': 'bagfile_topics', # output
'panrate': 'panrate',
'tagid': 'tagid',
'tilt_left': 'tilt_left',
'tilt_right': 'tilt_right',
'tilt_rate': 'tilt_rate',
'tilt_block': 'tilt_block'
}, # output
transitions={
'aborted': 'CAPTURE_POSITIONS', # move to next location
'succeeded': 'TILT_LEFT'
}) # capture bag
smach.StateMachine.add(
'TILT_LEFT',
ServiceState('/robotis/servo_left_tilt_moveangle',
MoveAng,
request_slots=['angle', 'angvel', 'blocking']),
transitions={'succeeded': 'TILT_RIGHT'},
remapping={
'angle': 'tilt_left',
'angvel': 'tilt_rate',
'blocking': 'tilt_block'
})
smach.StateMachine.add(
'TILT_RIGHT',
ServiceState('/robotis/servo_right_tilt_moveangle',
MoveAng,
request_slots=['angle', 'angvel', 'blocking']),
transitions={'succeeded': 'START_BAG_CAPTURE'},
remapping={
'angle': 'tilt_right',
'angvel': 'tilt_rate',
'blocking': 'tilt_block'
})
smach.StateMachine.add('START_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request_slots=['topics', 'dest']),
remapping={
'topics': 'bagfile_topics',
'dest': 'bagfile_name'
},
transitions={'succeeded': 'FLAP'})
# Servoing is a basic state machine. Success means servoing finished @ obs.
smach.StateMachine.add(
'FLAP',
ServiceState('/rfid_orient/flap',
FlapEarsSrv,
request_slots=['data', 'panrate']),
transitions={'succeeded': 'STOP_BAG_CAPTURE'},
remapping={
'data': 'tagid', # input
'panrate': 'panrate'
}) # input
smach.StateMachine.add('STOP_BAG_CAPTURE',
ServiceState('/bag_cap/capture',
BagCapture,
request=BagCaptureRequest('', '')),
transitions={'succeeded': 'TUCK_LEFT'})
# Tuck Left (non-blocking)
smach.StateMachine.add(
'TUCK_LEFT',
ServiceState(
'robotis/servo_left_pan_moveangle',
MoveAng,
request=MoveAngRequest(
1.250, 0.2,
0)), # ang (float), angvel (float), blocking (bool)
transitions={'succeeded': 'TUCK_RIGHT'})
# Tuck Right (non-blocking)
smach.StateMachine.add(
'TUCK_RIGHT',
ServiceState(
'robotis/servo_right_pan_moveangle',
MoveAng,
request=MoveAngRequest(
-1.250, 0.2,
0)), # ang (float), angvel (float), blocking (bool)
transitions={'succeeded': 'CAPTURE_TAGS'})
return sm