def _create_environment_behavior(self):
# Set the appropriate weather conditions
env_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior")
weather_update = ChangeWeather(
OpenScenarioParser.get_weather_from_env_action(self.config.init, self.config.catalogs))
road_friction = ChangeRoadFriction(
OpenScenarioParser.get_friction_from_env_action(self.config.init, self.config.catalogs))
env_behavior.add_child(oneshot_with_check(variable_name="InitialWeather", behaviour=weather_update))
env_behavior.add_child(oneshot_with_check(variable_name="InitRoadFriction", behaviour=road_friction))
return env_behavior
def _create_condition_container(self, node, story, name='Conditions Group', sequence=None,
maneuver=None, success_on_all=True):
"""
This is a generic function to handle conditions utilising ConditionGroups
Each ConditionGroup is represented as a Sequence of Conditions
The ConditionGroups are grouped under a SUCCESS_ON_ONE Parallel
"""
parallel_condition_groups = py_trees.composites.Parallel(name,
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
for condition_group in node.iter("ConditionGroup"):
if success_on_all:
condition_group_sequence = py_trees.composites.Parallel(
name="Condition Group", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
else:
condition_group_sequence = py_trees.composites.Parallel(
name="Condition Group", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
for condition in condition_group.iter("Condition"):
criterion = OpenScenarioParser.convert_condition_to_atomic(
condition, self.other_actors + self.ego_vehicles)
if sequence is not None and maneuver is not None:
xml_path = get_xml_path(story, sequence) + '>' + \
get_xml_path(maneuver, condition) # See note in get_xml_path
else:
xml_path = get_xml_path(story, condition)
criterion = oneshot_with_check(variable_name=xml_path, behaviour=criterion)
condition_group_sequence.add_child(criterion)
if condition_group_sequence.children:
parallel_condition_groups.add_child(condition_group_sequence)
return parallel_condition_groups
def _initialize_parameters(self):
"""
Parse ParameterAction from Init and update global osc parameters.
"""
param_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="ParametersInit")
for i, global_action in enumerate(self.config.init.find('Actions').iter('GlobalAction')):
maneuver_name = 'InitParams'
if global_action.find('ParameterAction') is not None:
parameter_action = global_action.find('ParameterAction')
parameter_ref = parameter_action.attrib.get('parameterRef')
if parameter_action.find('ModifyAction') is not None:
action_rule = parameter_action.find('ModifyAction').find("Rule")
if action_rule.find("AddValue") is not None:
rule, value = '+', action_rule.find("AddValue").attrib.get('value')
else:
rule, value = '*', action_rule.find("MultiplyByValue").attrib.get('value')
else:
rule, value = None, parameter_action.find('SetAction').attrib.get('value')
parameter_update = ChangeParameter(parameter_ref, value=ParameterRef(value), rule=rule,
name=maneuver_name + '_%d' % i)
param_behavior.add_child(oneshot_with_check(variable_name="InitialParameters" + '_%d' % i,
behaviour=parameter_update))
return param_behavior
def _create_behavior(self):
"""
Basic behavior do nothing, i.e. Idle
"""
story_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Story")
joint_actor_list = self.other_actors + self.ego_vehicles + [None]
for act in self.config.story.iter("Act"):
act_sequence = py_trees.composites.Sequence(
name="Act StartConditions and behaviours")
start_conditions = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="StartConditions Group")
parallel_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="Maneuver + EndConditions Group")
parallel_sequences = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
name="Maneuvers")
for sequence in act.iter("ManeuverGroup"):
sequence_behavior = py_trees.composites.Sequence(
name=sequence.attrib.get('name'))
repetitions = sequence.attrib.get('maximumExecutionCount', 1)
for _ in range(int(repetitions)):
actor_ids = []
for actor in sequence.iter("Actors"):
for entity in actor.iter("EntityRef"):
entity_name = entity.attrib.get('entityRef', None)
for k, _ in enumerate(joint_actor_list):
if joint_actor_list[
k] and entity_name == joint_actor_list[
k].attributes['role_name']:
actor_ids.append(k)
break
if not actor_ids:
print(
"Warning: Maneuvergroup {} does not use reference actors!"
.format(sequence.attrib.get('name')))
actor_ids.append(len(joint_actor_list) - 1)
# Collect catalog reference maneuvers in order to process them at the same time as normal maneuvers
catalog_maneuver_list = []
for catalog_reference in sequence.iter("CatalogReference"):
catalog_maneuver = OpenScenarioParser.get_catalog_entry(
self.config.catalogs, catalog_reference)
catalog_maneuver_list.append(catalog_maneuver)
all_maneuvers = itertools.chain(
iter(catalog_maneuver_list), sequence.iter("Maneuver"))
single_sequence_iteration = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
name=sequence_behavior.name)
for maneuver in all_maneuvers: # Iterates through both CatalogReferences and Maneuvers
maneuver_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.
SUCCESS_ON_ALL,
name="Maneuver " + maneuver.attrib.get('name'))
for event in maneuver.iter("Event"):
event_sequence = py_trees.composites.Sequence(
name="Event " + event.attrib.get('name'))
parallel_actions = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.
SUCCESS_ON_ALL,
name="Actions")
for child in event.iter():
if child.tag == "Action":
for actor_id in actor_ids:
maneuver_behavior = OpenScenarioParser.convert_maneuver_to_atomic(
child, joint_actor_list[actor_id],
joint_actor_list,
self.config.catalogs)
maneuver_behavior = StoryElementStatusToBlackboard(
maneuver_behavior, "ACTION",
child.attrib.get('name'))
parallel_actions.add_child(
oneshot_with_check(variable_name=# See note in get_xml_path
get_xml_path(self.config.story, sequence) + '>' + \
get_xml_path(maneuver, child),
behaviour=maneuver_behavior))
if child.tag == "StartTrigger":
# There is always one StartConditions block per Event
parallel_condition_groups = self._create_condition_container(
child, "Parallel Condition Groups",
sequence, maneuver)
event_sequence.add_child(
parallel_condition_groups)
parallel_actions = StoryElementStatusToBlackboard(
parallel_actions, "EVENT",
event.attrib.get('name'))
event_sequence.add_child(parallel_actions)
maneuver_parallel.add_child(
oneshot_with_check(
variable_name=get_xml_path(
self.config.story, sequence) + '>' +
get_xml_path(maneuver,
event), # See get_xml_path
behaviour=event_sequence))
maneuver_parallel = StoryElementStatusToBlackboard(
maneuver_parallel, "MANEUVER",
maneuver.attrib.get('name'))
single_sequence_iteration.add_child(
oneshot_with_check(
variable_name=get_xml_path(
self.config.story, sequence) + '>' +
maneuver.attrib.get(
'name'), # See get_xml_path
behaviour=maneuver_parallel))
# OpenSCENARIO refers to Sequences as Scenes in this instance
single_sequence_iteration = StoryElementStatusToBlackboard(
single_sequence_iteration, "SCENE",
sequence.attrib.get('name'))
single_sequence_iteration = repeatable_behavior(
single_sequence_iteration,
get_xml_path(self.config.story, sequence))
sequence_behavior.add_child(single_sequence_iteration)
if sequence_behavior.children:
parallel_sequences.add_child(
oneshot_with_check(variable_name=get_xml_path(
self.config.story, sequence),
behaviour=sequence_behavior))
if parallel_sequences.children:
parallel_sequences = StoryElementStatusToBlackboard(
parallel_sequences, "ACT", act.attrib.get('name'))
parallel_behavior.add_child(parallel_sequences)
start_triggers = act.find("StartTrigger")
if list(start_triggers) is not None:
for start_condition in start_triggers:
parallel_start_criteria = self._create_condition_container(
start_condition, "StartConditions")
if parallel_start_criteria.children:
start_conditions.add_child(parallel_start_criteria)
end_triggers = act.find("StopTrigger")
if end_triggers is not None and list(end_triggers) is not None:
for end_condition in end_triggers:
parallel_end_criteria = self._create_condition_container(
end_condition, "EndConditions", success_on_all=False)
if parallel_end_criteria.children:
parallel_behavior.add_child(parallel_end_criteria)
if start_conditions.children:
act_sequence.add_child(start_conditions)
if parallel_behavior.children:
act_sequence.add_child(parallel_behavior)
if act_sequence.children:
story_behavior.add_child(act_sequence)
# Build behavior tree
behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
name="behavior")
env_behavior = self._create_environment_behavior()
if env_behavior is not None:
behavior.add_child(
oneshot_with_check(variable_name="InitialEnvironmentSettings",
behaviour=env_behavior))
init_behavior = self._create_init_behavior()
if init_behavior is not None:
behavior.add_child(
oneshot_with_check(variable_name="InitialActorSettings",
behaviour=init_behavior))
behavior.add_child(story_behavior)
return behavior