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
for act in self.config.story.iter("Act"):
if act.attrib.get('name') != 'Behavior':
continue
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("Sequence"):
sequence_behavior = py_trees.composites.Sequence()
repetitions = sequence.attrib.get('numberOfExecutions', 1)
actor_ids = []
for actor in sequence.iter("Actors"):
for entity in actor.iter("Entity"):
for k, _ in enumerate(joint_actor_list):
if entity.attrib.get('name', None) == joint_actor_list[k].attributes['role_name']:
actor_ids.append(k)
break
single_sequence_iteration = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=sequence.attrib.get('name'))
for maneuver in sequence.iter("Maneuver"):
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])
maneuver_behavior = StoryElementStatusToBlackboard(
maneuver_behavior, "ACTION", child.attrib.get('name'))
parallel_actions.add_child(
oneshot_behavior(maneuver_behavior))
if child.tag == "StartConditions":
# There is always one StartConditions block per Event
parallel_condition_groups = self._create_condition_container(
child, "Parallel Condition Groups")
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_behavior(event_sequence))
maneuver_parallel = StoryElementStatusToBlackboard(
maneuver_parallel, "MANEUVER", maneuver.attrib.get('name'))
single_sequence_iteration.add_child(
oneshot_behavior(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)
for _ in range(int(repetitions)):
sequence_behavior.add_child(single_sequence_iteration)
if sequence_behavior.children:
parallel_sequences.add_child(
oneshot_behavior(sequence_behavior))
if parallel_sequences.children:
parallel_sequences = StoryElementStatusToBlackboard(
parallel_sequences, "ACT", act.attrib.get('name'))
parallel_behavior.add_child(parallel_sequences)
for conditions in act.iter("Conditions"):
for start_condition in conditions.iter("Start"):
parallel_start_criteria = self._create_condition_container(
start_condition, "StartConditions", oneshot=True)
if parallel_start_criteria.children:
start_conditions.add_child(parallel_start_criteria)
for end_condition in conditions.iter("End"):
parallel_end_criteria = self._create_condition_container(
end_condition, "EndConditions")
if parallel_end_criteria.children:
parallel_behavior.add_child(parallel_end_criteria)
for cancel_condition in conditions.iter("Cancel"):
parallel_cancel_criteria = self._create_condition_container(
cancel_condition, "CancelConditions")
if parallel_cancel_criteria.children:
parallel_behavior.add_child(parallel_cancel_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
# sequence.add_child(maneuver_behavior)
return story_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
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"):
for k, _ in enumerate(joint_actor_list):
if entity.attrib.get('entityRef', None) == joint_actor_list[k].attributes['role_name']:
actor_ids.append(k)
break
if not actor_ids:
print("Warning: Maneuvergroup does not use reference actors!")
# 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 = self.config.catalogs[catalog_reference.attrib.get(
"catalogName")][catalog_reference.attrib.get("entryName")]
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])
maneuver_behavior = StoryElementStatusToBlackboard(
maneuver_behavior, "ACTION", child.attrib.get('name'))
parallel_actions.add_child(
oneshot_behavior(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_behavior(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_behavior(variable_name=get_xml_path(self.config.story, sequence) + '>' +
get_xml_path(maneuver, maneuver), # 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_behavior(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")
init_behavior = self._create_init_behavior()
if init_behavior is not None:
behavior.add_child(oneshot_behavior(variable_name=get_xml_path(
self.config.story, self.config.story), behaviour=init_behavior))
behavior.add_child(story_behavior)
return behavior
def _create_behavior(self):
"""
Basic behavior do nothing, i.e. Idle
"""
story_behavior = py_trees.composites.Sequence("Story")
for act in self.config.story.iter("Act"):
if act.attrib.get('name') != 'Behavior':
continue
parallel_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="Maneuver + EndConditions Group")
for sequence in act.iter("Sequence"):
sequence_behavior = py_trees.composites.Sequence()
repetitions = sequence.attrib.get('numberOfExecutions', 1)
actor_ids = []
for actor in sequence.iter("Actors"):
for entity in actor.iter("Entity"):
for k, _ in enumerate(self.other_actors):
if entity.attrib.get(
'name', None
) == self.config.other_actors[k].rolename:
actor_ids.append(k)
break
tmp_sequence_behavior = py_trees.composites.Sequence(
name=sequence.attrib.get('name'))
for maneuver in sequence.iter("Maneuver"):
maneuver_sequence = py_trees.composites.Sequence(
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, self.other_actors[actor_id])
parallel_actions.add_child(
maneuver_behavior)
if child.tag == "StartConditions":
# There is always on StartConditions block per Event
for condition in child.iter('Condition'):
condition_behavior = OpenScenarioParser.convert_condition_to_atomic(
condition,
self.other_actors + self.ego_vehicles)
condition_behavior.name += " for {}".format(
parallel_actions.name)
if condition_behavior:
event_sequence.add_child(
condition_behavior)
event_sequence.add_child(parallel_actions)
maneuver_sequence.add_child(event_sequence)
tmp_sequence_behavior.add_child(maneuver_sequence)
for _ in range(int(repetitions)):
sequence_behavior.add_child(tmp_sequence_behavior)
if sequence_behavior.children:
parallel_behavior.add_child(sequence_behavior)
for conditions in act.iter("Conditions"):
start_condition_behavior = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
name="StartConditions Group")
for start_condition in conditions.iter("Start"):
for condition in start_condition.iter('Condition'):
condition_behavior = OpenScenarioParser.convert_condition_to_atomic(
condition, self.other_actors + self.ego_vehicles)
oneshot_idiom = oneshot_behavior(
name=condition_behavior.name,
variable_name=condition_behavior.name,
behaviour=condition_behavior)
start_condition_behavior.add_child(oneshot_idiom)
for end_condition in conditions.iter("End"):
for condition in end_condition.iter('Condition'):
condition_behavior = OpenScenarioParser.convert_condition_to_atomic(
condition, self.other_actors + self.ego_vehicles)
parallel_behavior.add_child(condition_behavior)
for end_condition in conditions.iter("Cancel"):
for condition in end_condition.iter('Condition'):
condition_behavior = OpenScenarioParser.convert_condition_to_atomic(
condition, self.other_actors + self.ego_vehicles)
parallel_behavior.add_child(condition_behavior)
if start_condition_behavior.children:
story_behavior.add_child(start_condition_behavior)
if parallel_behavior.children:
story_behavior.add_child(parallel_behavior)
# Build behavior tree
# sequence.add_child(maneuver_behavior)
return story_behavior
