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