diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 68a0af373..a50746887 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -14,10 +14,14 @@ ### :rocket: New Features * OpenSCENARIO support: - Added `--openscenarioparams` argument to overwrite global `ParameterDeclaration` + - Added controller using CARLA's autopilot (in replacement for ActivateControllerAction) + - Added support for storyboards with multiple stories ### :bug: Bug Fixes * Fixed bug at the Getting Started docs which caused an import error ### :ghost: Maintenance +* Extended SimpleVehicleController (OSC) to handle traffic lights * Generalized visualizer attached to OSC controllers +* Fixed bug at the Getting Started docs which caused an import error ## CARLA ScenarioRunner 0.9.11 ### :rocket: New Features diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index 834b4c35b..ad15be0ba 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -48,7 +48,7 @@ def __init__(self, filename, client, custom_params): self.weather = carla.WeatherParameters() self.storyboard = self.xml_tree.find("Storyboard") - self.story = self.storyboard.find("Story") + self.stories = self.storyboard.findall("Story") self.init = self.storyboard.find("Init") logging.basicConfig() @@ -61,7 +61,7 @@ def __init__(self, filename, client, custom_params): def _validate_openscenario_configuration(self): """ - Validate the given OpenSCENARIO config against the 0.9.1 XSD + Validate the given OpenSCENARIO config against the 1.0 XSD Note: This will throw if the config is not valid. But this is fine here. """ @@ -71,7 +71,7 @@ def _validate_openscenario_configuration(self): def _validate_openscenario_catalog_configuration(self, catalog_xml_tree): """ - Validate the given OpenSCENARIO catalog config against the 0.9.1 XSD + Validate the given OpenSCENARIO catalog config against the 1.0 XSD Note: This will throw if the catalog config is not valid. But this is fine here. """ @@ -168,15 +168,25 @@ def _set_carla_town(self): # workaround for relative positions during init world = self.client.get_world() - if world is None or world.get_map().name != self.town: - self.logger.warning(" Wrong OpenDRIVE map in use. Forcing reload of CARLA world") + wmap = None + if world: + wmap = world.get_map() + + if world is None or (wmap is not None and wmap.name != self.town): if ".xodr" in self.town: with open(self.town) as od_file: data = od_file.read() - self.client.generate_opendrive_world(str(data)) + old_map = "" + if wmap is not None: + old_map = wmap.to_opendrive() + if data != old_map: + self.logger.warning(" Wrong OpenDRIVE map in use. Forcing reload of CARLA world") + self.client.generate_opendrive_world(str(data)) + world = self.client.get_world() else: + self.logger.warning(" Wrong map in use. Forcing reload of CARLA world") self.client.load_world(self.town) - world = self.client.get_world() + world = self.client.get_world() CarlaDataProvider.set_world(world) world.wait_for_tick() else: diff --git a/srunner/scenariomanager/actorcontrols/carla_autopilot.py b/srunner/scenariomanager/actorcontrols/carla_autopilot.py new file mode 100644 index 000000000..5b030119f --- /dev/null +++ b/srunner/scenariomanager/actorcontrols/carla_autopilot.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +This module provides an example control for vehicles which +use CARLA's autopilot functionality + +Limitations: +- No direct velocity control +- No lateral maneuvers can be enforced +""" + + +from srunner.scenariomanager.actorcontrols.basic_control import BasicControl + + +class CarlaAutoPilotControl(BasicControl): + + """ + Controller class for vehicles derived from BasicControl. + + The controller uses CARLA's autopilot functionality. As a result, + the vehicle respects other traffic participants and traffic rules. + However, no longitudinal or lateral maneuvers can be enforced. + + Args: + actor (carla.Actor): Vehicle actor that should be controlled. + args (dictionary): Dictonary of (key, value) arguments to be used by the controller. + """ + + def __init__(self, actor, args=None): + super(CarlaAutoPilotControl, self).__init__(actor) + self._actor.set_autopilot(enabled=True) + + def reset(self): + """ + Reset the controller + """ + self._actor.set_autopilot(enabled=False) + + def run_step(self): + """ + Everything is controlled through CARLA's autopilot functionality. + Nothing to do here + """ + pass diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index f0f2a8c6c..90b7481e9 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright (c) 2020 Intel Corporation +# Copyright (c) 2020-2021 Intel Corporation # # This work is licensed under the terms of the MIT license. # For a copy, see . @@ -10,7 +10,7 @@ does not use CARLA's vehicle engine. Limitations: -- Does not respect any traffic regulation: speed limit, traffic light, priorities, etc. +- Does not respect any traffic regulation: speed limit, priorities, etc. - Can only consider obstacles in forward facing reaching (i.e. in tight corners obstacles may be ignored). """ @@ -41,14 +41,23 @@ class SimpleVehicleControl(BasicControl): which checks for obstacles in the direct forward channel of the vehicle, i.e. there are limitation with sideways obstacles and while cornering. + The controller can also respect red traffic lights, and use a given deceleration + value for more realistic behavior. Both behaviors are activated via the corresponding + controller arguments. + Args: actor (carla.Actor): Vehicle actor that should be controlled. args (dictionary): Dictonary of (key, value) arguments to be used by the controller. - May include: (consider_obstacles, true/false) - Enable consideration of obstacles - (proximity_threshold, distance) - Distance in front of actor in which - obstacles are considered - (attach_camera, true/false) - Attach OpenCV display to actor - (useful for debugging) + May include: (consider_obstacles, true/false) - Enable consideration of obstacles + (proximity_threshold, distance) - Distance in front of actor in which + obstacles are considered + (consider_trafficlights, true/false) - Enable consideration of traffic lights + (max_deceleration, float) - Use a reasonable deceleration value for + this vehicle + (max_acceleration, float) - Use a reasonable acceleration value for + this vehicle + (attach_camera, true/false) - Attach OpenCV display to actor + (useful for debugging) Attributes: @@ -77,8 +86,11 @@ def __init__(self, actor, args=None): super(SimpleVehicleControl, self).__init__(actor) self._generated_waypoint_list = [] self._last_update = None + self._consider_traffic_lights = False self._consider_obstacles = False self._proximity_threshold = float('inf') + self._max_deceleration = None + self._max_acceleration = None self._obstacle_sensor = None self._obstacle_distance = float('inf') @@ -99,6 +111,15 @@ def __init__(self, actor, args=None): bp, carla.Transform(carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor) self._obstacle_sensor.listen(lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda + if args and 'consider_trafficlights' in args and strtobool(args['consider_trafficlights']): + self._consider_traffic_lights = strtobool(args['consider_trafficlights']) + + if args and 'max_deceleration' in args: + self._max_deceleration = float(args['max_deceleration']) + + if args and 'max_acceleration' in args: + self._max_acceleration = float(args['max_acceleration']) + if args and 'attach_camera' in args and strtobool(args['attach_camera']): self._visualizer = Visualizer(self._actor) @@ -138,8 +159,7 @@ def run_step(self): If _waypoints is empty, the vehicle moves in its current direction with the given _target_speed. - If _consider_obstacles is true, the speed is adapted according to the closest - obstacle in front of the actor, if it is within the _proximity_threshold distance. + For further details see :func:`_set_new_velocity` """ if self._reached_goal: @@ -164,13 +184,18 @@ def run_step(self): else: map_wp = CarlaDataProvider.get_map().get_waypoint(self._generated_waypoint_list[-1].location) while len(self._generated_waypoint_list) < 50: - map_wps = map_wp.next(3.0) + map_wps = map_wp.next(2.0) if map_wps: self._generated_waypoint_list.append(map_wps[0].transform) map_wp = map_wps[0] else: break + # Remove all waypoints that are too close to the vehicle + while (self._generated_waypoint_list and + self._generated_waypoint_list[0].location.distance(self._actor.get_location()) < 0.5): + self._generated_waypoint_list = self._generated_waypoint_list[1:] + direction_norm = self._set_new_velocity(self._offset_waypoint(self._generated_waypoint_list[0])) if direction_norm < 2.0: self._generated_waypoint_list = self._generated_waypoint_list[1:] @@ -182,11 +207,14 @@ def run_step(self): self._waypoints = self._waypoints[1:] self._reached_goal = False - direction_norm = self._set_new_velocity(self._offset_waypoint(self._waypoints[0])) - if direction_norm < 4.0: - self._waypoints = self._waypoints[1:] - if not self._waypoints: - self._reached_goal = True + if not self._waypoints: + self._reached_goal = True + else: + direction_norm = self._set_new_velocity(self._offset_waypoint(self._waypoints[0])) + if direction_norm < 4.0: + self._waypoints = self._waypoints[1:] + if not self._waypoints: + self._reached_goal = True def _offset_waypoint(self, transform): """ @@ -215,6 +243,11 @@ def _set_new_velocity(self, next_location): If _consider_obstacles is true, the speed is adapted according to the closest obstacle in front of the actor, if it is within the _proximity_threshold distance. + If _consider_trafficlights is true, the vehicle will enforce a stop at a red + traffic light. + If _max_deceleration is set, the vehicle will reduce its speed according to the + given deceleration value. + If the vehicle reduces its speed, braking lights will be activated. Args: next_location (carla.Location): Next target location of the actor @@ -229,12 +262,13 @@ def _set_new_velocity(self, next_location): if not self._last_update: self._last_update = current_time + current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) + if self._consider_obstacles: # If distance is less than the proximity threshold, adapt velocity if self._obstacle_distance < self._proximity_threshold: distance = max(self._obstacle_distance, 0) if distance > 0: - current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2) current_speed_other = math.sqrt( self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2) if current_speed_other < current_speed: @@ -243,6 +277,22 @@ def _set_new_velocity(self, next_location): else: target_speed = 0 + if self._consider_traffic_lights: + if (self._actor.is_at_traffic_light() and + self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): + target_speed = 0 + + if target_speed < current_speed: + self._actor.set_light_state(carla.VehicleLightState.Brake) + if self._max_deceleration is not None: + target_speed = max(target_speed, current_speed - (current_time - + self._last_update) * self._max_deceleration) + else: + self._actor.set_light_state(carla.VehicleLightState.NONE) + if self._max_acceleration is not None: + target_speed = min(target_speed, current_speed + (current_time - + self._last_update) * self._max_acceleration) + # set new linear velocity velocity = carla.Vector3D(0, 0, 0) direction = next_location - CarlaDataProvider.get_location(self._actor) diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index 6b8b3c35e..40e3a1904 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -593,7 +593,15 @@ def request_new_actors(actor_list): _spawn_point.rotation = transform.rotation _spawn_point.location.x = transform.location.x _spawn_point.location.y = transform.location.y - _spawn_point.location.z = transform.location.z + 0.2 + if blueprint.has_tag('walker'): + # On imported OpenDRIVE maps, spawning of pedestrians can fail. + # By increasing the z-value the chances of success are increased. + if not CarlaDataProvider._map.name.startswith('OpenDrive'): + _spawn_point.location.z = transform.location.z + 0.2 + else: + _spawn_point.location.z = transform.location.z + 0.8 + else: + _spawn_point.location.z = transform.location.z + 0.2 # Get the command command = SpawnActor(blueprint, _spawn_point) diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index c1b0935f9..ec481e8d1 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -184,7 +184,7 @@ def __init__(self, world, ego_vehicles, config, config_file, debug_mode=False, c # Timeout of scenario in seconds self.timeout = timeout - super(OpenScenario, self).__init__("OpenScenario", ego_vehicles=ego_vehicles, config=config, + super(OpenScenario, self).__init__(self.config.name, ego_vehicles=ego_vehicles, config=config, world=world, debug_mode=debug_mode, terminate_on_failure=False, criteria_enable=criteria_enable) @@ -249,135 +249,147 @@ 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") - + stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, + name="OSCStories") 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) + for story in self.config.stories: + story_name = story.get("name") + story_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, + name=story_name) + for act in 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(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, story, "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(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(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(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(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, + story, + "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, story, "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) + + stories_behavior.add_child(oneshot_with_check(variable_name=get_xml_path(story, story) + '>' + + story_name, # See get_xml_path + behaviour=story_behavior)) # Build behavior tree behavior = py_trees.composites.Parallel( @@ -391,11 +403,11 @@ def _create_behavior(self): if init_behavior is not None: behavior.add_child(oneshot_with_check(variable_name="InitialActorSettings", behaviour=init_behavior)) - behavior.add_child(story_behavior) + behavior.add_child(stories_behavior) return behavior - def _create_condition_container(self, node, name='Conditions Group', sequence=None, + 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 @@ -417,10 +429,10 @@ def _create_condition_container(self, node, name='Conditions Group', sequence=No 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(self.config.story, sequence) + '>' + \ + xml_path = get_xml_path(story, sequence) + '>' + \ get_xml_path(maneuver, condition) # See note in get_xml_path else: - xml_path = get_xml_path(self.config.story, condition) + xml_path = get_xml_path(story, condition) criterion = oneshot_with_check(variable_name=xml_path, behaviour=criterion) condition_group_sequence.add_child(criterion)