Skip to content
This repository has been archived by the owner on Dec 1, 2020. It is now read-only.

Feature/pm 391 set state machine to unknown #506

Merged
merged 9 commits into from
Apr 10, 2020
Merged

Feature/pm 391 set state machine to unknown #506

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
a646c73
Added an unknown tag to the gait instruction message to set the state…
JorisWeeda Apr 6, 2020
55848f2
Added the possibility to add a callback in the control_flow.py to the…
JorisWeeda Apr 6, 2020
856ec1a
Merge branch 'develop' into feature/PM-391-set-state-machine-to-unknown
JorisWeeda Apr 7, 2020
8884670
removed redundant space
JorisWeeda Apr 7, 2020
23d46a3
Added failed transition to the UNKNOWN state and reset the callback w…
JorisWeeda Apr 8, 2020
c0ef997
Merge branch 'feature/PM-391-set-state-machine-to-unknown' of https:/…
JorisWeeda Apr 8, 2020
6afffa3
Added 'failed' and 'preempted' as default outcomes to the idle state.
JorisWeeda Apr 9, 2020
c7479cc
Merge branch 'develop' into feature/PM-391-set-state-machine-to-unknown
JorisWeeda Apr 10, 2020
6cf4533
Integrate changes into new tilted_path_left_knee_bend_sm.py
JorisWeeda Apr 10, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions march_shared_resources/msg/GaitInstruction.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ Header header
int8 type

# All possible instruction types.
int8 UNKNOWN = -1
int8 STOP = 0
int8 GAIT = 1
int8 PAUSE = 2
Expand Down
9 changes: 9 additions & 0 deletions march_state_machine/src/march_state_machine/control_flow.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,14 @@ def __init__(self):
self._stopped_cb = None
self._gait_selected_cb = None
self._gait_transition_cb = None
self._set_sm_to_unknown_cb = None

def clear_callbacks(self):
"""Clears all currently registered callbacks."""
self._stopped_cb = None
self._gait_selected_cb = None
self._gait_transition_cb = None
self._set_sm_to_unknown_cb = None

def set_stopped_callback(self, cb):
"""Sets the callback for when stop is pressed.
Expand All @@ -49,6 +51,9 @@ def set_gait_transition_callback(self, cb):
"""
self._gait_transition_cb = cb

def set_state_machine_to_unknown(self, cb):
self._set_sm_to_unknown_cb = cb

def get_transition_integer(self):
"""Used to return the transition in the integer-format."""
if self._transition_index == GaitInstruction.INCREMENT_STEP_SIZE:
Expand Down Expand Up @@ -128,5 +133,9 @@ def callback_input_device_instruction(self, msg):
if callable(self._gait_transition_cb):
self._gait_transition_cb()

if msg.type == GaitInstruction.UNKNOWN:
if callable(self._set_sm_to_unknown_cb):
self._set_sm_to_unknown_cb()


control_flow = ControlFlow()
2 changes: 1 addition & 1 deletion march_state_machine/src/march_state_machine/gaits/slope_down_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ def create():
smach.StateMachine.add('GAIT RD SLOPE DOWN', SlopeStateMachine('ramp_door_slope_down'),
transitions={'succeeded': 'STANDING SLOPE DOWN'})

