diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index 9b5b15783..621420499 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -21,6 +21,9 @@ ### :bug: Bug Fixes * Fixed bug at OtherLeadingVehicle scenario causing the vehicles to move faster than intended * Fixed bug causing some debris at ControlLoss scenario to be floating, instead of at ground level +* Fixed bug causing OpenSCENARIO UserDefinedCommand subprocess runner to exit before executing script +* Fixed retrieval of OpenSCENARIO parameter values +* Fixed bug causing effects of OpenSCENARIO ParameterAction to be ignored in the execution of maneuvers ## CARLA ScenarioRunner 0.9.13 ### :rocket: New Features diff --git a/Docs/openscenario_support.md b/Docs/openscenario_support.md index 359b715ad..c2cc5b445 100755 --- a/Docs/openscenario_support.md +++ b/Docs/openscenario_support.md @@ -69,7 +69,7 @@ contains of submodules, which are not listed, the support status applies to all | ---------------------------------------- | ---------------------------------------- | ---------------------------------------- | ---------------------------------------- | | `EntityAction` | ❌ | ❌ | | | `EnvironmentAction` | ✅ | ❌ | | -| `ParameterAction` | ✅ | ✅ | | +| `ParameterAction` | ✅ | ✅ | OSC Actions fully support ParameterActions. OSC conditions do not yet support ParameterAction: Changes to parameter values by ParameterAction aren't reflected in condition execution. | | `InfrastructureAction` `TrafficSignalAction`
`TrafficAction` | ❌ | ❌ | | | `InfrastructureAction` `TrafficSignalAction`
`TrafficSignalControllerAction` | ❌ | ❌ | | | `InfrastructureAction` `TrafficSignalAction`
`TrafficSignalStateAction` | ❌ | ✅ | As traffic signals in CARLA towns have no unique ID, they have to be set by providing their position (Example: TrafficSignalStateAction name="pos=x,y" state="green"). The id can also be used for none CARLA town (Example: TrafficSignalStateAction name="id=n" state="green") | diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index ce4d99b09..f5f6e41bf 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright (c) 2019 Intel Corporation +# Copyright (c) 2019-2021 Intel Corporation # # This work is licensed under the terms of the MIT license. # For a copy, see . @@ -98,7 +98,8 @@ def _check_version(self): Ensure correct OpenSCENARIO version is used """ header = self.xml_tree.find("FileHeader") - if not (header.attrib.get('revMajor') == "1" and header.attrib.get('revMinor') == "0"): + if not (str(ParameterRef(header.attrib.get('revMajor'))) == "1" and + str(ParameterRef(header.attrib.get('revMinor'))) == "0"): raise AttributeError("Only OpenSCENARIO 1.0 is supported") def _load_catalogs(self): @@ -124,7 +125,8 @@ def _load_catalogs(self): if catalog is None: continue - catalog_path = catalog.find("Directory").attrib.get('path') + "/" + catalog_type + "Catalog.xosc" + catalog_path = str(ParameterRef(catalog.find("Directory").attrib.get('path'))) + \ + "/" + catalog_type + "Catalog.xosc" if not os.path.isabs(catalog_path) and "xosc" in self.filename: catalog_path = os.path.dirname(os.path.abspath(self.filename)) + "/" + catalog_path @@ -134,17 +136,17 @@ def _load_catalogs(self): xml_tree = ET.parse(catalog_path) self._validate_openscenario_catalog_configuration(xml_tree) catalog = xml_tree.find("Catalog") - catalog_name = catalog.attrib.get("name") + catalog_name = str(ParameterRef(catalog.attrib.get("name"))) self.catalogs[catalog_name] = {} for entry in catalog: - self.catalogs[catalog_name][entry.attrib.get("name")] = entry + self.catalogs[catalog_name][str(ParameterRef(entry.attrib.get("name")))] = entry def _set_scenario_name(self): """ Extract the scenario name from the OpenSCENARIO header information """ header = self.xml_tree.find("FileHeader") - self.name = header.attrib.get('description', 'Unknown') + self.name = str(ParameterRef(header.attrib.get('description', 'Unknown'))) if self.name.startswith("CARLA:"): self.name = self.name[6:] @@ -158,7 +160,7 @@ def _set_carla_town(self): Hence, there can be multiple towns specified. We just use the _last_ one. """ for logic in self.xml_tree.find("RoadNetwork").findall("LogicFile"): - self.town = logic.attrib.get('filepath', None) + self.town = str(ParameterRef(logic.attrib.get('filepath', None))) if self.town is not None and ".xodr" in self.town: if not os.path.isabs(self.town): @@ -235,7 +237,7 @@ def _set_actor_information(self): """ for entity in self.xml_tree.iter("Entities"): for obj in entity.iter("ScenarioObject"): - rolename = obj.attrib.get('name', 'simulation') + rolename = str(ParameterRef(obj.attrib.get('name', 'simulation'))) args = {} for prop in obj.iter("Property"): key = prop.get('name') @@ -290,8 +292,8 @@ def _extract_vehicle_information(self, obj, rolename, vehicle, args): Helper function to _set_actor_information for getting vehicle information from XML tree """ color = None - model = vehicle.attrib.get('name', "vehicle.*") - category = vehicle.attrib.get('vehicleCategory', "car") + model = str(ParameterRef(vehicle.attrib.get('name', "vehicle.*"))) + category = str(ParameterRef(vehicle.attrib.get('vehicleCategory', "car"))) ego_vehicle = False for prop in obj.iter("Property"): if prop.get('name', '') == 'type': @@ -312,7 +314,7 @@ def _extract_pedestrian_information(self, obj, rolename, pedestrian, args): """ Helper function to _set_actor_information for getting pedestrian information from XML tree """ - model = pedestrian.attrib.get('model', "walker.*") + model = str(ParameterRef(pedestrian.attrib.get('model', "walker.*"))) speed = self._get_actor_speed(rolename) new_actor = ActorConfigurationData(model, None, rolename, speed, category="pedestrian", args=args) @@ -323,13 +325,13 @@ def _extract_misc_information(self, obj, rolename, misc, args): """ Helper function to _set_actor_information for getting vehicle information from XML tree """ - category = misc.attrib.get('miscObjectCategory') + category = str(ParameterRef(misc.attrib.get('miscObjectCategory'))) if category == "barrier": model = "static.prop.streetbarrier" elif category == "guardRail": model = "static.prop.chainbarrier" else: - model = misc.attrib.get('name') + model = str(ParameterRef(misc.attrib.get('name'))) new_actor = ActorConfigurationData(model, None, rolename, category="misc", args=args) self.other_actors.append(new_actor) @@ -351,7 +353,7 @@ def _get_actor_transform(self, actor_name): actor_found = False for private_action in self.init.iter("Private"): - if private_action.attrib.get('entityRef', None) == actor_name: + if str(ParameterRef(private_action.attrib.get('entityRef', None))) == actor_name: if actor_found: # pylint: disable=line-too-long self.logger.warning( @@ -380,7 +382,7 @@ def _get_actor_speed(self, actor_name): actor_found = False for private_action in self.init.iter("Private"): - if private_action.attrib.get('entityRef', None) == actor_name: + if str(ParameterRef(private_action.attrib.get('entityRef', None))) == actor_name: if actor_found: # pylint: disable=line-too-long self.logger.warning( diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index f98beb047..00ae80fbe 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -40,7 +40,6 @@ from srunner.tools.scenario_helper import detect_lane_obstacle from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane - import srunner.tools as sr_tools EPSILON = 0.001 @@ -87,6 +86,16 @@ def get_actor_control(actor): return control, actor_type +def get_param_value(param_ref, param_type): + """ + Method to retrieve the value of an OSC parameter from ParameterRef + """ + if param_ref is None: + return None + else: + return param_type(param_ref) + + class AtomicBehavior(py_trees.behaviour.Behaviour): """ @@ -143,13 +152,13 @@ class RunScript(AtomicBehavior): This is an atomic behavior to start execution of an additional script. Args: - script (str): String containing the interpreter, scriptpath and arguments - Example: "python /path/to/script.py --arg1" + script (list(str)): A list of String elements that contain the interpreter, scriptpath and arguments + Example: ['python', '/path/to/script.py', '--arg1', '$parameter_arg'] base_path (str): String containing the base path of for the script Attributes: - _script (str): String containing the interpreter, scriptpath and arguments - Example: "python /path/to/script.py --arg1" + _script (list(str)): A list of String elements that contain the interpreter, scriptpath and arguments + Example: ['python', '/path/to/script.py', '--arg1', '$parameter_arg'] _base_path (str): String containing the base path of for the script Example: "/path/to/" @@ -171,9 +180,14 @@ def update(self): Start script """ path = None - script_components = self._script.split(' ') - if len(script_components) > 1: - path = script_components[1] + + interpreted_components = [] + for component in self._script: + interpreted_components.append(str(component)) + interpreted_script = ' '.join(interpreted_components) + + if len(interpreted_components) > 1: + path = interpreted_components[1] if not os.path.isfile(path): path = os.path.join(self._base_path, path) @@ -181,7 +195,15 @@ def update(self): new_status = py_trees.common.Status.FAILURE print("Script file does not exists {}".format(path)) else: - subprocess.Popen(self._script, shell=True, cwd=self._base_path) # pylint: disable=consider-using-with + process = subprocess.Popen(interpreted_script, shell=True, # pylint: disable=consider-using-with + cwd=self._base_path) # pylint: disable=consider-using-with + + while process.poll() is None: + try: + process.wait(timeout=5) + except subprocess.TimeoutExpired: + print("Waiting for user defined command to complete...") + new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) @@ -229,29 +251,34 @@ class ChangeWeather(AtomicBehavior): """ Atomic to write a new weather configuration into the blackboard. Used in combination with WeatherBehavior() to have a continuous weather simulation. - The behavior immediately terminates with SUCCESS after updating the blackboard. - Args: - weather (srunner.scenariomanager.weather_sim.Weather): New weather settings. name (string): Name of the behavior. Defaults to 'UpdateWeather'. - + global_action (OSC GlobalAction xml): An OSC global action containing the EnvironmentAction, + or the reference to the catalog it is defined in. + catalogs: XML Catalogs that could contain the EnvironmentAction Attributes: _weather (srunner.scenariomanager.weather_sim.Weather): Weather settings. """ - def __init__(self, weather, name="ChangeWeather"): + def __init__(self, global_action, catalogs, name="ChangeWeather"): """ Setup parameters """ super(ChangeWeather, self).__init__(name) - self._weather = weather + self._global_action = global_action + self._catalogs = catalogs + self._weather = None + + def initialise(self): + if self._weather is None: + self._weather = sr_tools.openscenario_parser.OpenScenarioParser.get_weather_from_env_action( + self._global_action, self._catalogs) def update(self): """ Write weather into blackboard and exit with success - returns: py_trees.common.Status.SUCCESS """ @@ -267,20 +294,28 @@ class ChangeRoadFriction(AtomicBehavior): The behavior immediately terminates with SUCCESS after updating the friction. Args: - friction (float): New friction coefficient. name (string): Name of the behavior. Defaults to 'UpdateRoadFriction'. - + global_action (OSC GlobalAction xml): An OSC global action containing the EnvironmentAction, + or the reference to the catalog it is defined in. + catalogs: XML Catalogs that could contain the EnvironmentAction Attributes: - _friction (float): Friction coefficient. + _friction (float | ParameterRef(float)): Friction coefficient. """ - def __init__(self, friction, name="ChangeRoadFriction"): + def __init__(self, global_action, catalogs, name="ChangeRoadFriction"): """ Setup parameters """ super(ChangeRoadFriction, self).__init__(name) - self._friction = friction + self._global_action = global_action + self._catalogs = catalogs + self._friction = None + + def initialise(self): + if self._friction is None: + self._friction = sr_tools.openscenario_parser.OpenScenarioParser.get_friction_from_env_action( + self._global_action, self._catalogs) def update(self): """ @@ -329,14 +364,32 @@ class ChangeActorControl(AtomicBehavior): _actor_control (ActorControl): Instance of the actor control. """ - def __init__(self, actor, control_py_module, args, scenario_file_path=None, name="ChangeActorControl"): + def __init__(self, actor, osc_controller_action=None, catalogs=None, scenario_file_path=None, + name="ChangeActorControl"): """ Setup actor controller. """ super(ChangeActorControl, self).__init__(name, actor) - self._actor_control = ActorControl(actor, control_py_module=control_py_module, - args=args, scenario_file_path=scenario_file_path) + self._actor = actor + self._actor_control = None + self._controller_action = osc_controller_action + self._catalogs = catalogs + self._scenario_file_path = scenario_file_path + + def initialise(self): + + if self._controller_action is not None: + control_py_module, args = sr_tools.openscenario_parser.OpenScenarioParser.get_controller( + self._controller_action, self._catalogs) + else: + control_py_module = None + args = {} + + self._actor_control = ActorControl(self._actor, control_py_module=control_py_module, + args=args, scenario_file_path=self._scenario_file_path) + + super(ChangeActorControl, self).initialise() def update(self): """ @@ -415,50 +468,86 @@ class ChangeActorTargetSpeed(AtomicBehavior): for the same actor is triggered. Args: - actor (carla.Actor): Controlled actor. - target_speed (float): New target speed [m/s]. - init_speed (boolean): Flag to indicate if the speed is the initial actor speed. + actor (carla.Actor): + Controlled actor. + target_speed (float | ParameterRef(float)): + New target speed [m/s]. + init_speed (boolean): + Flag to indicate if the speed is the initial actor speed. Defaults to False. - duration (float): Duration of the maneuver [s]. + dimension (string | ParameterRef(string)): + 'distance' or 'duration': if maneuver length is specified by its distance or duration. Defaults to None. - distance (float): Distance of the maneuver [m]. + dimension_value (float | ParameterRef(float)): + Duration [s] or distance [m] of the maneuver. Defaults to None. - relative_actor (carla.Actor): If the target speed setting should be relative to another actor. + relative_actor_name (string | ParameterRef(string)): + If the target speed setting should be relative to another actor. Defaults to None. - value (float): Offset, if the target speed setting should be relative to another actor. + value (float | ParameterRef(float)): + Offset, if the target speed setting should be relative to another actor. Defaults to None. - value_type (string): Either 'Delta' or 'Factor' influencing how the offset to the reference actors - velocity is applied. Defaults to None. - continuous (boolean): If True, the atomic remains in RUNNING, independent of duration or distance. + value_type (string | ParameterRef(string)): + Either 'Delta' or 'Factor' influencing how the offset to the reference actors velocity is applied. + Defaults to None. + continuous (boolean | ParameterRef(boolean)): + If True, the atomic remains in RUNNING, independent of duration or distance. Defaults to False. - name (string): Name of the behavior. + actor_list (list(carla.Actor)): + List of current carla actors + Defaults to None + name (string): + Name of the behavior. Defaults to 'ChangeActorTargetSpeed'. Attributes: - _target_speed (float): New target speed [m/s]. - _init_speed (boolean): Flag to indicate if the speed is the initial actor speed. + _target_speed (float): + New target speed [m/s]. + _init_speed (boolean): + Flag to indicate if the speed is the initial actor speed. Defaults to False. - _start_time (float): Start time of the atomic [s]. + _start_time (float): + Start time of the atomic [s]. + Defaults to None. + _start_location (carla.Location): + Start location of the atomic. Defaults to None. - _start_location (carla.Location): Start location of the atomic. + _duration (float): + Duration of the maneuver [s]. Defaults to None. - _duration (float): Duration of the maneuver [s]. + _distance (float): + Distance of the maneuver [m]. Defaults to None. - _distance (float): Distance of the maneuver [m]. + _relative_actor (carla.Actor): + If the target speed setting should be relative to another actor. Defaults to None. - _relative_actor (carla.Actor): If the target speed setting should be relative to another actor. + _dimension (string | ParameterRef(string)): + 'distance' or 'duration': if maneuver length is specified by its distance or duration. Defaults to None. - _value (float): Offset, if the target speed setting should be relative to another actor. + _dimension_value (float | ParameterRef(float)): + Duration [s] or distance [m] of the maneuver. Defaults to None. - _value_type (string): Either 'Delta' or 'Factor' influencing how the offset to the reference actors + _relative_actor_name (string | ParameterRef(string)): + If the target speed setting should be relative to another actor. + Defaults to None. + _value (float | ParameterRef(float)): + Offset, if the target speed setting should be relative to another actor. + Defaults to None. + _value_type (string | ParameterRef(string)): + Either 'Delta' or 'Factor' influencing how the offset to the reference actors velocity is applied. Defaults to None. - _continuous (boolean): If True, the atomic remains in RUNNING, independent of duration or distance. + _continuous (boolean | ParameterRef(boolean)): + If True, the atomic remains in RUNNING, independent of duration or distance. Defaults to False. + _actor_list (list(carla.Actor)): + List of current carla actors + Defaults to None """ def __init__(self, actor, target_speed, init_speed=False, - duration=None, distance=None, relative_actor=None, - value=None, value_type=None, continuous=False, name="ChangeActorTargetSpeed"): + dimension=None, dimension_value=None, relative_actor_name=None, + value=None, value_type=None, continuous=False, + actor_list=None, name="ChangeActorTargetSpeed"): """ Setup parameters """ @@ -469,13 +558,17 @@ def __init__(self, actor, target_speed, init_speed=False, self._start_time = None self._start_location = None + self._duration = None + self._distance = None + self._relative_actor = None - self._relative_actor = relative_actor + self._dimension = dimension + self._dimension_value = dimension_value + self._relative_actor_name = relative_actor_name self._value = value self._value_type = value_type self._continuous = continuous - self._duration = duration - self._distance = distance + self._actor_list = actor_list def initialise(self): """ @@ -485,6 +578,7 @@ def initialise(self): May throw if actor is not available as key for the ActorsWithController dictionary from Blackboard. """ + self._get_parameter_values() actor_dict = {} try: @@ -517,6 +611,35 @@ def initialise(self): super(ChangeActorTargetSpeed, self).initialise() + def _get_parameter_values(self): + """ + Get the current OSC parameter values from ParameterRef's + """ + self._target_speed = get_param_value(self._target_speed, float) + self._value = get_param_value(self._value, float) + self._value_type = get_param_value(self._value_type, str) + self._continuous = get_param_value(self._continuous, bool) + + dimension = get_param_value(self._dimension, str) + if dimension == "distance": + self._distance = get_param_value(self._dimension_value, float) + self._duration = float('inf') + elif dimension == "duration": + self._distance = float('inf') + self._duration = get_param_value(self._dimension_value, float) + elif dimension is None: + pass + else: + raise ValueError("SpeedActionDynamics:" + + "Please specify any one attribute of: [distance, duration]") + + obj = get_param_value(self._relative_actor_name, str) + if obj is not None: + for traffic_actor in self._actor_list: + if (traffic_actor is not None and 'role_name' in traffic_actor.attributes and + traffic_actor.attributes['role_name'] == obj): + self._relative_actor = traffic_actor + def update(self): """ Check the actor's current state and update target speed, if it is relative to another actor. @@ -576,22 +699,24 @@ class SyncArrivalOSC(AtomicBehavior): Args: actor (carla.Actor): Controlled actor. - master_actor (carla.Actor): Reference actor to sync up to. - actor_target (carla.Transform): Endpoint of the actor after the behavior finishes. - master_target (carla.Transform): Endpoint of the master_actor after the behavior finishes. - final_speed (float): Speed of the actor after the behavior ends. + master_actor_name (string | ParameterRef(string)): Name of the reference actor to sync up to. + actor_target (OSC position): Endpoint of the actor after the behavior finishes. + master_target (OSC position): Endpoint of the master_actor after the behavior finishes. + final_speed (float | ParameterRef(float)): Speed of the actor after the behavior ends. relative_to_master (boolean): Whether or not the final speed is relative to master_actor. Defaults to False. - relative_type (string): Type of relative speed. Either 'delta' or 'factor'. + relative_type (string | ParameterRef(string)): Type of relative speed. Either 'delta' or 'factor'. Defaults to ''. + actor_list (list(carla.Actor)): List of the current Carla actors. + Defaults to None. name (string): Name of the behavior. Defaults to 'SyncArrivalOSC'. """ DISTANCE_THRESHOLD = 1 - def __init__(self, actor, master_actor, actor_target, master_target, final_speed, - relative_to_master=False, relative_type='', name="SyncArrivalOSC"): + def __init__(self, actor, master_actor_name, actor_target, master_target, final_speed, + relative_to_master=False, relative_type='', actor_list=None, name="SyncArrivalOSC"): """ Setup required parameters """ @@ -599,17 +724,44 @@ def __init__(self, actor, master_actor, actor_target, master_target, final_speed self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._actor = actor - self._actor_target = actor_target - self._master_actor = master_actor - self._master_target = master_target + self._master_actor_name = master_actor_name + self._master_actor = None + + self._actor_target_osc = actor_target + self._actor_target = None + self._master_target_osc = master_target + self._master_target = None self._final_speed = final_speed self._final_speed_set = False self._relative_to_master = relative_to_master self._relative_type = relative_type + self._actor_list = actor_list self._start_time = None + def _get_parameter_values(self): + """ + Get the current OSC parameter values from ParameterRef's + """ + self._relative_type = get_param_value(self._relative_type, str) + self._final_speed = get_param_value(self._final_speed, float) + + master_actor_name = get_param_value(self._master_actor_name, str) + for actor_ins in self._actor_list: + if actor_ins is not None and 'role_name' in actor_ins.attributes: + if master_actor_name == actor_ins.attributes['role_name']: + self._master_actor = actor_ins + break + if self._master_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format( + master_actor_name)) + + self._actor_target = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( + self._actor_target_osc) + self._master_target = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform( + self._master_target_osc) + def initialise(self): """ Set initial parameters and get (actor, controller) pair from Blackboard. @@ -617,6 +769,7 @@ def initialise(self): May throw if actor is not available as key for the ActorsWithController dictionary from Blackboard. """ + self._get_parameter_values() actor_dict = {} try: @@ -741,11 +894,20 @@ class ChangeActorWaypoints(AtomicBehavior): waypoints (List of (OSC position, OSC route option)): List of (Position, Route Option) as OpenScenario elements. position will be converted to Carla transforms, considering the corresponding route option (e.g. shortest, fastest) + Defaults to None. Optional with osc_route_action and catalogs. + osc_route_action (OSC RoutingAction xml): An OSC routing action definition. + Defaults to None. Optional with waypoints. + catalogs: XML Catalogs that could contain the route. + Defaults to None. Optional with waypoints. name (string): Name of the behavior. Defaults to 'ChangeActorWaypoints'. Attributes: _waypoints (List of (OSC position, OSC route option)): List of (Position, Route Option) as OpenScenario elements + _osc_route_action (OSC RoutingAction xml): An OSC routing action definition. + Defaults to None. Optional with waypoints. + _catalogs: XML Catalogs that could contain the route. + Defaults to None. Optional with waypoints. _start_time (float): Start time of the atomic [s]. Defaults to None. @@ -753,15 +915,22 @@ class ChangeActorWaypoints(AtomicBehavior): in synchronous mode """ - def __init__(self, actor, waypoints, name="ChangeActorWaypoints"): + def __init__(self, actor, waypoints=None, osc_route_action=None, catalogs=None, name="ChangeActorWaypoints"): """ Setup parameters """ super(ChangeActorWaypoints, self).__init__(name, actor) + self._route_action = osc_route_action + self._catalogs = catalogs self._waypoints = waypoints self._start_time = None + def _get_parameter_values(self): + if self._waypoints is None: + self._waypoints = sr_tools.openscenario_parser.OpenScenarioParser.get_route( + self._route_action, self._catalogs) + def initialise(self): """ Set _start_time and get (actor, controller) pair from Blackboard. @@ -771,6 +940,7 @@ def initialise(self): May throw if actor is not available as key for the ActorsWithController dictionary from Blackboard. """ + self._get_parameter_values() actor_dict = {} try: @@ -885,51 +1055,99 @@ class ChangeActorLateralMotion(AtomicBehavior): waypoints until such impossibility is found. Args: - actor (carla.Actor): Controlled actor. - direction (string): Lane change direction ('left' or 'right'). - Defaults to 'left'. - distance_lane_change (float): Distance of the lance change [meters]. + actor (carla.Actor): + Controlled actor. + target_lane_rel (int | ParameterRef(int)): + Number of lane changes to perform relative to current lane. Negative integers to + switch left, positive to switch right. + Defaults to 1. + dimension (string | ParameterRef(string)): + The dimension along which to control dynamics of the lane change. 'direction' or 'duration'. + 'duration' currently not implemented. + Defaults to 'direction' + dimension_value (float | ParameterRef(float)): + The distance (m) or duration (s) of the lane change. Defaults to 25. - distance_other_lane (float): Driven distance after the lange change [meters]. + distance_other_lane (float): + Driven distance after the lange change [meters]. Defaults to 100. name (string): Name of the behavior. Defaults to 'ChangeActorLateralMotion'. Attributes: _waypoints (List of carla.Transform): List of waypoints representing the lane change (CARLA transforms). - _direction (string): Lane change direction ('left' or 'right'). - _distance_same_lane (float): Distance on the same lane before the lane change starts [meters] + _dimension (string | ParameterRef(string)): + The dimension along which to control dynamics of the lane change. 'direction' or 'duration'. + 'duration' currently not implemented. + Defaults to 'direction' + _dimension_value (float | ParameterRef(float)): + The distance (m) or duration (s) of the lane change. + Defaults to 25. + _direction (string): + Lane change direction ('left' or 'right'). + _distance_same_lane (float): + Distance on the same lane before the lane change starts [meters] Constant to 5. - _distance_other_lane (float): Max. distance on the target lane after the lance change [meters] + _distance_other_lane (float): + Max. distance on the target lane after the lane change [meters] Constant to 100. - _distance_lane_change (float): Max. total distance of the lane change [meters]. - _pos_before_lane_change: carla.Location of the actor before the lane change. + _distance_lane_change (float): + Max. total distance of the lane change [meters]. + _duration_lane_change (float): + Max. total duration of the lane change [s]. + _pos_before_lane_change: + carla.Location of the actor before the lane change. Defaults to None. - _target_lane_id (int): Id of the target lane + _target_lane_id (int): + Id of the target lane Defaults to None. - _start_time (float): Start time of the atomic [s]. + _start_time (float): + Start time of the atomic [s]. Defaults to None. """ - def __init__(self, actor, direction='left', distance_lane_change=25, distance_other_lane=100, - lane_changes=1, name="ChangeActorLateralMotion"): + def __init__(self, actor, target_lane_rel=1, dimension="distance", dimension_value=25, distance_other_lane=100, + name="ChangeActorLateralMotion"): """ Setup parameters """ super(ChangeActorLateralMotion, self).__init__(name, actor) self._waypoints = [] - self._direction = direction - self._distance_same_lane = 5 + self._target_lane_rel = target_lane_rel + self._dimension = dimension + self._dimension_value = dimension_value self._distance_other_lane = distance_other_lane - self._distance_lane_change = distance_lane_change - self._lane_changes = lane_changes + + self._distance_same_lane = 5 + self._distance_lane_change = float('inf') + self._duration_lane_change = float('inf') + + self._direction = None + self._lane_changes = None self._pos_before_lane_change = None self._target_lane_id = None self._plan = None self._start_time = None + def _get_parameter_values(self): + """ + Get the current OSC parameter values from ParameterRef's + """ + self._direction = "left" if self._target_lane_rel > 0 else "right" + self._lane_changes = abs(self._target_lane_rel) + + # duration and distance + dimension = str(self._dimension) + if dimension == "distance": + self._distance_lane_change = float(self._dimension_value) + elif dimension == "duration": + self._duration_lane_change = float(self._dimension_value) + raise NotImplementedError("LaneChangeActionDynamics: duration is not implemented") + else: + raise ValueError("Dimension has to be one of ['distance', 'duration']") + def initialise(self): """ Set _start_time and get (actor, controller) pair from Blackboard. @@ -940,6 +1158,7 @@ def initialise(self): May throw if actor is not available as key for the ActorsWithController dictionary from Blackboard. """ + self._get_parameter_values() actor_dict = {} try: @@ -1042,16 +1261,24 @@ class ChangeActorLaneOffset(AtomicBehavior): Args: actor (carla.Actor): Controlled actor. - offset (float): Float determined the distance to the center of the lane. Positive distance imply a - displacement to the right, while negative displacements are to the left. - relative_actor (carla.Actor): The actor from which the offset is taken from. Defaults to None - continuous (bool): If True, the behaviour never ends. If False, the behaviour ends when the lane - offset is reached. Defaults to True. + offset (float | ParameterRef(float)): Float determined the distance to the center of the lane. + Positive distance imply a displacement to the right, negative displacements are to the left. + relative_actor_name (string | ParameterRef(string)): Name of the actor from which the offset is taken. + Defaults to None. + continuous (bool | ParameterRef(bool)): If True, the behaviour never ends. If False, the behaviour + ends when the lane offset is reached. Defaults to True. + actor_list (list(carla.Actor)): List of current carla actors + Defaults to None Attributes: - _offset (float): lane offset. - _relative_actor (carla.Actor): relative actor. - _continuous (bool): stored the value of the 'continuous' argument. + _offset (float | ParameterRef(float)): lane offset. + _relative_actor_name (string | ParameterRef(string)): stored value of the relative_actor_name argument. + Defaults to None. + _continuous (bool | ParameterRef(bool)): stored the value of the 'continuous' argument. + _relative_actor (carla.Actor): The actor from which the offset is taken from. + Defaults to None + _actor_list (list(carla.Actor)): List of current carla actors + Defaults to None _start_time (float): Start time of the atomic [s]. Defaults to None. _overwritten (bool): flag to check whether or not this behavior was overwritten by another. Helps @@ -1062,21 +1289,41 @@ class ChangeActorLaneOffset(AtomicBehavior): OFFSET_THRESHOLD = 0.1 - def __init__(self, actor, offset, relative_actor=None, continuous=True, name="ChangeActorWaypoints"): + def __init__(self, actor, offset, relative_actor_name=None, continuous=True, actor_list=None, + name="ChangeActorWaypoints"): """ Setup parameters """ super(ChangeActorLaneOffset, self).__init__(name, actor) self._offset = offset - self._relative_actor = relative_actor + self._relative_actor_name = relative_actor_name self._continuous = continuous + self._relative_actor = None + self._actor_list = actor_list self._start_time = None self._current_target_offset = 0 self._overwritten = False self._map = CarlaDataProvider.get_map() + def _get_parameter_values(self): + """ + Get the current OSC parameter values from ParameterRef's + """ + self._offset = get_param_value(self._offset, float) + self._continuous = get_param_value(self._continuous, bool) + self._relative_actor_name = get_param_value(self._relative_actor_name, str) + + if self._relative_actor_name is not None: + for _actor in self._actor_list: + if _actor is not None and 'role_name' in _actor.attributes: + if self._relative_actor_name == _actor.attributes['role_name']: + self._relative_actor = _actor + break + if self._relative_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format(self._relative_actor_name)) + def initialise(self): """ Set _start_time and get (actor, controller) pair from Blackboard. @@ -1086,6 +1333,7 @@ def initialise(self): May throw if actor is not available as key for the ActorsWithController dictionary from Blackboard. """ + self._get_parameter_values() actor_dict = {} try: @@ -1500,7 +1748,7 @@ class ChangeAutoPilot(AtomicBehavior): Important parameters: - actor: CARLA actor to execute the behavior - - activate: True (=enable autopilot) or False (=disable autopilot) + - activate (bool | ParameterRef(bool)): True (=enable autopilot) or False (=disable autopilot) - lane_change: Traffic Manager parameter. True (=enable lane changes) or False (=disable lane changes) - distance_between_vehicles: Traffic Manager parameter - max_speed: Traffic Manager parameter. Max speed of the actor. This will only work for road segments @@ -1524,7 +1772,7 @@ def update(self): """ De/activate autopilot """ - self._actor.set_autopilot(self._activate, CarlaDataProvider.get_traffic_manager_port()) + self._actor.set_autopilot(bool(self._activate), CarlaDataProvider.get_traffic_manager_port()) if self._parameters is not None: if "auto_lane_change" in self._parameters: @@ -2315,8 +2563,8 @@ class TrafficLightStateSetter(AtomicBehavior): This class contains an atomic behavior to set the state of a given traffic light Args: - actor (carla.TrafficLight): ID of the traffic light that shall be changed - state (carla.TrafficLightState): New target state + actor (ParameterRef(string)): ID of the traffic light that shall be changed + state (ParameterRef(string)): New target state The behavior terminates after trying to set the new state """ @@ -2327,7 +2575,7 @@ def __init__(self, actor, state, name="TrafficLightStateSetter"): """ super(TrafficLightStateSetter, self).__init__(name) - self._actor = actor if "traffic_light" in actor.type_id else None + self._actor = actor self._state = state self.logger.debug("%s.__init__()" % (self.__class__.__name__)) @@ -2335,12 +2583,21 @@ def update(self): """ Change the state of the traffic light """ - if self._actor is None: + actor = sr_tools.openscenario_parser.OpenScenarioParser.get_traffic_light_from_osc_name( + str(self._actor)) + traffic_light = actor if "traffic_light" in actor.type_id else None + + tl_state = str(self._state).upper() + if tl_state not in sr_tools.openscenario_parser.OpenScenarioParser.tl_states: + raise KeyError("CARLA only supports Green, Red, Yellow or Off") + traffic_light_state = sr_tools.openscenario_parser.OpenScenarioParser.tl_states[tl_state] + + if traffic_light is None: return py_trees.common.Status.FAILURE new_status = py_trees.common.Status.RUNNING - if self._actor.is_alive: - self._actor.set_state(self._state) + if traffic_light.is_alive: + traffic_light.set_state(traffic_light_state) new_status = py_trees.common.Status.SUCCESS else: # For some reason the actor is gone... @@ -3026,7 +3283,7 @@ class KeepLongitudinalGap(AtomicBehavior): """ def __init__(self, actor, reference_actor, gap, gap_type="distance", max_speed=None, continues=False, - freespace=False, name="AutoKeepDistance"): + freespace=False, actor_list=None, name="AutoKeepDistance"): """ Setup parameters """ @@ -3037,15 +3294,35 @@ def __init__(self, actor, reference_actor, gap, gap_type="distance", max_speed=N self._gap_type = gap_type self._continues = continues self._freespace = freespace + self._actor_list = actor_list self._global_rp = None max_speed_limit = 100 - self.max_speed = max_speed_limit if max_speed is None else float(max_speed) - if freespace and self._gap_type == "distance": - self._gap += self._reference_actor.bounding_box.extent.x + self._actor.bounding_box.extent.x - + self.max_speed = max_speed_limit if max_speed is None else max_speed self._start_time = None + def _get_parameter_values(self): + """ + Get the current values of OSC parameters. + Type cast fetches the value. + """ + + obj = get_param_value(self._reference_actor, str) + for traffic_actor in self._actor_list: + if (traffic_actor is not None and 'role_name' in traffic_actor.attributes and + traffic_actor.attributes['role_name'] == obj): + obj_actor = traffic_actor + + self._reference_actor = obj_actor + self._gap = get_param_value(self._gap, float) + self._continues = get_param_value(self._continues, bool) + self._freespace = get_param_value(self._freespace, bool) + self.max_speed = get_param_value(self.max_speed, float) + + if self._freespace and self._gap_type == "distance": + self._gap += self._reference_actor.bounding_box.extent.x + self._actor.bounding_box.extent.x + def initialise(self): + self._get_parameter_values() actor_dict = {} try: diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 4236d4425..85b2b1560 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -226,10 +226,9 @@ def _create_environment_behavior(self): env_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior") - weather_update = ChangeWeather( - OpenScenarioParser.get_weather_from_env_action(self.config.init, self.config.catalogs)) - road_friction = ChangeRoadFriction( - OpenScenarioParser.get_friction_from_env_action(self.config.init, self.config.catalogs)) + weather_update = ChangeWeather(self.config.init, self.config.catalogs) + road_friction = ChangeRoadFriction(self.config.init, self.config.catalogs) + env_behavior.add_child(oneshot_with_check(variable_name="InitialWeather", behaviour=weather_update)) env_behavior.add_child(oneshot_with_check(variable_name="InitRoadFriction", behaviour=road_friction)) @@ -252,14 +251,15 @@ 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( - controller_action, self.config.catalogs) + controller_atomic = ChangeActorControl( - carla_actor, control_py_module=module, args=args, + carla_actor, + osc_controller_action=controller_action, + catalogs=self.config.catalogs, scenario_file_path=os.path.dirname(self.config.filename)) if controller_atomic is None: - controller_atomic = ChangeActorControl(carla_actor, control_py_module=None, args={}) + controller_atomic = ChangeActorControl(carla_actor) actor_init_behavior.add_child(controller_atomic) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 65e239737..457892e77 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -12,11 +12,12 @@ from __future__ import print_function from distutils.util import strtobool -import re import copy import datetime import math import operator +from os import environ +import re import py_trees import carla @@ -120,6 +121,22 @@ def _is_matching(self, pattern: str) -> bool: return matching_string == self.reference_text return False + def _cast_type(self, other): + try: + if isinstance(other, str): + return self.__str__() + elif isinstance(other, float): + return self.__float__() + elif isinstance(other, int): + return self.__int__() + elif isinstance(other, bool): + return self.__bool__() + else: + raise Exception("Type conversion for type {} not implemented".format(type(other))) + except ValueError: + # If the parameter value can't be converted into type of 'other' + return None + def get_interpreted_value(self): """ Returns: interpreted value from reference_text @@ -152,10 +169,20 @@ def __str__(self) -> str: value = self.get_interpreted_value() return str(value) if value is not None else self.reference_text + def __bool__(self) -> bool: + try: + return bool(strtobool(self.__str__())) + except ValueError: + raise Exception("could not convert '{}' to bool".format(self.__str__())) + def __repr__(self): value = self.get_interpreted_value() return value if value is not None else self.reference_text + def __hash__(self): + value = self.get_interpreted_value() + return hash(value) if value is not None else hash(self.reference_text) + def __radd__(self, other) -> bool: return other + self.__float__() @@ -181,34 +208,44 @@ def __rtruediv__(self, other) -> bool: return other / self.__float__() def __eq__(self, other) -> bool: - return other == self.__float__() + value = self._cast_type(other) + return other == value def __ne__(self, other) -> bool: - return other != self.__float__() + value = self._cast_type(other) + return other != value def __ge__(self, other) -> bool: - return self.__float__() >= other + value = self._cast_type(other) + return value >= other def __le__(self, other) -> bool: - return self.__float__() <= other + value = self._cast_type(other) + return value <= other def __gt__(self, other) -> bool: - return self.__float__() > other + value = self._cast_type(other) + return value > other def __lt__(self, other) -> bool: - return self.__float__() < other + value = self._cast_type(other) + return value < other def __GE__(self, other) -> bool: # pylint: disable=invalid-name - return self.__float__() >= other + value = self._cast_type(other) + return value >= other def __LE__(self, other) -> bool: # pylint: disable=invalid-name - return self.__float__() <= other + value = self._cast_type(other) + return value <= other def __GT__(self, other) -> bool: # pylint: disable=invalid-name - return self.__float__() > other + value = self._cast_type(other) + return value > other def __LT__(self, other) -> bool: # pylint: disable=invalid-name - return self.__float__() < other + value = self._cast_type(other) + return value < other def __iadd__(self, other) -> bool: return self.__float__() + other @@ -361,8 +398,9 @@ def get_catalog_entry(catalogs, catalog_reference): returns: Catalog entry (XML ElementTree) """ - entry_name = str(ParameterRef(catalog_reference.attrib.get("entryName"))) - entry = catalogs[catalog_reference.attrib.get("catalogName")][entry_name] + catalog_name = ParameterRef(catalog_reference.attrib.get("catalogName")) + catalog_entry = ParameterRef(catalog_reference.attrib.get("entryName")) + entry = catalogs[catalog_name][catalog_entry] entry_copy = copy.deepcopy(entry) catalog_copy = copy.deepcopy(catalog_reference) entry = OpenScenarioParser.assign_catalog_parameters(entry_copy, catalog_copy) @@ -414,12 +452,10 @@ def assign_catalog_parameters(entry_instance, catalog_reference): def get_friction_from_env_action(xml_tree, catalogs): """ Extract the CARLA road friction coefficient from an OSC EnvironmentAction - Args: xml_tree: Containing the EnvironmentAction, or the reference to the catalog it is defined in. catalogs: XML Catalogs that could contain the EnvironmentAction - returns: friction (float) """ @@ -440,7 +476,7 @@ def get_friction_from_env_action(xml_tree, catalogs): road_condition = environment.iter("RoadCondition") for condition in road_condition: - friction = condition.attrib.get('frictionScaleFactor') + friction = float(ParameterRef(condition.attrib.get('frictionScaleFactor'))) return friction @@ -448,12 +484,10 @@ def get_friction_from_env_action(xml_tree, catalogs): def get_weather_from_env_action(xml_tree, catalogs): """ Extract the CARLA weather parameters from an OSC EnvironmentAction - Args: xml_tree: Containing the EnvironmentAction, or the reference to the catalog it is defined in. catalogs: XML Catalogs that could contain the EnvironmentAction - returns: Weather (srunner.scenariomanager.weather_sim.Weather) """ @@ -474,11 +508,11 @@ def get_weather_from_env_action(xml_tree, catalogs): sun = weather.find("Sun") carla_weather = carla.WeatherParameters() - carla_weather.sun_azimuth_angle = math.degrees(float(sun.attrib.get('azimuth', 0))) - carla_weather.sun_altitude_angle = math.degrees(float(sun.attrib.get('elevation', 0))) - carla_weather.cloudiness = 100 - float(sun.attrib.get('intensity', 0)) * 100 + carla_weather.sun_azimuth_angle = math.degrees(float(ParameterRef(sun.attrib.get('azimuth', 0)))) + carla_weather.sun_altitude_angle = math.degrees(float(ParameterRef(sun.attrib.get('elevation', 0)))) + carla_weather.cloudiness = 100 - float(ParameterRef(sun.attrib.get('intensity', 0))) * 100 fog = weather.find("Fog") - carla_weather.fog_distance = float(fog.attrib.get('visualRange', 'inf')) + carla_weather.fog_distance = float(ParameterRef(fog.attrib.get('visualRange', 'inf'))) if carla_weather.fog_distance < 1000: carla_weather.fog_density = 100 carla_weather.precipitation = 0 @@ -486,16 +520,16 @@ def get_weather_from_env_action(xml_tree, catalogs): carla_weather.wetness = 0 carla_weather.wind_intensity = 30.0 precepitation = weather.find("Precipitation") - if precepitation.attrib.get('precipitationType') == "rain": - carla_weather.precipitation = float(precepitation.attrib.get('intensity')) * 100 + if ParameterRef(precepitation.attrib.get('precipitationType')) == "rain": + carla_weather.precipitation = float(ParameterRef(precepitation.attrib.get('intensity'))) * 100 carla_weather.precipitation_deposits = 100 # if it rains, make the road wet carla_weather.wetness = carla_weather.precipitation - elif precepitation.attrib.get('type') == "snow": + elif ParameterRef(precepitation.attrib.get('type')) == "snow": raise AttributeError("CARLA does not support snow precipitation") time_of_day = environment.find("TimeOfDay") - weather_animation = strtobool(time_of_day.attrib.get("animation")) - time = time_of_day.attrib.get("dateTime") + weather_animation = strtobool(str(ParameterRef(time_of_day.attrib.get("animation")))) + time = str(ParameterRef(time_of_day.attrib.get("dateTime"))) dtime = datetime.datetime.strptime(time, "%Y-%m-%dT%H:%M:%S") return Weather(carla_weather, dtime, weather_animation) @@ -527,14 +561,14 @@ def get_controller(xml_tree, catalogs): module = None args = {} for prop in properties: - if prop.attrib.get('name') == "module": - module = prop.attrib.get('value') + if ParameterRef(prop.attrib.get('name')) == "module": + module = str(ParameterRef(prop.attrib.get('value'))) else: - args[prop.attrib.get('name')] = prop.attrib.get('value') + args[str(ParameterRef(prop.attrib.get('name')))] = str(ParameterRef(prop.attrib.get('value'))) override_action = xml_tree.find('OverrideControllerValueAction') for child in override_action: - if strtobool(child.attrib.get('active')): + if strtobool(str(ParameterRef(child.attrib.get('active')))): raise NotImplementedError("Controller override actions are not yet supported") return module, args @@ -568,7 +602,7 @@ def get_route(xml_tree, catalogs): if route is not None: for waypoint in route.iter('Waypoint'): position = waypoint.find('Position') - routing_option = str(waypoint.attrib.get('routeStrategy')) + routing_option = str(ParameterRef(waypoint.attrib.get('routeStrategy'))) waypoints.append((position, routing_option)) else: raise AttributeError("No waypoints has been set") @@ -637,7 +671,7 @@ def convert_position_to_transform(position, actor_list=None): droll = 0 if rel_pos.find('Orientation') is not None: orientation = rel_pos.find('Orientation') - is_absolute = (orientation.attrib.get('type') == "absolute") + is_absolute = (str(ParameterRef(orientation.attrib.get('type'))) == "absolute") dyaw = math.degrees(float(ParameterRef(orientation.attrib.get('h', 0)))) dpitch = math.degrees(float(ParameterRef(orientation.attrib.get('p', 0)))) droll = math.degrees(float(ParameterRef(orientation.attrib.get('r', 0)))) @@ -810,19 +844,17 @@ def convert_position_to_transform(position, actor_list=None): def convert_condition_to_atomic(condition, actor_list): """ Convert an OpenSCENARIO condition into a Behavior/Criterion atomic - If there is a delay defined in the condition, then the condition is checked after the delay time passed by, e.g. . - Note: Not all conditions are currently supported. """ atomic = None delay_atomic = None - condition_name = condition.attrib.get('name') + condition_name = str(ParameterRef(condition.attrib.get('name'))) - if condition.attrib.get('delay') is not None and float(condition.attrib.get('delay')) != 0: - delay = float(condition.attrib.get('delay')) + if condition.attrib.get('delay') is not None and float(ParameterRef(condition.attrib.get('delay'))) != 0: + delay = float(ParameterRef(condition.attrib.get('delay'))) delay_atomic = TimeOut(delay) if condition.find('ByEntityCondition') is not None: @@ -833,14 +865,15 @@ def convert_condition_to_atomic(condition, actor_list): for triggering_entities in condition.find('ByEntityCondition').iter('TriggeringEntities'): for entity in triggering_entities.iter('EntityRef'): for actor in actor_list: - if actor is not None and entity.attrib.get('entityRef', None) == actor.attributes['role_name']: + ref_name = str(ParameterRef(entity.attrib.get('entityRef', None))) + if (actor is not None and ref_name == actor.attributes['role_name']): trigger_actor = actor break for entity_condition in condition.find('ByEntityCondition').iter('EntityCondition'): if entity_condition.find('EndOfRoadCondition') is not None: end_road_condition = entity_condition.find('EndOfRoadCondition') - condition_duration = ParameterRef(end_road_condition.attrib.get('duration')) + condition_duration = float(ParameterRef(end_road_condition.attrib.get('duration'))) atomic_cls = py_trees.meta.inverter(EndofRoadTest) atomic = atomic_cls(trigger_actor, condition_duration, terminate_on_failure=True, name=condition_name) @@ -852,20 +885,21 @@ def convert_condition_to_atomic(condition, actor_list): collision_entity = collision_condition.find('EntityRef') for actor in actor_list: - if collision_entity.attrib.get('entityRef', None) == actor.attributes['role_name']: + if (str(ParameterRef(collision_entity.attrib.get('entityRef', None))) + == actor.attributes['role_name']): triggered_actor = actor break if triggered_actor is None: raise AttributeError("Cannot find actor '{}' for condition".format( - collision_condition.attrib.get('entityRef', None))) + str(ParameterRef(collision_condition.attrib.get('entityRef', None))))) atomic_cls = py_trees.meta.inverter(CollisionTest) atomic = atomic_cls(trigger_actor, other_actor=triggered_actor, terminate_on_failure=True, name=condition_name) elif collision_condition.find('ByType') is not None: - collision_type = collision_condition.find('ByType').attrib.get('type', None) + collision_type = str(ParameterRef(collision_condition.find('ByType').attrib.get('type', None))) triggered_type = OpenScenarioParser.actor_types[collision_type] @@ -879,7 +913,7 @@ def convert_condition_to_atomic(condition, actor_list): elif entity_condition.find('OffroadCondition') is not None: off_condition = entity_condition.find('OffroadCondition') - condition_duration = ParameterRef(off_condition.attrib.get('duration')) + condition_duration = float(ParameterRef(off_condition.attrib.get('duration'))) atomic_cls = py_trees.meta.inverter(OffRoadTest) atomic = atomic_cls(trigger_actor, condition_duration, terminate_on_failure=True, name=condition_name) @@ -888,19 +922,22 @@ def convert_condition_to_atomic(condition, actor_list): condition_value = ParameterRef(headtime_condition.attrib.get('value')) - condition_rule = headtime_condition.attrib.get('rule') + condition_rule = str(ParameterRef(headtime_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] - condition_freespace = strtobool(headtime_condition.attrib.get('freespace', False)) - condition_along_route = strtobool(headtime_condition.attrib.get('alongRoute', False)) + condition_freespace = strtobool( + str(ParameterRef(headtime_condition.attrib.get('freespace', False)))) + condition_along_route = strtobool( + str(ParameterRef(headtime_condition.attrib.get('alongRoute', False)))) for actor in actor_list: - if headtime_condition.attrib.get('entityRef', None) == actor.attributes['role_name']: + if (str(ParameterRef(headtime_condition.attrib.get('entityRef', None))) + == actor.attributes['role_name']): triggered_actor = actor break if triggered_actor is None: raise AttributeError("Cannot find actor '{}' for condition".format( - headtime_condition.attrib.get('entityRef', None))) + str(ParameterRef(headtime_condition.attrib.get('entityRef', None))))) atomic = InTimeToArrivalToVehicle( trigger_actor, triggered_actor, condition_value, condition_freespace, @@ -910,15 +947,15 @@ def convert_condition_to_atomic(condition, actor_list): elif entity_condition.find('TimeToCollisionCondition') is not None: ttc_condition = entity_condition.find('TimeToCollisionCondition') - condition_rule = ttc_condition.attrib.get('rule') + condition_rule = str(ParameterRef(ttc_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] condition_value = ParameterRef(ttc_condition.attrib.get('value')) condition_target = ttc_condition.find('TimeToCollisionConditionTarget') entity_ref_ = condition_target.find('EntityRef') - condition_freespace = strtobool(ttc_condition.attrib.get('freespace', False)) - condition_along_route = strtobool(ttc_condition.attrib.get('alongRoute', False)) + condition_freespace = strtobool(str(ParameterRef(ttc_condition.attrib.get('freespace', False)))) + condition_along_route = strtobool(str(ParameterRef(ttc_condition.attrib.get('alongRoute', False)))) if condition_target.find('Position') is not None: position = condition_target.find('Position') @@ -926,12 +963,13 @@ def convert_condition_to_atomic(condition, actor_list): trigger_actor, position, condition_value, condition_along_route, condition_operator) else: for actor in actor_list: - if entity_ref_.attrib.get('entityRef', None) == actor.attributes['role_name']: + if (str(ParameterRef(entity_ref_.attrib.get('entityRef', None))) + == actor.attributes['role_name']): triggered_actor = actor break if triggered_actor is None: raise AttributeError("Cannot find actor '{}' for condition".format( - entity_ref_.attrib.get('entityRef', None))) + str(ParameterRef(entity_ref_.attrib.get('entityRef', None))))) atomic = InTimeToArrivalToVehicle( trigger_actor, triggered_actor, condition_value, condition_freespace, @@ -939,18 +977,18 @@ def convert_condition_to_atomic(condition, actor_list): elif entity_condition.find('AccelerationCondition') is not None: accel_condition = entity_condition.find('AccelerationCondition') condition_value = ParameterRef(accel_condition.attrib.get('value')) - condition_rule = accel_condition.attrib.get('rule') + condition_rule = str(ParameterRef(accel_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] atomic = TriggerAcceleration( trigger_actor, condition_value, condition_operator, condition_name) elif entity_condition.find('StandStillCondition') is not None: ss_condition = entity_condition.find('StandStillCondition') - duration = ParameterRef(ss_condition.attrib.get('duration')) + duration = float(ParameterRef(ss_condition.attrib.get('duration'))) atomic = StandStill(trigger_actor, condition_name, duration) elif entity_condition.find('SpeedCondition') is not None: spd_condition = entity_condition.find('SpeedCondition') condition_value = ParameterRef(spd_condition.attrib.get('value')) - condition_rule = spd_condition.attrib.get('rule') + condition_rule = str(ParameterRef(spd_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] atomic = TriggerVelocity( @@ -958,43 +996,45 @@ def convert_condition_to_atomic(condition, actor_list): elif entity_condition.find('RelativeSpeedCondition') is not None: relspd_condition = entity_condition.find('RelativeSpeedCondition') condition_value = ParameterRef(relspd_condition.attrib.get('value')) - condition_rule = relspd_condition.attrib.get('rule') + condition_rule = str(ParameterRef(relspd_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] for actor in actor_list: - if relspd_condition.attrib.get('entityRef', None) == actor.attributes['role_name']: + if (str(ParameterRef(relspd_condition.attrib.get('entityRef', None))) + == actor.attributes['role_name']): triggered_actor = actor break if triggered_actor is None: raise AttributeError("Cannot find actor '{}' for condition".format( - relspd_condition.attrib.get('entityRef', None))) + str(ParameterRef(relspd_condition.attrib.get('entityRef', None))))) atomic = RelativeVelocityToOtherActor( trigger_actor, triggered_actor, condition_value, condition_operator, condition_name) elif entity_condition.find('TraveledDistanceCondition') is not None: distance_condition = entity_condition.find('TraveledDistanceCondition') - distance_value = ParameterRef(distance_condition.attrib.get('value')) + distance_value = float(ParameterRef(distance_condition.attrib.get('value'))) atomic = DriveDistance(trigger_actor, distance_value, name=condition_name) elif entity_condition.find('ReachPositionCondition') is not None: rp_condition = entity_condition.find('ReachPositionCondition') - distance_value = ParameterRef(rp_condition.attrib.get('tolerance')) + distance_value = float(ParameterRef(rp_condition.attrib.get('tolerance'))) position = rp_condition.find('Position') atomic = InTriggerDistanceToOSCPosition( trigger_actor, position, distance_value, name=condition_name) elif entity_condition.find('DistanceCondition') is not None: distance_condition = entity_condition.find('DistanceCondition') - distance_value = ParameterRef(distance_condition.attrib.get('value')) + distance_value = float(ParameterRef(distance_condition.attrib.get('value'))) - distance_rule = distance_condition.attrib.get('rule') + distance_rule = str(ParameterRef(distance_condition.attrib.get('rule'))) distance_operator = OpenScenarioParser.operators[distance_rule] - distance_freespace = strtobool(distance_condition.attrib.get('freespace', False)) + distance_freespace = strtobool(str(ParameterRef(distance_condition.attrib.get('freespace', False)))) if distance_freespace: raise NotImplementedError( "DistanceCondition: freespace attribute is currently not implemented") - distance_along_route = strtobool(distance_condition.attrib.get('alongRoute', False)) + distance_along_route = strtobool( + str(ParameterRef(distance_condition.attrib.get('alongRoute', False)))) if distance_condition.find('Position') is not None: position = distance_condition.find('Position') @@ -1004,20 +1044,22 @@ def convert_condition_to_atomic(condition, actor_list): elif entity_condition.find('RelativeDistanceCondition') is not None: distance_condition = entity_condition.find('RelativeDistanceCondition') - distance_value = ParameterRef(distance_condition.attrib.get('value')) + distance_value = float(ParameterRef(distance_condition.attrib.get('value'))) - distance_freespace = distance_condition.attrib.get('freespace', "false") == "true" - rel_dis_type = distance_condition.attrib.get('relativeDistanceType') + distance_freespace = str(ParameterRef( + distance_condition.attrib.get('freespace', "false"))) == "true" + rel_dis_type = str(ParameterRef(distance_condition.attrib.get('relativeDistanceType'))) for actor in actor_list: - if distance_condition.attrib.get('entityRef', None) == actor.attributes['role_name']: + if (str(ParameterRef(distance_condition.attrib.get('entityRef', None))) + == actor.attributes['role_name']): triggered_actor = actor break if triggered_actor is None: raise AttributeError("Cannot find actor '{}' for condition".format( - distance_condition.attrib.get('entityRef', None))) + str(ParameterRef(distance_condition.attrib.get('entityRef', None))))) - condition_rule = distance_condition.attrib.get('rule') + condition_rule = str(ParameterRef(distance_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] atomic = InTriggerDistanceToVehicle(triggered_actor, trigger_actor, distance_value, condition_operator, distance_type=rel_dis_type, @@ -1026,9 +1068,9 @@ def convert_condition_to_atomic(condition, actor_list): value_condition = condition.find('ByValueCondition') if value_condition.find('ParameterCondition') is not None: parameter_condition = value_condition.find('ParameterCondition') - arg_name = parameter_condition.attrib.get('parameterRef') + arg_name = str(ParameterRef(parameter_condition.attrib.get('parameterRef'))) value = ParameterRef(parameter_condition.attrib.get('value')) - rule = parameter_condition.attrib.get('rule') + rule = str(ParameterRef(parameter_condition.attrib.get('rule'))) if condition_name.startswith('criteria_'): if str(value) != '': arg_value = float(value) @@ -1052,19 +1094,19 @@ def convert_condition_to_atomic(condition, actor_list): elif value_condition.find('SimulationTimeCondition') is not None: simtime_condition = value_condition.find('SimulationTimeCondition') value = ParameterRef(simtime_condition.attrib.get('value')) - rule = OpenScenarioParser.operators[simtime_condition.attrib.get('rule')] + rule = OpenScenarioParser.operators[str(ParameterRef(simtime_condition.attrib.get('rule')))] atomic = SimulationTimeCondition(value, comparison_operator=rule) elif value_condition.find('TimeOfDayCondition') is not None: tod_condition = value_condition.find('TimeOfDayCondition') - condition_date = tod_condition.attrib.get('dateTime') - condition_rule = tod_condition.attrib.get('rule') + condition_date = str(ParameterRef(tod_condition.attrib.get('dateTime'))) + condition_rule = str(ParameterRef(tod_condition.attrib.get('rule'))) condition_operator = OpenScenarioParser.operators[condition_rule] atomic = TimeOfDayComparison(condition_date, condition_operator, condition_name) elif value_condition.find('StoryboardElementStateCondition') is not None: state_condition = value_condition.find('StoryboardElementStateCondition') - element_name = state_condition.attrib.get('storyboardElementRef') - element_type = state_condition.attrib.get('storyboardElementType') - state = state_condition.attrib.get('state') + element_name = str(ParameterRef(state_condition.attrib.get('storyboardElementRef'))) + element_type = str(ParameterRef(state_condition.attrib.get('storyboardElementType'))) + state = str(ParameterRef(state_condition.attrib.get('state'))) if state == "startTransition": atomic = OSCStartEndCondition(element_type, element_name, rule="START", name=state + "Condition") elif state in ["stopTransition", "endTransition", "completeState"]: @@ -1077,10 +1119,10 @@ def convert_condition_to_atomic(condition, actor_list): elif value_condition.find('TrafficSignalCondition') is not None: tl_condition = value_condition.find('TrafficSignalCondition') - name_condition = tl_condition.attrib.get('name') + name_condition = str(ParameterRef(tl_condition.attrib.get('name'))) traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition) - tl_state = tl_condition.attrib.get('state').upper() + tl_state = str(ParameterRef(tl_condition.attrib.get('state'))).upper() if tl_state not in OpenScenarioParser.tl_states: raise KeyError("CARLA only supports Green, Red, Yellow or Off") state_condition = OpenScenarioParser.tl_states[tl_state] @@ -1108,10 +1150,9 @@ def convert_condition_to_atomic(condition, actor_list): def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): """ Convert an OpenSCENARIO maneuver action into a Behavior atomic - Note not all OpenSCENARIO actions are currently supported """ - maneuver_name = action.attrib.get('name', 'unknown') + maneuver_name = str(ParameterRef(action.attrib.get('name', 'unknown'))) if action.find('GlobalAction') is not None: global_action = action.find('GlobalAction') @@ -1120,23 +1161,19 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): if infrastructure_action.find('TrafficSignalStateAction') is not None: traffic_light_action = infrastructure_action.find('TrafficSignalStateAction') - name_condition = traffic_light_action.attrib.get('name') - traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition) - - tl_state = traffic_light_action.attrib.get('state').upper() - if tl_state not in OpenScenarioParser.tl_states: - raise KeyError("CARLA only supports Green, Red, Yellow or Off") - traffic_light_state = OpenScenarioParser.tl_states[tl_state] + name_condition_ref = ParameterRef(traffic_light_action.attrib.get('name')) + tl_state_ref = ParameterRef(traffic_light_action.attrib.get('state')) atomic = TrafficLightStateSetter( - traffic_light, traffic_light_state, name=maneuver_name + "_" + str(traffic_light.id)) + name_condition_ref, tl_state_ref, name=maneuver_name + "_" + str( + traffic_light_action.attrib.get('name'))) else: raise NotImplementedError("TrafficLights can only be influenced via TrafficSignalStateAction") + elif global_action.find('EnvironmentAction') is not None: - weather_behavior = ChangeWeather( - OpenScenarioParser.get_weather_from_env_action(global_action, catalogs)) - friction_behavior = ChangeRoadFriction( - OpenScenarioParser.get_friction_from_env_action(global_action, catalogs)) + + weather_behavior = ChangeWeather(global_action, catalogs) + friction_behavior = ChangeRoadFriction(global_action, catalogs) env_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=maneuver_name) @@ -1165,7 +1202,16 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): user_defined_action = action.find('UserDefinedAction') if user_defined_action.find('CustomCommandAction') is not None: command = user_defined_action.find('CustomCommandAction').attrib.get('type') - atomic = RunScript(command, base_path=OpenScenarioParser.osc_filepath, name=maneuver_name) + + # treat each part of the command string, separated by whitespaces, + # as possible OSC parameter variables + script_components = command.split(' ') + script_component_refs = [] + for component in script_components: + script_component_refs.append(ParameterRef(component)) + + atomic = RunScript(script_component_refs, base_path=OpenScenarioParser.osc_filepath, name=maneuver_name) + elif action.find('PrivateAction') is not None: private_action = action.find('PrivateAction') @@ -1175,57 +1221,47 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): if private_action.find('SpeedAction') is not None: long_maneuver = private_action.find('SpeedAction') - # duration and distance - distance = float('inf') - duration = float('inf') - dimension = long_maneuver.find("SpeedActionDynamics").attrib.get('dynamicsDimension') - if dimension == "distance": - distance = ParameterRef(long_maneuver.find("SpeedActionDynamics").attrib.get( - 'value', float("inf"))) - else: - duration = ParameterRef(long_maneuver.find("SpeedActionDynamics").attrib.get( - 'value', float("inf"))) + dimension_ref = ParameterRef(long_maneuver.find( + "SpeedActionDynamics").attrib.get('dynamicsDimension')) + + dimension_value_ref = ParameterRef(long_maneuver.find("SpeedActionDynamics").attrib.get( + 'value', float("inf"))) # absolute velocity with given target speed if long_maneuver.find("SpeedActionTarget").find("AbsoluteTargetSpeed") is not None: target_speed = ParameterRef(long_maneuver.find("SpeedActionTarget").find( "AbsoluteTargetSpeed").attrib.get('value', 0)) + atomic = ChangeActorTargetSpeed( - actor, float(target_speed), distance=distance, duration=duration, name=maneuver_name) + actor, target_speed, dimension=dimension_ref, + dimension_value=dimension_value_ref, name=maneuver_name) # relative velocity to given actor if long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed") is not None: relative_speed = long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed") - obj = relative_speed.attrib.get('entityRef') - value = ParameterRef(relative_speed.attrib.get('value', 0)) - value_type = relative_speed.attrib.get('speedTargetValueType') - continuous = bool(strtobool(relative_speed.attrib.get('continuous'))) - for traffic_actor in actor_list: - if (traffic_actor is not None and 'role_name' in traffic_actor.attributes and - traffic_actor.attributes['role_name'] == obj): - obj_actor = traffic_actor + obj_ref = ParameterRef(relative_speed.attrib.get('entityRef')) + value_ref = ParameterRef(relative_speed.attrib.get('value', 0)) + value_type_ref = ParameterRef(relative_speed.attrib.get('speedTargetValueType')) + continuous_ref = ParameterRef(relative_speed.attrib.get('continuous')) atomic = ChangeActorTargetSpeed(actor, target_speed=0.0, - relative_actor=obj_actor, - value=value, - value_type=value_type, - continuous=continuous, - distance=distance, - duration=duration, + relative_actor_name=obj_ref, + value=value_ref, + value_type=value_type_ref, + continuous=continuous_ref, + dimension=dimension_ref, + dimension_value=dimension_value_ref, + actor_list=actor_list, name=maneuver_name) elif private_action.find('LongitudinalDistanceAction') is not None: long_dist_action = private_action.find("LongitudinalDistanceAction") - obj = long_dist_action.attrib.get('entityRef') - for traffic_actor in actor_list: - if (traffic_actor is not None and 'role_name' in traffic_actor.attributes and - traffic_actor.attributes['role_name'] == obj): - obj_actor = traffic_actor + obj_ref = ParameterRef(long_dist_action.attrib.get('entityRef')) if "distance" in long_dist_action.attrib and "timeGap" not in long_dist_action.attrib: - gap_type, gap = 'distance', ParameterRef(long_dist_action.attrib.get('distance')) + gap_type, gap_ref = 'distance', ParameterRef(long_dist_action.attrib.get('distance')) elif "timeGap" in long_dist_action.attrib and "distance" not in long_dist_action.attrib: raise NotImplementedError("LongitudinalDistanceAction: timeGap is not implemented") else: @@ -1233,67 +1269,74 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): "Please specify any one attribute of [distance, timeGap]") constraints = long_dist_action.find('DynamicConstraints') - max_speed = constraints.attrib.get('maxSpeed', None) if constraints is not None else None - continues = bool(strtobool(long_dist_action.attrib.get('continuous'))) - freespace = bool(strtobool(long_dist_action.attrib.get('freespace'))) - atomic = KeepLongitudinalGap(actor, reference_actor=obj_actor, gap=gap, gap_type=gap_type, - max_speed=max_speed, continues=continues, freespace=freespace, + + max_speed_ref = ParameterRef(constraints.attrib.get('maxSpeed', None) + ) if constraints is not None else None + continues_ref = ParameterRef(long_dist_action.attrib.get('continuous')) + freespace_ref = ParameterRef(long_dist_action.attrib.get('freespace')) + + atomic = KeepLongitudinalGap(actor, + reference_actor=obj_ref, + gap=gap_ref, + gap_type=gap_type, + max_speed=max_speed_ref, + continues=continues_ref, + freespace=freespace_ref, + actor_list=actor_list, name=maneuver_name) else: raise AttributeError("Unknown longitudinal action") elif private_action.find('LateralAction') is not None: + private_action = private_action.find('LateralAction') if private_action.find('LaneChangeAction') is not None: - # Note: LaneChangeActions are currently only supported for RelativeTargetLane lat_maneuver = private_action.find('LaneChangeAction') - target_lane_rel = ParameterRef(lat_maneuver.find("LaneChangeTarget").find( - "RelativeTargetLane").attrib.get('value', 0)) - direction = "left" if target_lane_rel > 0 else "right" - lane_changes = abs(target_lane_rel) - # duration and distance - distance = float('inf') - duration = float('inf') - dimension = lat_maneuver.find("LaneChangeActionDynamics").attrib.get('dynamicsDimension') - if dimension == "distance": - distance = ParameterRef( - lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf"))) - else: - duration = ParameterRef( - lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf"))) - atomic = ChangeActorLateralMotion(actor, direction=direction, - distance_lane_change=distance, - distance_other_lane=10, - lane_changes=lane_changes, - name=maneuver_name) + + if lat_maneuver.find("LaneChangeTarget").find("RelativeTargetLane") is not None: + + target_lane_rel_ref = ParameterRef(lat_maneuver.find("LaneChangeTarget").find( + "RelativeTargetLane").attrib.get('value', 0)) + dimension_ref = ParameterRef(lat_maneuver.find( + "LaneChangeActionDynamics").attrib.get('dynamicsDimension')) + dimension_value_ref = ParameterRef(lat_maneuver.find( + "LaneChangeActionDynamics").attrib.get('value', float("inf"))) + + # Note: dimension == duration is currently not implemented + atomic = ChangeActorLateralMotion(actor, + target_lane_rel=target_lane_rel_ref, + dimension=dimension_ref, + dimension_value=dimension_value_ref, + distance_other_lane=10, + name=maneuver_name) + + elif lat_maneuver.find("LaneChangeTarget").find("AbsoluteTargetLane") is not None: + raise NotImplementedError("LaneChangeTarget: AbsoluteTargetLane is not implemented") + elif private_action.find('LaneOffsetAction') is not None: + lat_maneuver = private_action.find('LaneOffsetAction') - continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "true"))) + continuous_ref = ParameterRef(lat_maneuver.attrib.get('continuous', "true")) # Parsing of the different Dynamic shapes is missing lane_target_offset = lat_maneuver.find('LaneOffsetTarget') if lane_target_offset.find('AbsoluteTargetLaneOffset') is not None: - absolute_offset = ParameterRef( + absolute_offset_ref = ParameterRef( lane_target_offset.find('AbsoluteTargetLaneOffset').attrib.get('value', 0)) atomic = ChangeActorLaneOffset( - actor, absolute_offset, continuous=continuous, name=maneuver_name) + actor, absolute_offset_ref, continuous=continuous_ref, name=maneuver_name) elif lane_target_offset.find('RelativeTargetLaneOffset') is not None: relative_target_offset = lane_target_offset.find('RelativeTargetLaneOffset') - relative_offset = ParameterRef(relative_target_offset.attrib.get('value', 0)) - - relative_actor = None - relative_actor_name = relative_target_offset.attrib.get('entityRef', None) - for _actor in actor_list: - if _actor is not None and 'role_name' in _actor.attributes: - if relative_actor_name == _actor.attributes['role_name']: - relative_actor = _actor - break - if relative_actor is None: - raise AttributeError("Cannot find actor '{}' for condition".format(relative_actor_name)) - atomic = ChangeActorLaneOffset(actor, relative_offset, relative_actor, - continuous=continuous, name=maneuver_name) + relative_offset_ref = ParameterRef(relative_target_offset.attrib.get('value', 0)) + relative_actor_name_ref = ParameterRef(relative_target_offset.attrib.get('entityRef', None)) + atomic = ChangeActorLaneOffset(actor, + relative_offset_ref, + relative_actor_name_ref, + continuous=continuous_ref, + actor_list=actor_list, + name=maneuver_name) else: raise AttributeError("Unknown target offset") else: @@ -1302,46 +1345,41 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): raise NotImplementedError("Visibility actions are not yet supported") elif private_action.find('SynchronizeAction') is not None: sync_action = private_action.find('SynchronizeAction') + master_actor_name_ref = ParameterRef(sync_action.attrib.get('masterEntityRef', None)) - master_actor = None - for actor_ins in actor_list: - if actor_ins is not None and 'role_name' in actor_ins.attributes: - if sync_action.attrib.get('masterEntityRef', None) == actor_ins.attributes['role_name']: - master_actor = actor_ins - break - - if master_actor is None: - raise AttributeError("Cannot find actor '{}' for condition".format( - sync_action.attrib.get('masterEntityRef', None))) - - master_position = OpenScenarioParser.convert_position_to_transform( - sync_action.find('TargetPositionMaster')) - position = OpenScenarioParser.convert_position_to_transform(sync_action.find('TargetPosition')) + master_position = sync_action.find('TargetPositionMaster') + position = sync_action.find('TargetPosition') if sync_action.find("FinalSpeed").find("AbsoluteSpeed") is not None: - final_speed = ParameterRef(sync_action.find("FinalSpeed").find( + final_speed_ref = ParameterRef(sync_action.find("FinalSpeed").find( "AbsoluteSpeed").attrib.get('value', 0)) + atomic = SyncArrivalOSC( - actor, master_actor, position, master_position, final_speed, name=maneuver_name) + actor, master_actor_name_ref, position, master_position, final_speed_ref, + actor_list=actor_list, name=maneuver_name) # relative velocity to given actor elif sync_action.find("FinalSpeed").find("RelativeSpeedToMaster") is not None: relative_speed = sync_action.find("FinalSpeed").find("RelativeSpeedToMaster") - final_speed = ParameterRef(relative_speed.attrib.get('value', 0)) - relative_type = relative_speed.attrib.get('speedTargetValueType') + final_speed_ref = ParameterRef(relative_speed.attrib.get('value', 0)) + relative_type_ref = ParameterRef(relative_speed.attrib.get('speedTargetValueType')) + atomic = SyncArrivalOSC( - actor, master_actor, position, master_position, final_speed, - relative_to_master=True, relative_type=relative_type, name=maneuver_name) + actor, master_actor_name_ref, position, master_position, final_speed_ref, + relative_to_master=True, relative_type=relative_type_ref, + actor_list=actor_list, name=maneuver_name) else: raise AttributeError("Unknown speed action") + elif private_action.find('ActivateControllerAction') is not None: + # No action implemented for the 'lateral' attribute private_action = private_action.find('ActivateControllerAction') - activate = strtobool(private_action.attrib.get('longitudinal')) + activate = ParameterRef(private_action.attrib.get('longitudinal')) atomic = ChangeAutoPilot(actor, activate, name=maneuver_name) elif private_action.find('ControllerAction') is not None: controller_action = private_action.find('ControllerAction') - module, args = OpenScenarioParser.get_controller(controller_action, catalogs) - atomic = ChangeActorControl(actor, control_py_module=module, args=args, + atomic = ChangeActorControl(actor, osc_controller_action=controller_action, + catalogs=catalogs, scenario_file_path=OpenScenarioParser.osc_filepath) elif private_action.find('TeleportAction') is not None: teleport_action = private_action.find('TeleportAction') @@ -1352,8 +1390,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): if private_action.find('AssignRouteAction') is not None: # @TODO: How to handle relative positions here? This might chance at runtime?! route_action = private_action.find('AssignRouteAction') - waypoints = OpenScenarioParser.get_route(route_action, catalogs) - atomic = ChangeActorWaypoints(actor, waypoints=waypoints, name=maneuver_name) + atomic = ChangeActorWaypoints(actor, osc_route_action=route_action, catalogs=catalogs, + name=maneuver_name) elif private_action.find('FollowTrajectoryAction') is not None: raise NotImplementedError("Private FollowTrajectory actions are not yet supported") elif private_action.find('AcquirePositionAction') is not None: