def gen(self):
"""Generate the state instantiation for this concurrent state."""
if not self.is_concurrent():
return self.gen_single()
p_names = ["states", "outcomes", "outcome_mapping"]
p_vals = [
self.gen_states_str(),
str(self.internal_outcomes),
str(self.internal_outcome_maps)
]
# Not really needed, but it's good to be explicit
concurrent_si_outcomes = self.internal_outcomes
concurrent_si_transitions = self.internal_transitions
autonomy = [
max(self.outcome_to_autonomy_list[o])
for o in self.internal_outcomes
]
#TODO: Handle userdata_keys and userdata_remapping for Concurrent state
return new_si("/" + self.name,
"ConcurrentState",
concurrent_si_outcomes,
concurrent_si_transitions,
None,
p_names,
p_vals,
autonomy=autonomy)
def gen(self):
"""Generate the state instantiation for this concurrent state."""
if not self.is_concurrent():
return self.gen_single()
p_names = ["states", "outcomes", "outcome_mapping"]
p_vals = [self.gen_states_str(),
str(self.internal_outcomes),
str(self.internal_outcome_maps)]
# Not really needed, but it's good to be explicit
concurrent_si_outcomes = self.internal_outcomes
concurrent_si_transitions = self.internal_transitions
autonomy = [max(self.outcome_to_autonomy_list[o])
for o in self.internal_outcomes]
#TODO: Handle userdata_keys and userdata_remapping for Concurrent state
return new_si("/" + self.name,
"ConcurrentState",
concurrent_si_outcomes,
concurrent_si_transitions,
None,
p_names,
p_vals,
autonomy = autonomy)
def get_init_temp_state(init_states):
"""
Return a temporary initial state that goes to every real initial state.
init_states: a list of initial states to go to
"""
init_state_names = [s.name for s in init_states]
return new_si(
state_path="/{0}".format(INIT_STATE_NAME),
state_class="LogState",
behavior_class='',
outcomes=["done" for s in init_state_names], # outcomes and
transitions=init_state_names, # transitions are the same
initial_state=None,
p_names=["text"],
p_vals=['"Initial state"'],
autonomy=[0 for s in init_states])
def get_init_temp_state(init_states):
"""
Return a temporary initial state that goes to every real initial state.
init_states: a list of initial states to go to
"""
init_state_names = [s.name for s in init_states]
return new_si(
state_path = "/{0}".format(INIT_STATE_NAME),
state_class = "LogState",
behavior_class = '',
outcomes = ["done" for s in init_state_names], # outcomes and
transitions = init_state_names, # transitions are the same
initial_state = None,
p_names = ["text"],
p_vals = ['"Initial state"'],
autonomy = [0 for s in init_states]
)
def gen_single(self):
"""
Generate the state instantiation for a single state. This should only
be called in the degenerate case when there's only one internal state,
so it doesn't make sense to use a concurrent state.
"""
label, decl = self.internal_states.items()[0]
outcomes = []
transitions = []
for out_map in self.internal_outcome_maps:
transitions.append(out_map['outcome'])
outcomes.append(out_map['condition'][label])
outcomes = self.internal_outcomes
transitions = self.internal_transitions
autonomy = [
max(self.outcome_to_autonomy_list[o])
for o in self.internal_outcomes
]
#TODO: Refactor/simplify once format is figured out:
if any(self.internal_userdata_keys):
userdata_keys = self.internal_userdata_keys
else:
userdata_keys = []
if any(self.internal_userdata_remapping):
userdata_remapping = self.internal_userdata_remapping
else:
userdata_remapping = []
state_class = decl['name']
behavior_class = decl.get('behavior_class', '')
return new_si("/" + self.name,
state_class,
behavior_class,
outcomes,
transitions,
None,
decl['param_names'],
decl['param_values'],
autonomy=autonomy,
userdata_keys=userdata_keys,
userdata_remapping=userdata_remapping)
def gen_single(self):
"""
Generate the state instantiation for a single state. This should only
be called in the degenerate case when there's only one internal state,
so it doesn't make sense to use a concurrent state.
"""
label, decl = self.internal_states.items()[0]
outcomes = []
transitions = []
for out_map in self.internal_outcome_maps:
transitions.append(out_map['outcome'])
outcomes.append(out_map['condition'][label])
outcomes = self.internal_outcomes
transitions = self.internal_transitions
autonomy = [max(self.outcome_to_autonomy_list[o])
for o in self.internal_outcomes]
#TODO: Refactor/simplify once format is figured out:
if any(self.internal_userdata_keys):
userdata_keys = self.internal_userdata_keys
else:
userdata_keys = []
if any(self.internal_userdata_remapping):
userdata_remapping = self.internal_userdata_remapping
else:
userdata_remapping = []
state_class = decl['name']
behavior_class = decl.get('behavior_class', '')
return new_si("/" + self.name,
state_class,
behavior_class,
outcomes,
transitions,
None,
decl['param_names'],
decl['param_values'],
autonomy = autonomy,
userdata_keys = userdata_keys,
userdata_remapping = userdata_remapping)
def generate_sm_handle(request):
"""
This method handles the GenerateFlexBESM service. It creates StateInstantiations
given a system name and an automaton.
Broadly speaking, there are two types of input variables, and two types
of output variables.
Input Variables
- Sensor - variables that are the results of a sensor reading
- Completion - variables that say whether or not something activated
Output Variables
- Activation - activate something
- Perform - perform something other than activate (e.g. "beep" or
"print") TODO: implement this
This distinction is important because Completion and Activation variables
are related. Specifically, only one state machine should be created for a
given completion-activation pair. The Activation variable tells what to
activate, and the Completion variable tells where to go once that thing is
activated.
Sensor and Perform variables require their own state machines.
The algorithm is as follows:
1. For each state X, list all the outputs of X, and find the corresponding
state machines in the YAML file.
a. For each SM, find (in YAML file) what each state machine output
mean in terms of input variables. (e.g. "changed" from the
ChangeControlModeActionState means an input variable of "step_c".)
b. If none of the outputs of the state machine correspond to inputs,
then that output is a Perform variable. (TODO: implement)
2. For each transition (X -> Y on input A),
a. Find what SM + outputs corresponds to A.
b. If no SM's output maps to input A, then A must come from an external
sensor. In this case, look up the definitions in the YAML file.
c. Otherwise, it's a completion variable. Connect it to the
corresponding SM.
@param request An instance of GenerateFlexBESMRequest
@return A GenerateFlexBESMResponse with generated StateInstantiation.
"""
sm_generation_pkg_dir = rospkg.RosPack().get_path('vigir_sm_generation')
config_dir = os.path.join(sm_generation_pkg_dir,
'src/vigir_sm_generation/configs')
yaml_file = os.path.join(config_dir, 'systems.yaml')
try:
with open(yaml_file) as yf:
systems = yaml.load(yf)
except IOError:
rospy.logerr(
"System file could not be loaded from {0}.".format(yaml_file))
raise SMGenError(SynthesisErrorCodes(error_code))
sa = request.automaton # FSAutomaton
all_out_vars = sa.output_variables
all_in_vars = sa.input_variables
automata = modify_names(all_out_vars, sa.automaton)
# Load the config file
system_name = request.system
if system_name not in systems:
rospy.logerr("System {0} is not in the systems file ({1})."\
.format(system_name, yaml_file))
raise SMGenError(SynthesisErrorCodes.NO_SYSTEM_CONFIG)
yaml_sys_file = os.path.join(config_dir, systems[system_name])
try:
with open(yaml_sys_file) as yf:
config = yaml.load(yf)
except IOError:
rospy.logerr("System {0} could not be loaded from {1}."\
.format(system_name, yaml_sys_file))
raise SMGenError(SynthesisErrorCodes.SYSTEM_CONFIG_NOT_FOUND)
helper = SMGenConfig(config, all_in_vars, all_out_vars, automata)
# Initialize list of StateInstantiation's with parent SI.
# SIs = [new_si("/", StateInstantiation.CLASS_STATEMACHINE,
# helper.get_sm_real_outputs(), [], INIT_STATE_NAME, [], [])]
init_states = helper.get_init_states()
# SIs.append(get_init_temp_state(init_states))
#TEMP: Assume single initial state for July experiments:
SIs = [
new_si("/", StateInstantiation.CLASS_STATEMACHINE, '',
helper.get_sm_real_outputs(), [], init_states[0], [], [])
]
for state in automata:
name = state.name
if helper.is_fake_state(name):
continue
csg = ConcurrentStateGenerator(name)
# Add an internal state for each output.
curr_state_output_vars = helper.get_state_output_vars(state)
for out_var in curr_state_output_vars:
decl = helper.get_class_decl(out_var)
csg.add_internal_state(out_var, decl)
csg.add_internal_userdata(helper.get_userdata_keys(out_var),
helper.get_userdata_remapping(out_var))
transitions = helper.get_transitions(state)
#TEMP: Ignore states where everything is False [?]
if not transitions:
continue
for next_state, conditions in transitions.items():
substate_name_to_out = {} # i.e. condition mapping
for in_var in conditions:
ss_name = helper.get_substate_name(in_var)
# go from input variable -> class declaration -> out map
# to get what this input variable (e.g. 'stand_c') maps to in
# the class declared (e.g. 'changed').
decl = helper.get_class_decl(in_var)
out_map = helper.get_out_map(decl)
substate_name_to_out[ss_name] = out_map[in_var]
# need to add internal state for sensor input variables
if not helper.is_response_var(in_var):
csg.add_internal_state(ss_name, decl)
is_concurrent = csg.is_concurrent()
csg.add_internal_outcome_and_transition(
helper.get_outcome_name(is_concurrent, next_state,
substate_name_to_out),
helper.get_real_name(next_state),
helper.get_autonomy_list(substate_name_to_out))
csg.add_internal_outcome_maps({
'outcome':
helper.get_outcome_name(is_concurrent, next_state,
substate_name_to_out),
'condition':
substate_name_to_out
})
rospy.logdebug("{0} -> {1} if: {2}".format(name, next_state,
substate_name_to_out))
SIs.append(csg.gen())
return GenerateFlexBESMResponse(
SIs, SynthesisErrorCodes(SynthesisErrorCodes.SUCCESS))
def generate_sm_handle(request):
"""
This method handles the GenerateFlexBESM service. It creates StateInstantiations
given a system name and an automaton.
Broadly speaking, there are two types of input variables, and two types
of output variables.
Input Variables
- Sensor - variables that are the results of a sensor reading
- Completion - variables that say whether or not something activated
Output Variables
- Activation - activate something
- Perform - perform something other than activate (e.g. "beep" or
"print") TODO: implement this
This distinction is important because Completion and Activation variables
are related. Specifically, only one state machine should be created for a
given completion-activation pair. The Activation variable tells what to
activate, and the Completion variable tells where to go once that thing is
activated.
Sensor and Perform variables require their own state machines.
The algorithm is as follows:
1. For each state X, list all the outputs of X, and find the corresponding
state machines in the YAML file.
a. For each SM, find (in YAML file) what each state machine output
mean in terms of input variables. (e.g. "changed" from the
ChangeControlModeActionState means an input variable of "step_c".)
b. If none of the outputs of the state machine correspond to inputs,
then that output is a Perform variable. (TODO: implement)
2. For each transition (X -> Y on input A),
a. Find what SM + outputs corresponds to A.
b. If no SM's output maps to input A, then A must come from an external
sensor. In this case, look up the definitions in the YAML file.
c. Otherwise, it's a completion variable. Connect it to the
corresponding SM.
@param request An instance of GenerateFlexBESMRequest
@return A GenerateFlexBESMResponse with generated StateInstantiation.
"""
sm_generation_pkg_dir = rospkg.RosPack().get_path('vigir_sm_generation')
config_dir = os.path.join(sm_generation_pkg_dir,
'src/vigir_sm_generation/configs')
yaml_file = os.path.join(config_dir, 'systems.yaml')
try:
with open(yaml_file) as yf:
systems = yaml.load(yf)
except IOError:
rospy.logerr("System file could not be loaded from {0}."
.format(yaml_file))
raise SMGenError(SynthesisErrorCodes(error_code))
sa = request.automaton # FSAutomaton
all_out_vars = sa.output_variables
all_in_vars = sa.input_variables
automata = modify_names(all_out_vars, sa.automaton)
# Load the config file
system_name = request.system
if system_name not in systems:
rospy.logerr("System {0} is not in the systems file ({1})."\
.format(system_name, yaml_file))
raise SMGenError(SynthesisErrorCodes.NO_SYSTEM_CONFIG)
yaml_sys_file = os.path.join(config_dir, systems[system_name])
try:
with open(yaml_sys_file) as yf:
config = yaml.load(yf)
except IOError:
rospy.logerr("System {0} could not be loaded from {1}."\
.format(system_name, yaml_sys_file))
raise SMGenError(SynthesisErrorCodes.SYSTEM_CONFIG_NOT_FOUND)
helper = SMGenConfig(config, all_in_vars, all_out_vars, automata)
# Initialize list of StateInstantiation's with parent SI.
# SIs = [new_si("/", StateInstantiation.CLASS_STATEMACHINE,
# helper.get_sm_real_outputs(), [], INIT_STATE_NAME, [], [])]
init_states = helper.get_init_states()
# SIs.append(get_init_temp_state(init_states))
#TEMP: Assume single initial state for July experiments:
SIs = [new_si("/", StateInstantiation.CLASS_STATEMACHINE, '',
helper.get_sm_real_outputs(), [], init_states[0], [], [])]
for state in automata:
name = state.name
if helper.is_fake_state(name):
continue
csg = ConcurrentStateGenerator(name)
# Add an internal state for each output.
curr_state_output_vars = helper.get_state_output_vars(state)
for out_var in curr_state_output_vars:
decl = helper.get_class_decl(out_var)
csg.add_internal_state(out_var, decl)
csg.add_internal_userdata(helper.get_userdata_keys(out_var),
helper.get_userdata_remapping(out_var))
transitions = helper.get_transitions(state)
#TEMP: Ignore states where everything is False [?]
if not transitions:
continue
for next_state, conditions in transitions.items():
substate_name_to_out = {} # i.e. condition mapping
for in_var in conditions:
ss_name = helper.get_substate_name(in_var)
# go from input variable -> class declaration -> out map
# to get what this input variable (e.g. 'stand_c') maps to in
# the class declared (e.g. 'changed').
decl = helper.get_class_decl(in_var)
out_map = helper.get_out_map(decl)
substate_name_to_out[ss_name] = out_map[in_var]
# need to add internal state for sensor input variables
if not helper.is_response_var(in_var):
csg.add_internal_state(ss_name, decl)
is_concurrent = csg.is_concurrent()
csg.add_internal_outcome_and_transition(
helper.get_outcome_name(is_concurrent, next_state,
substate_name_to_out),
helper.get_real_name(next_state),
helper.get_autonomy_list(substate_name_to_out)
)
csg.add_internal_outcome_maps({
'outcome': helper.get_outcome_name(is_concurrent, next_state,
substate_name_to_out),
'condition': substate_name_to_out
})
rospy.logdebug("{0} -> {1} if: {2}".format(name, next_state,
substate_name_to_out))
SIs.append(csg.gen())
return GenerateFlexBESMResponse(SIs, SynthesisErrorCodes(SynthesisErrorCodes.SUCCESS))
