From ba2de6e5699bd0e6ecfb85ff196ee45a6c0b7d40 Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Tue, 22 Dec 2020 14:04:28 +0100 Subject: [PATCH] Fixed spawning issue of pedestrians on Non-CARLA maps Change-Id: If395133f47102adbf88d00c35f814c49c7414eac --- Docs/CHANGELOG.md | 4 ++ .../actorcontrols/simple_vehicle_control.py | 2 + .../scenariomanager/carla_data_provider.py | 10 ++++- srunner/scenarios/open_scenario.py | 41 +++++++++++-------- 4 files changed, 39 insertions(+), 18 deletions(-) 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/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 3366df0bf..90b7481e9 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -54,6 +54,8 @@ class SimpleVehicleControl(BasicControl): (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) 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 e7323a738..ec481e8d1 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -249,12 +249,14 @@ def _create_behavior(self): Basic behavior do nothing, i.e. Idle """ - stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="OSCStories") + 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 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) + 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( @@ -280,7 +282,8 @@ def _create_behavior(self): 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']: + if (joint_actor_list[k] and + entity_name == joint_actor_list[k].attributes['role_name']): actor_ids.append(k) break @@ -292,7 +295,8 @@ def _create_behavior(self): # 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 = 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( @@ -310,14 +314,15 @@ def _create_behavior(self): 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) + 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)) + 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 @@ -331,14 +336,14 @@ def _create_behavior(self): 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)) + 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)) + maneuver.attrib.get('name'), # See get_xml_path + behaviour=maneuver_parallel)) # OpenSCENARIO refers to Sequences as Scenes in this instance single_sequence_iteration = StoryElementStatusToBlackboard( @@ -351,7 +356,7 @@ def _create_behavior(self): if sequence_behavior.children: parallel_sequences.add_child( oneshot_with_check(variable_name=get_xml_path(story, sequence), - behaviour=sequence_behavior)) + behaviour=sequence_behavior)) if parallel_sequences.children: parallel_sequences = StoryElementStatusToBlackboard( @@ -361,7 +366,9 @@ def _create_behavior(self): 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") + 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") @@ -381,8 +388,8 @@ def _create_behavior(self): 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)) + story_name, # See get_xml_path + behaviour=story_behavior)) # Build behavior tree behavior = py_trees.composites.Parallel(