smach.StateMachine.add('STANDING SLOPE DOWN', IdleState(outcomes=['gait_ramp_door_last_step', 'preempted']),
smach.StateMachine.add('STANDING SLOPE DOWN', IdleState(gait_outcomes=['gait_ramp_door_last_step']),
transitions={'gait_ramp_door_last_step': 'GAIT RD LAST STEP'})

smach.StateMachine.add('GAIT RD LAST STEP', StepStateMachine('ramp_door_last_step'),
Expand Down
6 changes: 3 additions & 3 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_left_flexed_knee_step_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@ def create():
transitions={'succeeded': 'STANDING TP LEFT STRAIGHT',
'rejected': 'STANDING TP LEFT STRAIGHT'})

smach.StateMachine.add('STANDING TP LEFT STRAIGHT', IdleState(outcomes=['gait_tilted_path_left_single_step',
'gait_tilted_path_left_straight_end',
'preempted']),
smach.StateMachine.add('STANDING TP LEFT STRAIGHT',
IdleState(gait_outcomes=['gait_tilted_path_left_single_step',
'gait_tilted_path_left_straight_end']),
transitions={'gait_tilted_path_left_single_step': 'GAIT TP LEFT SINGLE STEP',
'gait_tilted_path_left_straight_end': 'GAIT TP LEFT STRAIGHT END'})

Expand Down
10 changes: 5 additions & 5 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_left_knee_bend_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,20 +6,20 @@

def create():
sm_tilted_path_left_knee_bend = smach.StateMachine(outcomes=['succeeded', 'preempted', 'failed', 'rejected'])
sm_tilted_path_left_knee_bend .register_io_keys(['sounds'])
sm_tilted_path_left_knee_bend.register_io_keys(['sounds'])
with sm_tilted_path_left_knee_bend:
smach.StateMachine.add('GAIT TP LEFT KNEE BEND', StepStateMachine('tilted_path_left_knee_bend',
subgaits=['right_open']),
subgaits=['right_open']),
transitions={'succeeded': 'STANDING TP LEFT STRAIGHT'})

smach.StateMachine.add('GAIT TP LEFT SINGLE STEP', StepStateMachine('tilted_path_left_single_step',
subgaits=['right_open', 'left_close']),
transitions={'succeeded': 'STANDING TP LEFT STRAIGHT',
'rejected': 'STANDING TP LEFT STRAIGHT'})

smach.StateMachine.add('STANDING TP LEFT STRAIGHT', IdleState(outcomes=['gait_tilted_path_left_single_step',
'gait_tilted_path_left_straight_end',
'preempted']),
smach.StateMachine.add('STANDING TP LEFT STRAIGHT',
IdleState(gait_outcomes=['gait_tilted_path_left_single_step',
'gait_tilted_path_left_straight_end']),
transitions={'gait_tilted_path_left_single_step': 'GAIT TP LEFT SINGLE STEP',
'gait_tilted_path_left_straight_end': 'GAIT TP LEFT STRAIGHT END'})

Expand Down
6 changes: 3 additions & 3 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_left_straight_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@ def create():
transitions={'succeeded': 'STANDING TP LEFT STRAIGHT',
'rejected': 'STANDING TP LEFT STRAIGHT'})

smach.StateMachine.add('STANDING TP LEFT STRAIGHT', IdleState(outcomes=['gait_tilted_path_left_single_step',
'gait_tilted_path_left_straight_end',
'preempted']),
smach.StateMachine.add('STANDING TP LEFT STRAIGHT',
IdleState(gait_outcomes=['gait_tilted_path_left_single_step',
'gait_tilted_path_left_straight_end']),
transitions={'gait_tilted_path_left_single_step': 'GAIT TP LEFT SINGLE STEP',
'gait_tilted_path_left_straight_end': 'GAIT TP LEFT STRAIGHT END'})

Expand Down
6 changes: 3 additions & 3 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_right_flexed_knee_step_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@ def create():
transitions={'succeeded': 'STANDING TP RIGHT STRAIGHT',
'rejected': 'STANDING TP RIGHT STRAIGHT'})

smach.StateMachine.add('STANDING TP RIGHT STRAIGHT', IdleState(outcomes=['gait_tilted_path_right_single_step',
'gait_tilted_path_right_straight_end',
'preempted']),
smach.StateMachine.add('STANDING TP RIGHT STRAIGHT',
IdleState(gait_outcomes=['gait_tilted_path_right_single_step',
'gait_tilted_path_right_straight_end']),
transitions={'gait_tilted_path_right_single_step': 'GAIT TP RIGHT SINGLE STEP',
'gait_tilted_path_right_straight_end': 'GAIT TP RIGHT STRAIGHT END'})

Expand Down
6 changes: 3 additions & 3 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_right_straight_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@ def create():
transitions={'succeeded': 'STANDING TP RIGHT STRAIGHT',
'rejected': 'STANDING TP RIGHT STRAIGHT'})

smach.StateMachine.add('STANDING TP RIGHT STRAIGHT', IdleState(outcomes=['gait_tilted_path_right_single_step',
'gait_tilted_path_right_straight_end',
'preempted']),
smach.StateMachine.add('STANDING TP RIGHT STRAIGHT',
IdleState(gait_outcomes=['gait_tilted_path_right_single_step',
'gait_tilted_path_right_straight_end']),
transitions={'gait_tilted_path_right_single_step': 'GAIT TP RIGHT SINGLE STEP',
'gait_tilted_path_right_straight_end': 'GAIT TP RIGHT STRAIGHT END'})

Expand Down
3 changes: 1 addition & 2 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_sideways_end_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,7 @@ def create():
subgaits=['left_open', 'right_close']),
transitions={'succeeded': 'STANDING TP SIDEWAYS END'})

