def run_state_machine(): with sm: smach.StateMachine.add('InitState', PreemptableState(initState, input_keys=['init_counter_in'], output_keys=['init_counter_out'], outcomes=['init', 'square', 'circle', 'triangle']), transitions={'init' : 'InitState', 'square' : 'Square', 'circle' : 'Circle', 'triangle' : 'Triangle'}, remapping={'init_counter_in' : 'sm_counter', 'init_counter_out' : 'sm_counter'}) smach.StateMachine.add('Square', PreemptableState(square, input_keys=['square_counter_in'], output_keys=['square_counter_out'], outcomes=['square', 'circle', 'triangle', 'init']), transitions={'square' : 'Square', 'circle' : 'Circle', 'triangle' : 'Triangle', 'init' : 'InitState'}, remapping={'square_counter_in' : 'sm_counter', 'square_counter_out' : 'sm_counter'}) smach.StateMachine.add('Circle', PreemptableState(circle, input_keys=['circle_counter_in'], output_keys=['circle_counter_out'], outcomes=['square', 'circle', 'triangle', 'init']), transitions={'square' : 'Square', 'circle' : 'Circle', 'triangle' : 'Triangle', 'init' : 'InitState'}, remapping={'circle_counter_in' : 'sm_counter'}) smach.StateMachine.add('Triangle', PreemptableState(triangle, input_keys=['triangle_counter_in'], output_keys=['triangle_counter_out'], outcomes=['square', 'circle', 'triangle', 'init']), transitions={'square' : 'Square', 'circle' : 'Circle', 'triangle' : 'Triangle', 'init' : 'InitState'}, remapping={'triangle_counter_in' : 'sm_counter'}) outcome = sm.execute()
def construct(): sm = smach.StateMachine(outcomes=['preempted']) # Attach helper functions sm.set_ranges = MethodType(set_ranges, sm, sm.__class__) sm.get_twist = MethodType(get_twist, sm, sm.__class__) sm.set_config = MethodType(set_config, sm, sm.__class__) # Set initial values in userdata sm.userdata.velocity = (0, 0, 0) sm.userdata.mode = 1 sm.userdata.clearance = 0.3 sm.userdata.ranges = None sm.userdata.max_forward_velocity = 0.3 sm.userdata.default_rotational_speed = 0.5 sm.userdata.direction = 1 with sm: smach.StateMachine.add('SEARCH', PreemptableState(search, input_keys=['mode', 'all_min', 'clearance', 'max_forward_velocity'], output_keys=['velocity'], outcomes=['found_wall']), transitions={'found_wall': 'WALL'}) smach.StateMachine.add('ALLIGN', PreemptableState(allign, input_keys=['mode', 'width', 'side_balance', 'side_avg_distance', 'default_rotational_speed', 'clearance', 'max_forward_velocity', 'corner', 'front_min'], output_keys=['velocity'], outcomes=['is_alligned']), transitions={'is_alligned': 'FOLLOW'}) smach.StateMachine.add('FOLLOW', PreemptableState(follow, input_keys=['mode', 'width', 'front_min', 'clearance', 'max_forward_velocity', 'side_balance', 'side_avg_distance', 'default_rotational_speed', 'corner'], output_keys=['velocity'], outcomes=['avoid_wall', 'is_not_alligned']), transitions={'avoid_wall':'WALL', 'is_not_alligned':'ALLIGN'}) smach.StateMachine.add('WALL', PreemptableState(wall, input_keys=['mode', 'default_rotational_speed', 'clearance', 'front_min'], output_keys=['velocity'], outcomes=['wall_avoided']), transitions={'wall_avoided': 'FOLLOW'}) #=========================== YOUR CODE HERE =========================== # Instructions: construct the state machine by adding the states that # you have implemented. # Below is an example how to add a state: # # smach.StateMachine.add('SEARCH', # PreemptableState(search, # input_keys=['front_min', 'clearance'], # output_keys=['velocity'], # outcomes=['found_obstacle']), # transitions={'found_obstacle': 'ANOTHER_STATE'}) # # First argument is the state label, an arbitrary string # (by convention should be uppercase). Second argument is # an object that implements the state. In our case an # instance of the helper class PreemptableState is # created, and the state function in passed. Moreover, # we have to specify which keys in the userdata the # function will need to access for reading (input_keys) # and for writing (output_keys), and the list of possible # outcomes of the state. Finally, the transitions are # specified. Normally you would have one transition per # state outcome. # # Note: The first state that you add will become the initial state of # the state machine. #====================================================================== return sm
def construct(): sm = smach.StateMachine(outcomes=['preempted']) # Attach helper functions sm.set_ranges = MethodType(set_ranges, sm, sm.__class__) sm.get_twist = MethodType(get_twist, sm, sm.__class__) sm.set_config = MethodType(set_config, sm, sm.__class__) # Set initial values in userdata sm.userdata.velocity = (0, 0, 0) sm.userdata.mode = 1 sm.userdata.clearance = 0.7 sm.userdata.ranges = None sm.userdata.max_forward_velocity = 0.3 sm.userdata.default_rotational_speed = 0.5 sm.userdata.direction = 1 with sm: #pass #=========================== YOUR CODE HERE =========================== # Instructions: construct the state machine by adding the states that # you have implemented. # Below is an example how to add a state: # # smach.StateMachine.add('SEARCH', # PreemptableState(search, # input_keys=['front_min', 'clearance'], # output_keys=['velocity'], # outcomes=['found_obstacle']), # transitions={'found_obstacle': 'ANOTHER_STATE'}) # # First argument is the state label, an arbitrary string # (by convention should be uppercase). Second argument is # an object that implements the state. In our case an # instance of the helper class PreemptableState is # created, and the state function in passed. Moreover, # we have to specify which keys in the userdata the # function will need to access for reading (input_keys) # and for writing (output_keys), and the list of possible # outcomes of the state. Finally, the transitions are # specified. Normally you would have one transition per # state outcome. # # Note: The first state that you add will become the initial state of # the state machine. #====================================================================== #============================================================================== # SEARCH : Used to search for any wall, used during initialization #============================================================================== smach.StateMachine.add('SEARCH',PreemptableState(search,input_keys=['front_min','clearance'], output_keys=['velocity'], outcomes=['found_wall']), transitions={'found_wall': 'ALIGN'}) #============================================================================== # ALIGN : Used to alignt to a wall, accroding to mode selected #============================================================================== smach.StateMachine.add('ALIGN',PreemptableState(align,input_keys=['front_min', 'clearance', 'left_1','left_2', 'right_1','right_2', 'direction','default_rotational_speed'], output_keys=['velocity'], outcomes=['aligned']), transitions={'aligned': 'FOLLOW_WALL'}) #============================================================================== # FOLLOW_WALL : Used to follow a wall once aligned #============================================================================== smach.StateMachine.add('FOLLOW_WALL',PreemptableState(follow_wall,input_keys=['front_min', 'clearance', 'left_1','left_2', 'right_1','right_2', 'front_right','front_left', 'direction','default_rotational_speed', 'max_forward_velocity','back_right','back_left'], output_keys=['velocity'], outcomes=['concave','no_wall','convex','align']), transitions={'concave': 'CONCAVE', 'no_wall':'TRANSITION_SEARCH', 'convex':'CONVEX', 'align' : 'ALIGN'}) #============================================================================== # TRANSITION_SEARCH : If mode is changed during wall following, this is # used to search for wall in the required direction #============================================================================== smach.StateMachine.add('TRANSITION_SEARCH',PreemptableState(transition_search,input_keys=['clearance','front_min', 'left_1','left_2', 'right_1','right_2', 'direction','default_rotational_speed'], output_keys=['velocity'], outcomes=['found_wall']), transitions={'found_wall': 'ALIGN'}) #============================================================================== # Concave State is called to manoeuvre corners with walls straight ahead #============================================================================== smach.StateMachine.add('CONCAVE', PreemptableState(concave,input_keys=['front_min', 'clearance', 'front_1','front_2', 'left_1','left_2', 'right_1','right_2', 'front_right','front_left', 'direction','default_rotational_speed'], output_keys=['velocity'], outcomes=['navigated']), transitions={'navigated': 'FOLLOW_WALL' }) #============================================================================== # Convex State is called to manoeuvre corners with gaps in walls (like doors) #============================================================================== smach.StateMachine.add('CONVEX', PreemptableState(convex,input_keys=['front_min', 'clearance','max_forward_velocity', 'front_1','front_2', 'left_1','left_2', 'right_1','right_2', 'front_right','front_left', 'direction','default_rotational_speed'], output_keys=['velocity'], outcomes=['navigated']), transitions={'navigated': 'FOLLOW_WALL' }) """ #STOP state was used as a redundant temporary state for undefined states smach.StateMachine.add('STOP',PreemptableState(stop,input_keys=['front_min','clearance'], output_keys=['velocity'], outcomes=['stop']), transitions={'stop': 'STOP'}) """ return sm
def construct(): sm = smach.StateMachine(outcomes=['preempted']) # Attach helper functions sm.set_ranges = MethodType(set_ranges, sm, sm.__class__) sm.get_twist = MethodType(get_twist, sm, sm.__class__) sm.set_config = MethodType(set_config, sm, sm.__class__) # Set initial values in userdata sm.userdata.velocity = (0, 0, 0) sm.userdata.mode = 1 sm.userdata.clearance = 0.3 sm.userdata.ranges = None sm.userdata.max_forward_velocity = 0.3 sm.userdata.default_rotational_speed = 0.5 sm.userdata.direction = 1 # Newly defined state variables # Data acquired from amr_stage_worlds/models/sonars.inc sm.userdata.sonar_pose = [ [0.075, 0.130, radians(90)], # 0 [0.115, 0.115, radians(50)], # 1 [0.150, 0.080, radians(30)], # 2 [0.170, 0.025, radians(10)], # 3 [0.170, -0.025, radians(-10)], # 4 [0.150, -0.080, radians(-30)], # 5 [0.115, -0.115, radians(-50)], # 6 [0.075, -0.130, radians(-90)], # 7 [-0.155, -0.130, radians(-90)], # 8 [-0.195, -0.115, radians(-130)], # 9 [-0.230, -0.080, radians(-150)], # 10 [-0.250, -0.025, radians(-170)], # 11 [-0.250, 0.025, radians(170)], # 12 [-0.230, 0.080, radians(150)], # 13 [-0.195, 0.115, radians(130)], # 14 [-0.155, 0.130, radians(90)] ] # 15 sm.userdata.youbot_length = 0.58 sm.userdata.youbot_width = 0.38 sm.userdata.youbot_diag = sqrt(sm.userdata.youbot_length**2 + sm.userdata.youbot_width**2) sm.userdata.in_corner = False sm.userdata.convex_detected = False sm.userdata.side_front_sonar = 0 sm.userdata.side_back_sonar = 0 sm.userdata.side_mean = 0 sm.userdata.sonar_10 = 0 sm.userdata.sonar_30 = 0 sm.userdata.sonar_50 = 0 # Min of all sonar for searching state sm.userdata.min_all = 0 sm.userdata.min_all_index = 0 # Indices of sonar readings dependent on mode sm.userdata.sonar_10_index = 4 sm.userdata.sonar_30_index = 5 sm.userdata.sonar_50_index = 6 sm.userdata.sonar_side_front_index = 7 sm.userdata.sonar_side_back_index = 8 # Assuming 2 pairs of side sonar are mounted mirrored to each other, as # indicated in above data sm.userdata.side_sonar_distance = ( sm.userdata.sonar_pose[sm.userdata.sonar_side_front_index][_X_LINEAR] - sm.userdata.sonar_pose[sm.userdata.sonar_side_back_index][_Y_LINEAR]) with sm: smach.StateMachine.add( 'SEARCH', PreemptableState(search, input_keys=[ 'in_corner', 'front_distance', 'clearance', 'max_forward_velocity', 'min_all', 'min_all_index', 'sonar_pose' ], output_keys=['velocity'], outcomes=['wall_approached']), transitions={'wall_approached': 'ALIGN_ANGULAR_ONLY'}) smach.StateMachine.add( 'ALIGN_ANGULAR_ONLY', PreemptableState(align_angular_only, input_keys=[ 'side_front_sonar', 'side_back_sonar', 'youbot_width', 'youbot_diag', 'front_distance', 'side_mean', 'clearance', 'default_rotational_speed', 'min_all' ], output_keys=['velocity'], outcomes=['aligned', 'concave_detected', 'lost']), transitions={ 'aligned': 'ALIGN_FORWARD', 'concave_detected': 'CONCAVE', 'lost': 'SEARCH' }) smach.StateMachine.add( 'ALIGN_FORWARD', PreemptableState( align_forward, input_keys=[ 'side_back_sonar', 'side_front_sonar', 'side_sonar_distance', 'side_mean', 'in_corner', 'convex_detected', 'default_rotational_speed', 'max_forward_velocity', 'front_distance', 'clearance', 'min_all' ], output_keys=['velocity'], outcomes=['convex_detected', 'concave_detected', 'lost']), transitions={ 'convex_detected': 'CONVEX', 'concave_detected': 'CONCAVE', 'lost': 'SEARCH' }) smach.StateMachine.add( 'CONVEX', PreemptableState(convex, input_keys=[ 'convex_detected', 'default_rotational_speed', 'max_forward_velocity', 'clearance', 'min_all' ], output_keys=['velocity'], outcomes=['convex_passed', 'lost']), transitions={ 'convex_passed': 'ALIGN_FORWARD', 'lost': 'SEARCH' }) smach.StateMachine.add( 'CONCAVE', PreemptableState( concave, input_keys=[ 'in_corner', 'convex_detected', 'clearance', 'default_rotational_speed', 'front_distance', 'sonar_10', 'sonar_30', 'sonar_50', 'side_mean', 'min_all' ], output_keys=['velocity'], outcomes=['concave_passed', 'convex_detected', 'lost']), transitions={ 'concave_passed': 'ALIGN_FORWARD', 'convex_detected': 'CONVEX', 'lost': 'SEARCH' }) return sm
def construct(): sm = smach.StateMachine(outcomes=['preempted']) # Attach helper functions sm.set_ranges = MethodType(set_ranges, sm, sm.__class__) sm.get_twist = MethodType(get_twist, sm, sm.__class__) sm.set_config = MethodType(set_config, sm, sm.__class__) # Set initial values in userdata sm.userdata.velocity = (0, 0, 0) sm.userdata.mode = 1 sm.userdata.clearance = 0.7 sm.userdata.ranges = None sm.userdata.max_forward_velocity = 0.3 sm.userdata.default_rotational_speed = 0.5 sm.userdata.direction = 1 with sm: #pass #============================================================================== # SEARCH : Used to search for any wall, used during initialization #============================================================================== smach.StateMachine.add('SEARCH', PreemptableState( search, input_keys=['front_min', 'clearance'], output_keys=['velocity'], outcomes=['found_wall']), transitions={'found_wall': 'ALIGN'}) #============================================================================== # ALIGN : Used to alignt to a wall, accroding to mode selected #============================================================================== smach.StateMachine.add('ALIGN', PreemptableState(align, input_keys=[ 'front_min', 'clearance', 'left_1', 'left_2', 'right_1', 'right_2', 'direction', 'default_rotational_speed' ], output_keys=['velocity'], outcomes=['aligned']), transitions={'aligned': 'FOLLOW_WALL'}) #============================================================================== # FOLLOW_WALL : Used to follow a wall once aligned #============================================================================== smach.StateMachine.add( 'FOLLOW_WALL', PreemptableState(follow_wall, input_keys=[ 'front_min', 'clearance', 'left_1', 'left_2', 'right_1', 'right_2', 'front_right', 'front_left', 'direction', 'default_rotational_speed', 'max_forward_velocity', 'back_right', 'back_left' ], output_keys=['velocity'], outcomes=['concave', 'no_wall', 'align']), transitions={ 'concave': 'CONCAVE', 'no_wall': 'TRANSITION_SEARCH', 'align': 'ALIGN' }) #============================================================================== # TRANSITION_SEARCH : If mode is changed during wall following, this is # used to search for wall in the required direction #============================================================================== smach.StateMachine.add('TRANSITION_SEARCH', PreemptableState(transition_search, input_keys=[ 'clearance', 'front_min', 'left_1', 'left_2', 'right_1', 'right_2', 'direction', 'default_rotational_speed' ], output_keys=['velocity'], outcomes=['found_wall']), transitions={'found_wall': 'ALIGN'}) #============================================================================== # Concave State is called to manoeuvre corners with walls straight ahead #============================================================================== smach.StateMachine.add( 'CONCAVE', PreemptableState(concave, input_keys=[ 'front_min', 'clearance', 'front_1', 'front_2', 'left_1', 'left_2', 'right_1', 'right_2', 'front_right', 'front_left', 'direction', 'default_rotational_speed' ], output_keys=['velocity'], outcomes=['navigated']), transitions={'navigated': 'FOLLOW_WALL'}) return sm