def get_nav_approach(self):
def child_term_cb(outcome_map):
return True
def out_cb(outcome_map):
if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached':
return 'succeeded'
return 'shutdown'
sm_nav_approach_state = smach.Concurrence(
outcomes=['succeeded', 'shutdown'],
default_outcome='shutdown',
child_termination_cb=child_term_cb,
outcome_cb=out_cb,
output_keys=['nav_dist'])
with sm_nav_approach_state:
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.Concurrence.add(
'MOVE_FORWARD',
SimpleActionState('/approach_table/move_forward_act',
ApproachAction,
goal=approach_goal))
smach.Concurrence.add(
'DETECT_FORWARD_DISTANCE',
DetectForwardDistance(self.get_transform,
self.nav_approach_dist))
return sm_nav_approach_state
def get_nav_approach(self):
def child_term_cb(outcome_map):
return True
def out_cb(outcome_map):
if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached':
return 'succeeded'
return 'shutdown'
sm_nav_approach_state = smach.Concurrence(
outcomes=['succeeded', 'shutdown'],
default_outcome='shutdown',
child_termination_cb=child_term_cb,
outcome_cb=out_cb,
output_keys=['nav_dist'])
with sm_nav_approach_state:
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.Concurrence.add(
'MOVE_FORWARD',
SimpleActionState( '/approach_table/move_forward_act',
ApproachAction,
goal = approach_goal ))
smach.Concurrence.add(
'DETECT_FORWARD_DISTANCE',
DetectForwardDistance(self.get_transform, self.nav_approach_dist))
return sm_nav_approach_state
def sm_approach_table(listener=None):
# for various states, you need a tf listener, but only one per thread supported.
if listener == None:
try:
rospy.init_node('sm_approach')
except:
rospy.logout(
'sm_approach_table: Node already initialized elsewhere')
listener = tf.TransformListener()
# Python only allows one listener per thread. This function has two (or more)
# classes that require the listener. You can pass in one from higher in the
# hierarchy if you prefer.
# Create a SMACH state machine
sm = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['table_edge_poses'], # type PoseArray
output_keys=[
'movebase_pose_global', # type PoseStamped
'table_edge_global'
]) # type PoseStamped
# Open the container
with sm:
smach.StateMachine.add(
'POSE_SELECTION',
PoseSelection(listener=listener),
transitions={'succeeded': 'MOVE_BASE'},
remapping={
'candidate_poses': 'table_edge_poses', # input (PoseArray)
'selected_pose_global':
'table_edge_global', # output (PoseStamped)
'movebase_pose_global': 'movebase_pose_global'
}) # output (PoseStamped)
smach.StateMachine.add(
'MOVE_BASE',
SimpleActionState(
'/move_base',
MoveBaseAction,
goal_slots=['target_pose'], # PoseStamped
outcomes=['succeeded', 'aborted', 'preempted']),
transitions={
'succeeded': 'CHECK_HEADING',
'aborted': 'POSE_SELECTION'
},
remapping={'target_pose':
'movebase_pose_global'}) # input (PoseStamped)
smach.StateMachine.add(
'CHECK_HEADING',
CheckHeading(listener=listener),
transitions={'succeeded': 'ADJUST_HEADING'},
remapping={
'target_pose': 'movebase_pose_global', # input (PoseStamped)
'angular_error': 'angular_error'
}) # output (float)
smach.StateMachine.add(
'ADJUST_HEADING',
ServiceState('/rotate_backup',
RotateBackupSrv,
request_slots=['rotate']), # float (displace = 0.0)
transitions={'succeeded': 'MOVE_FORWARD'},
remapping={'rotate': 'angular_error'})
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.StateMachine.add('MOVE_FORWARD',
SimpleActionState(
'/approach_table/move_forward_act',
ApproachAction,
goal=approach_goal),
transitions={'succeeded': 'succeeded'})
return sm
def sm_approach_table( listener = None ):
# for various states, you need a tf listener, but only one per thread supported.
if listener == None:
try:
rospy.init_node( 'sm_approach' )
except:
rospy.logout( 'sm_approach_table: Node already initialized elsewhere' )
listener = tf.TransformListener()
# Python only allows one listener per thread. This function has two (or more)
# classes that require the listener. You can pass in one from higher in the
# hierarchy if you prefer.
# Create a SMACH state machine
sm = smach.StateMachine( outcomes = ['succeeded','aborted','preempted'],
input_keys = ['table_edge_poses'], # type PoseArray
output_keys = ['movebase_pose_global', # type PoseStamped
'table_edge_global']) # type PoseStamped
# Open the container
with sm:
smach.StateMachine.add(
'POSE_SELECTION',
PoseSelection( listener = listener ),
transitions = { 'succeeded' : 'MOVE_BASE' },
remapping = {'candidate_poses':'table_edge_poses', # input (PoseArray)
'selected_pose_global':'table_edge_global', # output (PoseStamped)
'movebase_pose_global':'movebase_pose_global'}) # output (PoseStamped)
smach.StateMachine.add(
'MOVE_BASE',
SimpleActionState( '/move_base',
MoveBaseAction,
goal_slots = ['target_pose'], # PoseStamped
outcomes = ['succeeded','aborted','preempted']),
transitions = { 'succeeded' : 'CHECK_HEADING',
'aborted' : 'POSE_SELECTION' },
remapping = {'target_pose':'movebase_pose_global'}) # input (PoseStamped)
smach.StateMachine.add(
'CHECK_HEADING',
CheckHeading( listener = listener ),
transitions = { 'succeeded':'ADJUST_HEADING' },
remapping = { 'target_pose':'movebase_pose_global', # input (PoseStamped)
'angular_error':'angular_error' }) # output (float)
smach.StateMachine.add(
'ADJUST_HEADING',
ServiceState( '/rotate_backup',
RotateBackupSrv,
request_slots = ['rotate']), # float (displace = 0.0)
transitions = { 'succeeded':'MOVE_FORWARD' },
remapping = {'rotate':'angular_error'})
approach_goal = ApproachGoal()
approach_goal.forward_vel = 0.05
approach_goal.forward_mult = 0.50
smach.StateMachine.add(
'MOVE_FORWARD',
SimpleActionState( '/approach_table/move_forward_act',
ApproachAction,
goal = approach_goal ),
transitions = { 'succeeded' : 'succeeded' })
return sm
