From 7c643416fdf0a75274ae0202663ff1000d4553c6 Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Fri, 16 Oct 2020 18:24:32 +0200 Subject: [PATCH] OSC Controllers --- .../openscenario_configuration.py | 30 +++++++++------- .../scenarioconfigs/scenario_configuration.py | 3 +- .../actorcontrols/simple_vehicle_control.py | 3 +- srunner/scenarios/open_scenario.py | 11 +++++- srunner/tools/openscenario_parser.py | 34 ++++++++++++++++++- 5 files changed, 65 insertions(+), 16 deletions(-) diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index f7a3df4d6..5afd7da3c 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -242,26 +242,30 @@ def _set_actor_information(self): value = prop.get('value') args[key] = value + actor_controller = [] + for controller in obj.iter("ObjectController"): + actor_controller.append(controller) + for catalog_reference in obj.iter("CatalogReference"): entry = OpenScenarioParser.get_catalog_entry(self.catalogs, catalog_reference) if entry.tag == "Vehicle": - self._extract_vehicle_information(entry, rolename, entry, args) + self._extract_vehicle_information(entry, rolename, entry, args, actor_controller) elif entry.tag == "Pedestrian": - self._extract_pedestrian_information(entry, rolename, entry, args) + self._extract_pedestrian_information(entry, rolename, entry, args, actor_controller) elif entry.tag == "MiscObject": - self._extract_misc_information(entry, rolename, entry, args) + self._extract_misc_information(entry, rolename, entry, args, actor_controller) else: self.logger.debug( " A CatalogReference specifies a reference that is not an Entity. Skipping...") for vehicle in obj.iter("Vehicle"): - self._extract_vehicle_information(obj, rolename, vehicle, args) + self._extract_vehicle_information(obj, rolename, vehicle, args, actor_controller) for pedestrian in obj.iter("Pedestrian"): - self._extract_pedestrian_information(obj, rolename, pedestrian, args) + self._extract_pedestrian_information(obj, rolename, pedestrian, args, actor_controller) for misc in obj.iter("MiscObject"): - self._extract_misc_information(obj, rolename, misc, args) + self._extract_misc_information(obj, rolename, misc, args, actor_controller) # Set transform for all actors # This has to be done in a multi-stage loop to resolve relative position settings @@ -285,7 +289,7 @@ def _set_actor_information(self): if actor.transform is None: all_actor_transforms_set = False - def _extract_vehicle_information(self, obj, rolename, vehicle, args): + def _extract_vehicle_information(self, obj, rolename, vehicle, args, actor_controller): """ Helper function to _set_actor_information for getting vehicle information from XML tree """ @@ -301,25 +305,26 @@ def _extract_vehicle_information(self, obj, rolename, vehicle, args): speed = self._get_actor_speed(rolename) new_actor = ActorConfigurationData( - model, None, rolename, speed, color=color, category=category, args=args) + model, None, rolename, speed, color=color, category=category, args=args, controller=actor_controller) if ego_vehicle: self.ego_vehicles.append(new_actor) else: self.other_actors.append(new_actor) - def _extract_pedestrian_information(self, obj, rolename, pedestrian, args): + def _extract_pedestrian_information(self, obj, rolename, pedestrian, args, actor_controller): """ Helper function to _set_actor_information for getting pedestrian information from XML tree """ model = pedestrian.attrib.get('model', "walker.*") speed = self._get_actor_speed(rolename) - new_actor = ActorConfigurationData(model, None, rolename, speed, category="pedestrian", args=args) + new_actor = ActorConfigurationData(model, None, rolename, speed, + category="pedestrian", args=args, controller=actor_controller) self.other_actors.append(new_actor) - def _extract_misc_information(self, obj, rolename, misc, args): + def _extract_misc_information(self, obj, rolename, misc, args, actor_controller): """ Helper function to _set_actor_information for getting vehicle information from XML tree """ @@ -330,7 +335,8 @@ def _extract_misc_information(self, obj, rolename, misc, args): model = "static.prop.chainbarrier" else: model = misc.attrib.get('name') - new_actor = ActorConfigurationData(model, None, rolename, category="misc", args=args) + new_actor = ActorConfigurationData(model, None, rolename, category="misc", + args=args, controller=actor_controller) self.other_actors.append(new_actor) diff --git a/srunner/scenarioconfigs/scenario_configuration.py b/srunner/scenarioconfigs/scenario_configuration.py index 388a7d2cf..26a2f4149 100644 --- a/srunner/scenarioconfigs/scenario_configuration.py +++ b/srunner/scenarioconfigs/scenario_configuration.py @@ -19,7 +19,7 @@ class ActorConfigurationData(object): """ def __init__(self, model, transform, rolename='other', speed=0, autopilot=False, - random=False, color=None, category="car", args=None): + random=False, color=None, category="car", args=None, controller=None): self.model = model self.rolename = rolename self.transform = transform @@ -29,6 +29,7 @@ def __init__(self, model, transform, rolename='other', speed=0, autopilot=False, self.color = color self.category = category self.args = args + self.controller = controller @staticmethod def parse_from_node(node, rolename): diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 473e78cd1..222fbd0b2 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -282,7 +282,8 @@ def _set_new_velocity(self, next_location): self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): target_speed = 0 - if target_speed < current_speed: + if target_speed < current_speed and math.fabs(target_speed - current_speed) > 0.01: + print(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 - diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index ec481e8d1..0c24008b4 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -225,12 +225,21 @@ def _create_init_behavior(self): if private.attrib.get('entityRef', None) == actor.rolename: for private_action in private.iter("PrivateAction"): for controller_action in private_action.iter('ControllerAction'): - module, args = OpenScenarioParser.get_controller( + module, args = OpenScenarioParser.get_controller_from_action( controller_action, self.config.catalogs) controller_atomic = ChangeActorControl( carla_actor, control_py_module=module, args=args, scenario_file_path=os.path.dirname(self.config.filename)) + if actor.controller is not None: + if controller_atomic is not None: + print("Warning: A controller was already assigned to actor {}".format(actor.model)) + else: + for controller in actor.controller: + print(actor.rolename) + module, args = OpenScenarioParser.get_controller(controller, self.config.catalogs) + controller_atomic = ChangeActorControl(carla_actor, control_py_module=module, args=args) + if controller_atomic is None: controller_atomic = ChangeActorControl(carla_actor, control_py_module=None, args={}) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 7aa355531..8bf52e106 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -365,7 +365,7 @@ def get_weather_from_env_action(xml_tree, catalogs): return Weather(carla_weather, dtime, weather_animation) @staticmethod - def get_controller(xml_tree, catalogs): + def get_controller_from_action(xml_tree, catalogs): """ Extract the object controller from the OSC XML or a catalog @@ -403,6 +403,38 @@ def get_controller(xml_tree, catalogs): return module, args + @staticmethod + def get_controller(xml_tree, catalogs): + """ + Extract the object controller from the OSC XML or a catalog + + Args: + xml_tree: Containing the controller information, + or the reference to the catalog it is defined in. + catalogs: XML Catalogs that could contain the EnvironmentAction + + returns: + module: Python module containing the controller implementation + args: Dictonary with (key, value) parameters for the controller + """ + + properties = None + if xml_tree.find('Controller') is not None: + properties = xml_tree.find('Controller').find('Properties') + elif xml_tree.find("CatalogReference") is not None: + catalog_reference = xml_tree.find("CatalogReference") + properties = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference).find('Properties') + + module = None + args = {} + for prop in properties: + if prop.attrib.get('name') == "module": + module = prop.attrib.get('value') + else: + args[prop.attrib.get('name')] = prop.attrib.get('value') + + return module, args + @staticmethod def get_route(xml_tree, catalogs): """