smach.StateMachine.add('STANDING TP SIDEWAYS END', IdleState(outcomes=['gait_tilted_path_second_end',
'preempted']),
smach.StateMachine.add('STANDING TP SIDEWAYS END', IdleState(gait_outcomes=['gait_tilted_path_second_end']),
transitions={'gait_tilted_path_second_end': 'GAIT TP SECOND END'})

smach.StateMachine.add('GAIT TP SECOND END', StepStateMachine('tilted_path_second_end',
Expand Down
3 changes: 1 addition & 2 deletions march_state_machine/src/march_state_machine/gaits/tilted_path_sideways_start_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,7 @@ def create():
subgaits=['left_open', 'right_close']),
transitions={'succeeded': 'STANDING TP SIDEWAYS START'})

smach.StateMachine.add('STANDING TP SIDEWAYS START', IdleState(outcomes=['gait_tilted_path_second_start',
'preempted']),
smach.StateMachine.add('STANDING TP SIDEWAYS START', IdleState(gait_outcomes=['gait_tilted_path_second_start']),
transitions={'gait_tilted_path_second_start': 'GAIT TP SECOND START'})

smach.StateMachine.add('GAIT TP SECOND START', StepStateMachine('tilted_path_second_start',
Expand Down
55 changes: 28 additions & 27 deletions march_state_machine/src/march_state_machine/healthy_sm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@ def __init__(self):

self.open()
self.add_auto('START', HealthyStart(), connector_outcomes=['succeeded'])
self.add('UNKNOWN', IdleState(outcomes=['home_sit', 'home_stand', 'failed', 'preempted']),
transitions={'home_sit': 'HOME SIT', 'home_stand': 'HOME STAND'})
self.add('UNKNOWN', IdleState(gait_outcomes=['home_sit', 'home_stand']),
transitions={'home_sit': 'HOME SIT', 'home_stand': 'HOME STAND', 'failed': 'UNKNOWN'})

self.add_state('HOME SIT', StepStateMachine('home', ['home_sit']), 'SITTING', rejected='UNKNOWN')
self.add_state('HOME STAND', StepStateMachine('home', ['home_stand']), 'STANDING', rejected='UNKNOWN')
Expand Down Expand Up @@ -84,8 +84,8 @@ def __init__(self):

self.add_state('GAIT SOFA SIT', StepStateMachine('sofa_sit', ['sit_down', 'sit_home']), 'SOFA SITTING',
rejected='STANDING')
self.add('SOFA SITTING', IdleState(outcomes=['gait_sofa_stand', 'preempted']),
transitions={'gait_sofa_stand': 'GAIT SOFA STAND'})
self.add('SOFA SITTING', IdleState(gait_outcomes=['gait_sofa_stand']),
transitions={'gait_sofa_stand': 'GAIT SOFA STAND', 'failed': 'UNKNOWN'})
self.add_state('GAIT SOFA STAND', StepStateMachine('sofa_stand', ['prepare_stand_up', 'stand_up']), 'STANDING',
rejected='SOFA SITTING')

Expand Down Expand Up @@ -121,28 +121,28 @@ def __init__(self):
self.add_state('GAIT TP SIDEWAYS START', tilted_path_sideways_start_sm.create(), 'STANDING')
self.add_state('GAIT TP SIDEWAYS END', tilted_path_sideways_end_sm.create(), 'STANDING')

self.add('SITTING', IdleState(outcomes=['gait_stand', 'preempted']),
transitions={'gait_stand': 'GAIT STAND'})
self.add('STANDING', IdleState(outcomes=['gait_sit', 'gait_walk', 'gait_walk_small', 'gait_single_step_small',
'gait_walk_large', 'gait_single_step_normal', 'gait_side_step_left',
'gait_side_step_right', 'gait_side_step_left_small',
'gait_side_step_right_small', 'gait_sofa_sit',
'gait_stairs_up', 'gait_stairs_down',
'gait_stairs_up_single_step', 'gait_stairs_down_single_step',
'gait_walk_small', 'gait_rough_terrain_high_step',
'gait_rough_terrain_middle_steps',
'gait_rough_terrain_first_middle_step',
'gait_rough_terrain_second_middle_step',
'gait_rough_terrain_third_middle_step',
'gait_ramp_door_slope_up', 'gait_ramp_door_slope_down',
'gait_tilted_path_left_straight_start',
'gait_tilted_path_left_flexed_knee_step',
'gait_tilted_path_left_knee_bend',
'gait_tilted_path_right_straight_start',
'gait_tilted_path_right_flexed_knee_step',
'gait_tilted_path_first_start',
'gait_tilted_path_first_end',
'preempted']),
self.add('SITTING', IdleState(gait_outcomes=['gait_stand']),
transitions={'gait_stand': 'GAIT STAND', 'failed': 'UNKNOWN'})
self.add('STANDING', IdleState(gait_outcomes=['gait_sit', 'gait_walk', 'gait_walk_small',
'gait_single_step_small', 'gait_walk_large',
'gait_single_step_normal', 'gait_side_step_left',
'gait_side_step_right', 'gait_side_step_left_small',
'gait_side_step_right_small', 'gait_sofa_sit',
'gait_stairs_up', 'gait_stairs_down',
'gait_stairs_up_single_step', 'gait_stairs_down_single_step',
'gait_walk_small', 'gait_rough_terrain_high_step',
'gait_rough_terrain_middle_steps',
'gait_rough_terrain_first_middle_step',
'gait_rough_terrain_second_middle_step',
'gait_rough_terrain_third_middle_step',
'gait_ramp_door_slope_up', 'gait_ramp_door_slope_down',
'gait_tilted_path_left_straight_start',
'gait_tilted_path_left_flexed_knee_step',
'gait_tilted_path_left_knee_bend',
'gait_tilted_path_right_straight_start',
'gait_tilted_path_right_flexed_knee_step',
'gait_tilted_path_first_start',
'gait_tilted_path_first_end']),
transitions={'gait_sit': 'GAIT SIT',
'gait_walk': 'GAIT WALK',
'gait_walk_small': 'GAIT WALK SMALL',
Expand Down Expand Up @@ -171,7 +171,8 @@ def __init__(self):
'gait_tilted_path_right_straight_start': 'GAIT TP RIGHT STRAIGHT',
'gait_tilted_path_right_flexed_knee_step': 'GAIT TP RIGHT FLEXED KNEE STEP',
'gait_tilted_path_first_start': 'GAIT TP SIDEWAYS START',
'gait_tilted_path_first_end': 'GAIT TP SIDEWAYS END'})
'gait_tilted_path_first_end': 'GAIT TP SIDEWAYS END',
'failed': 'UNKNOWN'})
self.close()

def add_state(self, label, state, succeeded, rejected=None, custom_start_label=None):
Expand Down
9 changes: 8 additions & 1 deletion march_state_machine/src/march_state_machine/states/idle_state.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@ class IdleState(smach.State):
Listens to instructions from the input device and reacts if they are known transitions.
"""

def __init__(self, outcomes):
def __init__(self, gait_outcomes):
outcomes = ['failed', 'preempted'] + gait_outcomes
super(IdleState, self).__init__(outcomes)

self._result_gait = None
Expand All @@ -32,6 +33,7 @@ def execute(self, userdata):
control_flow.set_stopped_callback(self._stopped_cb)
control_flow.set_gait_transition_callback(self._transition_cb)
control_flow.set_gait_selected_callback(self._gait_cb)
control_flow.set_state_machine_to_unknown(self._return_failed)

self._trigger_event.wait()
control_flow.clear_callbacks()
Expand Down Expand Up @@ -68,3 +70,8 @@ def _gait_cb(self, gait):
def request_preempt(self):
super(IdleState, self).request_preempt()
self._trigger_event.set()

def _return_failed(self):
rospy.logwarn('Current state is set to unknown.')
self._result_gait = None
self._trigger_event.set()