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