Fixed repetitions in OpenSCENARIO (Fixes #443)

Repetitions in OpenSCENARIO were not properly working. Reworked the repetition
handling by three major changes:
- Not copy py_tree elements, this will cause problems due to references in Python
- Replaced get_py_tree_path with get_xml_path, as the first was not working properly
- Avoid using ":" in py_tree behavior names, as this may break py_trees

Change-Id: I7829106136cd6c20e6863d1f424fbc600030bf13

Docs/CHANGELOG.md

@@ -48,6 +48,7 @@
     - Added support for MiscObjects (besides vehicles and pedestrians)
     - Reworked traffic signal handling: The name has to start now either with "id=" or "pos=" depending on whether the position or id is used as unique identifier
 ### :bug: Bug Fixes
+* Fixed #443: Repetitions in OpenSCENARIO were not properly working
 * Fixed bug causing occasional frame_errors
 * Fixed #426: Avoid underground vehicles fall forever by disabling physics when spawning underground.
 * Fixed #427: Removed unnecessary warnings when using get_next_traffic_light() with non-cached locations

srunner/scenarios/open_scenario.py

@@ -27,14 +27,14 @@ def repeatable_behavior(behaviour, name=None):
         name = behaviour.name
     clear_descendant_variables = ClearBlackboardVariablesStartingWith(
         name="Clear Descendant Variables of {}".format(name),
-        variable_name_beginning=get_py_tree_path(behaviour) + ">"
+        variable_name_beginning=name + ">"
     )
     # If it's a sequence, don't double-nest it in a redundant manner
     if isinstance(behaviour, py_trees.composites.Sequence):
         behaviour.add_child(clear_descendant_variables)
         sequence = behaviour
     else:
-        sequence = py_trees.composites.Sequence(name="RepeatableBehaviour")
+        sequence = py_trees.composites.Sequence(name="RepeatableBehaviour of {}".format(name))
         sequence.add_children([behaviour, clear_descendant_variables])
     return sequence
 
@@ -60,10 +60,10 @@ def initialise(self):
         """
         Delete the variables from the blackboard.
         """
-        blackboard_variables = [key for key, _ in py_trees.blackboard.__dict__.items(
+        blackboard_variables = [key for key, _ in py_trees.blackboard.Blackboard().__dict__.items(
         ) if key.startswith(self.variable_name_beginning)]
         for variable in blackboard_variables:
-            delattr(py_trees.blackboard, variable)
+            delattr(py_trees.blackboard.Blackboard(), variable)
 
 
 class StoryElementStatusToBlackboard(Decorator):
@@ -77,12 +77,7 @@ class StoryElementStatusToBlackboard(Decorator):
         element_name: the story element's name attribute
     """
 
-    def __init__(
-            self,
-            child,
-            story_element_type,
-            element_name
-    ):
+    def __init__(self, child, story_element_type, element_name):
         super(StoryElementStatusToBlackboard, self).__init__(name=child.name, child=child)
         self.story_element_type = story_element_type
         self.element_name = element_name
@@ -147,20 +142,20 @@ def terminate(self, new_status):
             )
 
 
-def get_py_tree_path(behaviour):
+def get_xml_path(tree, node):
     """
-    Accept a behaviour/composite and return a string representation of its full path
+    Extract the full path of a node within an XML tree
     """
+
     path = ""
-    target = behaviour
-    while True:
-        path = "{}>{}".format(target.name, path)
-        target = target.parent
-        if not target:
-            break
+    parent_map = {c: p for p in tree.iter() for c in p}
 
-    path = path[:-1]
+    cur_node = node
+    while cur_node != tree:
+        path = "{}>{}".format(cur_node.attrib.get('name'), path)
+        cur_node = parent_map[cur_node]
 
+    path = path[:-1]
     return path
 
 
@@ -229,67 +224,71 @@ def _create_behavior(self):
                 policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Maneuvers")
 
             for sequence in act.iter("Sequence"):
-                sequence_behavior = py_trees.composites.Sequence(name="Seq:" + sequence.attrib.get('name'))
+                sequence_behavior = py_trees.composites.Sequence(name=sequence.attrib.get('name'))
                 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(variable_name=get_py_tree_path(maneuver_behavior),
-                                                         behaviour=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(variable_name=get_py_tree_path(event_sequence), behaviour=event_sequence))
-                    maneuver_parallel = StoryElementStatusToBlackboard(
-                        maneuver_parallel, "MANEUVER", maneuver.attrib.get('name'))
-                    single_sequence_iteration.add_child(
-                        oneshot_behavior(variable_name=get_py_tree_path(maneuver_parallel),
-                                         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)
+
                 for _ in range(int(repetitions)):
+
+                    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_behavior.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(variable_name=get_xml_path(self.config.story, child),
+                                                             behaviour=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(variable_name=get_xml_path(self.config.story, event),
+                                                 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, maneuver),
+                                             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_py_tree_path(sequence_behavior),
+                        oneshot_behavior(variable_name=get_xml_path(self.config.story, sequence),
                                          behaviour=sequence_behavior))
 
             if parallel_sequences.children:
@@ -328,7 +327,8 @@ def _create_behavior(self):
 
         init_behavior = self._create_init_behavior()
         if init_behavior is not None:
-            behavior.add_child(oneshot_behavior(variable_name=get_py_tree_path(init_behavior), behaviour=init_behavior))
+            behavior.add_child(oneshot_behavior(variable_name=get_xml_path(
+                self.config.story, self.config.story), behaviour=init_behavior))
 
         behavior.add_child(story_behavior)
 
@@ -352,7 +352,8 @@ def _create_condition_container(self, node, name='Conditions Group', oneshot=Fal
                 criterion = OpenScenarioParser.convert_condition_to_atomic(
                     condition, self.other_actors + self.ego_vehicles)
                 if oneshot:
-                    criterion = oneshot_behavior(variable_name=get_py_tree_path(criterion), behaviour=criterion)
+                    criterion = oneshot_behavior(variable_name=get_xml_path(
+                        self.config.story, condition), behaviour=criterion)
                 condition_group_sequence.add_child(criterion)
 
             if condition_group_sequence.children: