From ed62a4a837b7805e55baef37e89fd0b8a30c1255 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 9 Nov 2021 15:03:51 +0200 Subject: [PATCH 01/19] Added __hash__ to ParameterRef to enable usage as dictionary key. Extended ParameterRef comparison operations from float to int and str types Added ParameterRef's to various places in openscenario_configuration.py and openscenario_parser.py --- .../openscenario_configuration.py | 24 +++--- srunner/tools/openscenario_parser.py | 84 ++++++++++++------- 2 files changed, 68 insertions(+), 40 deletions(-) diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index ce4d99b09..5454bde51 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -98,7 +98,7 @@ 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 +124,7 @@ 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 +134,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 +158,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 +235,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 +290,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 +312,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 +323,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) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 5dc0f8536..ce149d7fc 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -120,6 +120,20 @@ 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__() + 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 @@ -156,6 +170,10 @@ 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 +199,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,7 +389,7 @@ def get_catalog_entry(catalogs, catalog_reference): returns: Catalog entry (XML ElementTree) """ - entry = catalogs[catalog_reference.attrib.get("catalogName")][ParameterRef(catalog_reference.attrib.get("entryName"))] + entry = catalogs[ParameterRef(catalog_reference.attrib.get("catalogName"))][ParameterRef(catalog_reference.attrib.get("entryName"))] entry_copy = copy.deepcopy(entry) catalog_copy = copy.deepcopy(catalog_reference) entry = OpenScenarioParser.assign_catalog_parameters(entry_copy, catalog_copy) @@ -439,7 +467,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 @@ -473,11 +501,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 @@ -485,16 +513,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) @@ -526,14 +554,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 @@ -567,7 +595,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") @@ -1110,7 +1138,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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') @@ -1163,7 +1191,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): elif action.find('UserDefinedAction') is not None: user_defined_action = action.find('UserDefinedAction') if user_defined_action.find('CustomCommandAction') is not None: - command = user_defined_action.find('CustomCommandAction').attrib.get('type') + command = str(ParameterRef(user_defined_action.find('CustomCommandAction').attrib.get('type'))) atomic = RunScript(command, base_path=OpenScenarioParser.osc_filepath, name=maneuver_name) elif action.find('PrivateAction') is not None: private_action = action.find('PrivateAction') From 18a3409ec7c836241b4779b93ab94f50c6300a27 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 9 Nov 2021 17:59:38 +0200 Subject: [PATCH 02/19] Moved UserDefinedAction parameter resolving into atomicbehaviours to make ParameterAction work --- .../openscenario_configuration.py | 10 +- .../scenarioatomics/atomic_behaviors.py | 10 +- srunner/tools/openscenario_parser.py | 314 +++++------------- srunner/tools/scenario_param_ref.py | 164 +++++++++ 4 files changed, 264 insertions(+), 234 deletions(-) create mode 100644 srunner/tools/scenario_param_ref.py diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index 5454bde51..efbeecd72 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 . @@ -21,8 +21,8 @@ from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData, ScenarioConfiguration # pylint: enable=line-too-long from srunner.scenariomanager.carla_data_provider import CarlaDataProvider # workaround -from srunner.tools.openscenario_parser import OpenScenarioParser, ParameterRef - +from srunner.tools.openscenario_parser import OpenScenarioParser +from srunner.tools.scenario_param_ref import ParameterRef class OpenScenarioConfiguration(ScenarioConfiguration): @@ -351,7 +351,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 +380,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 378b13af8..cc9f0743f 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -39,6 +39,7 @@ from srunner.scenariomanager.timer import GameTime from srunner.tools.scenario_helper import detect_lane_obstacle from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane +from srunner.tools.scenario_param_ref import ParameterRef import srunner.tools as sr_tools @@ -172,6 +173,13 @@ def update(self): """ path = None script_components = self._script.split(' ') + + interpreted_components = [] + for component in script_components: + interpreted_components.append( str(ParameterRef(component)) ) + script_components = interpreted_components + interpreted_script = ' '.join(script_components) + if len(script_components) > 1: path = script_components[1] @@ -181,7 +189,7 @@ 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 + subprocess.Popen(interpreted_script, shell=True, cwd=self._base_path) # pylint: disable=consider-using-with new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index ce149d7fc..c7d87ccf7 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -72,6 +72,7 @@ from srunner.scenariomanager.timer import TimeOut, SimulationTimeCondition from srunner.tools.py_trees_port import oneshot_behavior from srunner.tools.scenario_helper import get_offset_transform, get_troad_from_transform +from srunner.tools.scenario_param_ref import ParameterRef def oneshot_with_check(variable_name, behaviour, name=None): @@ -87,165 +88,7 @@ def oneshot_with_check(variable_name, behaviour, name=None): return oneshot_behavior(variable_name, behaviour, name) -class ParameterRef: - """ - This class stores osc parameter reference in its original form. - Returns the converted value whenever it is used. - """ - - def __init__(self, reference_text) -> None: - # TODO: (for OSC1.1) add methods(lexer and math_interpreter) to - # recognize and interpret math expression from reference_text - self.reference_text = str(reference_text) - - def is_literal(self) -> bool: - """ - Returns: True when text is a literal/number - """ - return self._is_matching(pattern=r"(-)?\d+(\.\d*)?") - - def is_parameter(self) -> bool: - """ - Returns: True when text is a parameter - """ - return self._is_matching(pattern=r"[$][A-Za-z_][\w]*") - - def _is_matching(self, pattern: str) -> bool: - """ - Returns: True when pattern is matching with text - """ - match = re.search(pattern, self.reference_text) - if match is not None: - matching_string = match.group() - 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__() - 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 - """ - if self.is_literal(): - value = self.reference_text - elif self.is_parameter(): - value = CarlaDataProvider.get_osc_global_param_value(self.reference_text) - if value is None: - raise Exception("Parameter '{}' is not defined".format(self.reference_text[1:])) - else: - value = None - return value - - def __float__(self) -> float: - value = self.get_interpreted_value() - if value is not None: - return float(value) - else: - raise Exception("could not convert '{}' to float".format(self.reference_text)) - - def __int__(self) -> int: - value = self.get_interpreted_value() - if value is not None: - return int(float(value)) - else: - raise Exception("could not convert '{}' to int".format(self.reference_text)) - - def __str__(self) -> str: - value = self.get_interpreted_value() - return str(value) if value is not None else self.reference_text - - 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__() - - def __add__(self, other) -> bool: - return other + self.__float__() - - def __rsub__(self, other) -> bool: - return other - self.__float__() - - def __sub__(self, other) -> bool: - return self.__float__() - other - - def __rmul__(self, other) -> bool: - return other * self.__float__() - - def __mul__(self, other) -> bool: - return other * self.__float__() - - def __truediv__(self, other) -> bool: - return self.__float__() / other - - def __rtruediv__(self, other) -> bool: - return other / self.__float__() - - def __eq__(self, other) -> bool: - value = self._cast_type(other) - return other == value - - def __ne__(self, other) -> bool: - value = self._cast_type(other) - return other != value - - def __ge__(self, other) -> bool: - value = self._cast_type(other) - return value >= other - - def __le__(self, other) -> bool: - value = self._cast_type(other) - return value <= other - - def __gt__(self, other) -> bool: - value = self._cast_type(other) - return value > other - - def __lt__(self, other) -> bool: - value = self._cast_type(other) - return value < other - - def __GE__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value >= other - - def __LE__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value <= other - - def __GT__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value > other - - def __LT__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value < other - - def __iadd__(self, other) -> bool: - return self.__float__() + other - - def __isub__(self, other) -> bool: - return self.__float__() - other - - def __abs__(self): - return abs(self.__float__()) class OpenScenarioParser(object): @@ -389,7 +232,9 @@ def get_catalog_entry(catalogs, catalog_reference): returns: Catalog entry (XML ElementTree) """ - entry = catalogs[ParameterRef(catalog_reference.attrib.get("catalogName"))][ParameterRef(catalog_reference.attrib.get("entryName"))] + 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) @@ -664,7 +509,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)))) @@ -833,23 +678,22 @@ def convert_position_to_transform(position, actor_list=None): else: raise AttributeError("Unknown position") + @staticmethod 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: @@ -860,14 +704,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) @@ -879,20 +724,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] @@ -906,7 +752,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) @@ -915,19 +761,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, @@ -937,15 +786,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') @@ -953,12 +802,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, @@ -966,18 +816,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( @@ -985,43 +835,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') @@ -1031,20 +883,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, @@ -1053,9 +907,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) @@ -1079,19 +933,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"]: @@ -1104,10 +958,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] @@ -1135,7 +989,6 @@ 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 = str(ParameterRef(action.attrib.get('name', 'unknown'))) @@ -1147,10 +1000,10 @@ 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') + name_condition = str(ParameterRef(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() + tl_state = str(ParameterRef(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] @@ -1191,8 +1044,9 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): elif action.find('UserDefinedAction') is not None: user_defined_action = action.find('UserDefinedAction') if user_defined_action.find('CustomCommandAction') is not None: - command = str(ParameterRef(user_defined_action.find('CustomCommandAction').attrib.get('type'))) + command = user_defined_action.find('CustomCommandAction').attrib.get('type') atomic = RunScript(command, base_path=OpenScenarioParser.osc_filepath, name=maneuver_name) + elif action.find('PrivateAction') is not None: private_action = action.find('PrivateAction') @@ -1205,7 +1059,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): # duration and distance distance = float('inf') duration = float('inf') - dimension = long_maneuver.find("SpeedActionDynamics").attrib.get('dynamicsDimension') + dimension = str(ParameterRef(long_maneuver.find( + "SpeedActionDynamics").attrib.get('dynamicsDimension'))) if dimension == "distance": distance = ParameterRef(long_maneuver.find("SpeedActionDynamics").attrib.get( 'value', float("inf"))) @@ -1223,10 +1078,10 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): # 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') + obj = str(ParameterRef(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'))) + value_type = str(ParameterRef(relative_speed.attrib.get('speedTargetValueType'))) + continuous = bool(strtobool(str(ParameterRef(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 @@ -1245,7 +1100,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): elif private_action.find('LongitudinalDistanceAction') is not None: long_dist_action = private_action.find("LongitudinalDistanceAction") - obj = long_dist_action.attrib.get('entityRef') + obj = str(ParameterRef(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): @@ -1260,9 +1115,10 @@ 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'))) + max_speed = float(ParameterRef(constraints.attrib.get('maxSpeed', None)) + ) if constraints is not None else None + continues = bool(strtobool(str(ParameterRef(long_dist_action.attrib.get('continuous'))))) + freespace = bool(strtobool(str(ParameterRef(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, name=maneuver_name) @@ -1280,7 +1136,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): # duration and distance distance = float('inf') duration = float('inf') - dimension = lat_maneuver.find("LaneChangeActionDynamics").attrib.get('dynamicsDimension') + dimension = str(ParameterRef(lat_maneuver.find( + "LaneChangeActionDynamics").attrib.get('dynamicsDimension'))) if dimension == "distance": distance = ParameterRef( lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf"))) @@ -1294,7 +1151,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): name=maneuver_name) elif private_action.find('LaneOffsetAction') is not None: lat_maneuver = private_action.find('LaneOffsetAction') - continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "true"))) + continuous = bool(strtobool(str(ParameterRef(lat_maneuver.attrib.get('continuous', "true"))))) # Parsing of the different Dynamic shapes is missing lane_target_offset = lat_maneuver.find('LaneOffsetTarget') @@ -1309,7 +1166,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): relative_offset = ParameterRef(relative_target_offset.attrib.get('value', 0)) relative_actor = None - relative_actor_name = relative_target_offset.attrib.get('entityRef', None) + relative_actor_name = str(ParameterRef(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']: @@ -1333,13 +1190,14 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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']: + if (str(ParameterRef(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))) + str(ParameterRef(sync_action.attrib.get('masterEntityRef', None))))) master_position = OpenScenarioParser.convert_position_to_transform( sync_action.find('TargetPositionMaster')) @@ -1355,7 +1213,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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') + relative_type = float(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) @@ -1363,7 +1221,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): raise AttributeError("Unknown speed action") elif private_action.find('ActivateControllerAction') is not None: private_action = private_action.find('ActivateControllerAction') - activate = strtobool(private_action.attrib.get('longitudinal')) + activate = strtobool(str(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') diff --git a/srunner/tools/scenario_param_ref.py b/srunner/tools/scenario_param_ref.py new file mode 100644 index 000000000..ada4c412c --- /dev/null +++ b/srunner/tools/scenario_param_ref.py @@ -0,0 +1,164 @@ +import re +import math + +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + +class ParameterRef: + """ + This class stores osc parameter reference in its original form. + Returns the converted value whenever it is used. + """ + + def __init__(self, reference_text) -> None: + # TODO: (for OSC1.1) add methods(lexer and math_interpreter) to + # recognize and interpret math expression from reference_text + self.reference_text = str(reference_text) + + def is_literal(self) -> bool: + """ + Returns: True when text is a literal/number + """ + return self._is_matching(pattern=r"(-)?\d+(\.\d*)?") + + def is_parameter(self) -> bool: + """ + Returns: True when text is a parameter + """ + return self._is_matching(pattern=r"[$][A-Za-z_][\w]*") + + def _is_matching(self, pattern: str) -> bool: + """ + Returns: True when pattern is matching with text + """ + match = re.search(pattern, self.reference_text) + if match is not None: + matching_string = match.group() + 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__() + 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 + """ + if self.is_literal(): + value = self.reference_text + elif self.is_parameter(): + value = CarlaDataProvider.get_osc_global_param_value(self.reference_text) + if value is None: + raise Exception("Parameter '{}' is not defined".format(self.reference_text[1:])) + else: + value = None + return value + + def __float__(self) -> float: + value = self.get_interpreted_value() + if value is not None: + return float(value) + else: + raise Exception("could not convert '{}' to float".format(self.reference_text)) + + def __int__(self) -> int: + value = self.get_interpreted_value() + if value is not None: + return int(float(value)) + else: + raise Exception("could not convert '{}' to int".format(self.reference_text)) + + def __str__(self) -> str: + value = self.get_interpreted_value() + return str(value) if value is not None else self.reference_text + + 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__() + + def __add__(self, other) -> bool: + return other + self.__float__() + + def __rsub__(self, other) -> bool: + return other - self.__float__() + + def __sub__(self, other) -> bool: + return self.__float__() - other + + def __rmul__(self, other) -> bool: + return other * self.__float__() + + def __mul__(self, other) -> bool: + return other * self.__float__() + + def __truediv__(self, other) -> bool: + return self.__float__() / other + + def __rtruediv__(self, other) -> bool: + return other / self.__float__() + + def __eq__(self, other) -> bool: + value = self._cast_type(other) + return other == value + + def __ne__(self, other) -> bool: + value = self._cast_type(other) + return other != value + + def __ge__(self, other) -> bool: + value = self._cast_type(other) + return value >= other + + def __le__(self, other) -> bool: + value = self._cast_type(other) + return value <= other + + def __gt__(self, other) -> bool: + value = self._cast_type(other) + return value > other + + def __lt__(self, other) -> bool: + value = self._cast_type(other) + return value < other + + def __GE__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value >= other + + def __LE__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value <= other + + def __GT__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value > other + + def __LT__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value < other + + def __iadd__(self, other) -> bool: + return self.__float__() + other + + def __isub__(self, other) -> bool: + return self.__float__() - other + + def __abs__(self): + return abs(self.__float__()) \ No newline at end of file From 5fae911881f8ff4b945aff05039a735bfea98829 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 21 Dec 2021 13:47:40 +0200 Subject: [PATCH 03/19] Wait for subrprocess completion in RunScript --- srunner/scenariomanager/scenarioatomics/atomic_behaviors.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index cc9f0743f..51f93dfed 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -189,7 +189,8 @@ def update(self): new_status = py_trees.common.Status.FAILURE print("Script file does not exists {}".format(path)) else: - subprocess.Popen(interpreted_script, shell=True, cwd=self._base_path) # pylint: disable=consider-using-with + process = subprocess.Popen(interpreted_script, shell=True, cwd=self._base_path) # pylint: disable=consider-using-with + process.wait(timeout=10) new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) From 9287f955bc2bf03d029d5673b099637b31006b29 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 21 Dec 2021 14:08:12 +0200 Subject: [PATCH 04/19] Updated runscript fix --- .../scenariomanager/scenarioatomics/atomic_behaviors.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 51f93dfed..deaa728e5 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -190,7 +190,13 @@ def update(self): print("Script file does not exists {}".format(path)) else: process = subprocess.Popen(interpreted_script, shell=True, cwd=self._base_path) # pylint: disable=consider-using-with - process.wait(timeout=10) + + 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)) From 0813aa65992e834ad41413e91de8022821ecef24 Mon Sep 17 00:00:00 2001 From: lauri Date: Thu, 23 Dec 2021 13:32:13 +0200 Subject: [PATCH 05/19] Moved ParameterRef resolution into runtime for AbsoluteTargetSpeed and RelativeTargetSpeed --- .../openscenario_configuration.py | 4 +- .../scenarioatomics/atomic_behaviors.py | 138 ++++++++++++++---- srunner/tools/openscenario_parser.py | 50 +++---- srunner/tools/scenario_param_ref.py | 20 ++- 4 files changed, 149 insertions(+), 63 deletions(-) diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index efbeecd72..9995a2b4e 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -24,6 +24,7 @@ from srunner.tools.openscenario_parser import OpenScenarioParser from srunner.tools.scenario_param_ref import ParameterRef + class OpenScenarioConfiguration(ScenarioConfiguration): """ @@ -124,7 +125,8 @@ def _load_catalogs(self): if catalog is None: continue - catalog_path = str(ParameterRef(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 diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 9dca4bb01..a8f21e246 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -41,7 +41,6 @@ from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane from srunner.tools.scenario_param_ref import ParameterRef - import srunner.tools as sr_tools EPSILON = 0.001 @@ -88,6 +87,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): """ @@ -176,7 +185,7 @@ def update(self): interpreted_components = [] for component in script_components: - interpreted_components.append( str(ParameterRef(component)) ) + interpreted_components.append(str(ParameterRef(component))) script_components = interpreted_components interpreted_script = ' '.join(script_components) @@ -189,7 +198,8 @@ def update(self): new_status = py_trees.common.Status.FAILURE print("Script file does not exists {}".format(path)) else: - process = subprocess.Popen(interpreted_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: @@ -430,50 +440,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 """ @@ -484,13 +530,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): """ @@ -500,6 +550,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: @@ -532,6 +583,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. diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index c7d87ccf7..04b694edc 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -12,7 +12,6 @@ from __future__ import print_function from distutils.util import strtobool -import re import copy import datetime import math @@ -88,9 +87,6 @@ def oneshot_with_check(variable_name, behaviour, name=None): return oneshot_behavior(variable_name, behaviour, name) - - - class OpenScenarioParser(object): """ @@ -678,7 +674,6 @@ def convert_position_to_transform(position, actor_list=None): else: raise AttributeError("Unknown position") - @staticmethod def convert_condition_to_atomic(condition, actor_list): """ @@ -1056,46 +1051,39 @@ 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 = str(ParameterRef(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 = str(ParameterRef(relative_speed.attrib.get('entityRef'))) - value = ParameterRef(relative_speed.attrib.get('value', 0)) - value_type = str(ParameterRef(relative_speed.attrib.get('speedTargetValueType'))) - continuous = bool(strtobool(str(ParameterRef(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: diff --git a/srunner/tools/scenario_param_ref.py b/srunner/tools/scenario_param_ref.py index ada4c412c..84c348fe8 100644 --- a/srunner/tools/scenario_param_ref.py +++ b/srunner/tools/scenario_param_ref.py @@ -1,8 +1,16 @@ +#!/usr/bin/env python + +""" +This module provides a class for retrieving parameter values of +scenario configuration files based on OpenSCENARIO +""" + import re -import math +from distutils.util import strtobool from srunner.scenariomanager.carla_data_provider import CarlaDataProvider + class ParameterRef: """ This class stores osc parameter reference in its original form. @@ -44,6 +52,8 @@ def _cast_type(self, other): 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: @@ -82,6 +92,12 @@ 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 @@ -161,4 +177,4 @@ def __isub__(self, other) -> bool: return self.__float__() - other def __abs__(self): - return abs(self.__float__()) \ No newline at end of file + return abs(self.__float__()) From 8039deb6d0ede39a67bb33bda525f6f62cd22853 Mon Sep 17 00:00:00 2001 From: lauri Date: Mon, 3 Jan 2022 18:04:28 +0200 Subject: [PATCH 06/19] OSC EnvironmentAction: Moved OSC parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 83 ++++++++++++++++--- srunner/scenarios/open_scenario.py | 25 +++++- srunner/tools/openscenario_parser.py | 82 ++++++++++-------- 3 files changed, 138 insertions(+), 52 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index a8f21e246..faa6746d9 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -21,6 +21,7 @@ import os import time import subprocess +import datetime import numpy as np from numpy import random @@ -37,10 +38,12 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.actorcontrols.actor_control import ActorControl from srunner.scenariomanager.timer import GameTime +from srunner.scenariomanager.weather_sim import Weather from srunner.tools.scenario_helper import detect_lane_obstacle from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane from srunner.tools.scenario_param_ref import ParameterRef + import srunner.tools as sr_tools EPSILON = 0.001 @@ -258,20 +261,51 @@ class ChangeWeather(AtomicBehavior): The behavior immediately terminates with SUCCESS after updating the blackboard. Args: - weather (srunner.scenariomanager.weather_sim.Weather): New weather settings. + sun_azimuth_angle (float | ParameterRef(float)): Azimuth of the sun in radians. + sun_altitude_angle (float | ParameterRef(float)): Elevation of the sun in radians. + sun_intensity (float | ParameterRef(float)): Intensity of sun illumination. + fog_distance (float | ParameterRef(float)): Distance of the beginning fog from vehicle. + precepitation_type (string | ParameterRef(string)): Dry or rain. + precepitation_intensity (float | ParameterRef(float)): Intensity of rain. + weather_animation (bool | ParameterRef(bool)): If the weather should follow time of the day. + time (string | ParameterRef(string)): Time of the day at the beginning of simulation. name (string): Name of the behavior. Defaults to 'UpdateWeather'. Attributes: - _weather (srunner.scenariomanager.weather_sim.Weather): Weather settings. - """ - - def __init__(self, weather, name="ChangeWeather"): + _sun_azimuth_angle (float | ParameterRef(float)): Azimuth of the sun in radians. + _sun_altitude_angle (float | ParameterRef(float)): Elevation of the sun in radians. + _sun_intensity (float | ParameterRef(float)): Intensity of sun illumination. + _fog_distance (float | ParameterRef(float)): Distance of the beginning fog from vehicle. + _precepitation_type (string | ParameterRef(string)): Dry or rain. + _precepitation_intensity (float | ParameterRef(float)): Intensity of rain. + _weather_animation (bool | ParameterRef(bool)): If the weather should follow time of the day. + _time (string | ParameterRef(string)): Time of the day at the beginning of simulation. + """ + + def __init__(self, + sun_azimuth_angle, + sun_altitude_angle, + sun_intensity, + fog_distance, + precepitation_type, + precepitation_intensity, + weather_animation, + time, + name="ChangeWeather"): """ Setup parameters """ super(ChangeWeather, self).__init__(name) - self._weather = weather + + self._sun_azimuth_angle_ref = sun_azimuth_angle + self._sun_altitude_angle_ref = sun_altitude_angle + self._sun_intensity_ref = sun_intensity + self._fog_distance_ref = fog_distance + self._precepitation_type_ref = precepitation_type + self._precepitation_intensity_ref = precepitation_intensity + self._weather_animation_ref = weather_animation + self._time_ref = time def update(self): """ @@ -280,7 +314,32 @@ def update(self): returns: py_trees.common.Status.SUCCESS """ - py_trees.blackboard.Blackboard().set("CarlaWeather", self._weather, overwrite=True) + + carla_weather = carla.WeatherParameters() + carla_weather.sun_azimuth_angle = math.degrees(float(self._sun_azimuth_angle_ref)) + carla_weather.sun_altitude_angle = math.degrees(float(self._sun_altitude_angle_ref)) + carla_weather.cloudiness = 100 - float(self._sun_intensity_ref) * 100 + carla_weather.fog_distance = float(self._fog_distance_ref) + if carla_weather.fog_distance < 1000: + carla_weather.fog_density = 100 + carla_weather.precipitation = 0 + carla_weather.precipitation_deposits = 0 + carla_weather.wetness = 0 + carla_weather.wind_intensity = 30.0 + if self._precepitation_type_ref == "rain": + carla_weather.precipitation = float(self._precepitation_intensity_ref) * 100 + carla_weather.precipitation_deposits = 100 # if it rains, make the road wet + carla_weather.wetness = carla_weather.precipitation + elif self._precepitation_type_ref == "snow": + raise AttributeError("CARLA does not support snow precipitation") + + weather_animation = bool(self._weather_animation_ref) + time = str(self._time_ref) + dtime = datetime.datetime.strptime(time, "%Y-%m-%dT%H:%M:%S") + + weather = Weather(carla_weather, dtime, weather_animation) + + py_trees.blackboard.Blackboard().set("CarlaWeather", weather, overwrite=True) return py_trees.common.Status.SUCCESS @@ -292,20 +351,20 @@ class ChangeRoadFriction(AtomicBehavior): The behavior immediately terminates with SUCCESS after updating the friction. Args: - friction (float): New friction coefficient. + friction (float | ParameterRef(float)): New friction coefficient. name (string): Name of the behavior. Defaults to 'UpdateRoadFriction'. Attributes: - _friction (float): Friction coefficient. + _friction (float | ParameterRef(float)): Friction coefficient. """ - def __init__(self, friction, name="ChangeRoadFriction"): + def __init__(self, friction_ref, name="ChangeRoadFriction"): """ Setup parameters """ super(ChangeRoadFriction, self).__init__(name) - self._friction = friction + self._friction_ref = friction_ref def update(self): """ @@ -320,7 +379,7 @@ def update(self): friction_bp = CarlaDataProvider.get_world().get_blueprint_library().find('static.trigger.friction') extent = carla.Location(1000000.0, 1000000.0, 1000000.0) - friction_bp.set_attribute('friction', str(self._friction)) + friction_bp.set_attribute('friction', str(self._friction_ref)) friction_bp.set_attribute('extent_x', str(extent.x)) friction_bp.set_attribute('extent_y', str(extent.y)) friction_bp.set_attribute('extent_z', str(extent.z)) diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 4236d4425..258840483 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -226,10 +226,27 @@ 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)) + sun_azimuth_angle_ref, \ + sun_altitude_angle_ref, \ + sun_intensity_ref, \ + fog_distance_ref, \ + precepitation_type_ref, \ + precepitation_intensity_ref, \ + weather_animation_ref, \ + time_ref = OpenScenarioParser.get_weather_refs_from_env_action(self.config.init, self.config.catalogs) + + weather_update = ChangeWeather(sun_azimuth_angle_ref, + sun_altitude_angle_ref, + sun_intensity_ref, + fog_distance_ref, + precepitation_type_ref, + precepitation_intensity_ref, + weather_animation_ref, + time_ref) + + friction_ref = OpenScenarioParser.get_friction_refs_from_env_action(self.config.init, self.config.catalogs) + road_friction = ChangeRoadFriction( friction_ref ) + 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)) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 04b694edc..0f04143ed 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -16,6 +16,7 @@ import datetime import math import operator +from os import environ import py_trees import carla @@ -279,7 +280,7 @@ def assign_catalog_parameters(entry_instance, catalog_reference): return entry_instance @staticmethod - def get_friction_from_env_action(xml_tree, catalogs): + def get_friction_refs_from_env_action(xml_tree, catalogs): """ Extract the CARLA road friction coefficient from an OSC EnvironmentAction @@ -304,16 +305,14 @@ def get_friction_from_env_action(xml_tree, catalogs): catalog_reference = set_environment.find("CatalogReference") environment = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference) - friction = 1.0 + road_condition = environment.find("RoadCondition") + friction_ref = ParameterRef(road_condition.attrib.get('frictionScaleFactor')) - road_condition = environment.iter("RoadCondition") - for condition in road_condition: - friction = float(ParameterRef(condition.attrib.get('frictionScaleFactor'))) + return friction_ref - return friction @staticmethod - def get_weather_from_env_action(xml_tree, catalogs): + def get_weather_refs_from_env_action(xml_tree, catalogs): """ Extract the CARLA weather parameters from an OSC EnvironmentAction @@ -338,36 +337,30 @@ def get_weather_from_env_action(xml_tree, catalogs): catalog_reference = set_environment.find("CatalogReference") environment = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference) + time_of_day = environment.find("TimeOfDay") weather = environment.find("Weather") sun = weather.find("Sun") - - carla_weather = carla.WeatherParameters() - 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(ParameterRef(fog.attrib.get('visualRange', 'inf'))) - if carla_weather.fog_distance < 1000: - carla_weather.fog_density = 100 - carla_weather.precipitation = 0 - carla_weather.precipitation_deposits = 0 - carla_weather.wetness = 0 - carla_weather.wind_intensity = 30.0 precepitation = weather.find("Precipitation") - 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 ParameterRef(precepitation.attrib.get('type')) == "snow": - raise AttributeError("CARLA does not support snow precipitation") - - time_of_day = environment.find("TimeOfDay") - 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) + sun_azimuth_angle_ref = ParameterRef(sun.attrib.get('azimuth', 0)) + sun_altitude_angle_ref = ParameterRef(sun.attrib.get('elevation', 0)) + sun_intensity_ref = ParameterRef(sun.attrib.get('intensity', 0)) + fog_distance_ref = ParameterRef(fog.attrib.get('visualRange', 'inf')) + precepitation_type_ref = ParameterRef(precepitation.attrib.get('precipitationType')) + precepitation_intensity_ref = ParameterRef(precepitation.attrib.get('intensity')) + weather_animation_ref = ParameterRef(time_of_day.attrib.get("animation")) + time_ref = ParameterRef(time_of_day.attrib.get("dateTime")) + + return sun_azimuth_angle_ref, \ + sun_altitude_angle_ref, \ + sun_intensity_ref, \ + fog_distance_ref, \ + precepitation_type_ref, \ + precepitation_intensity_ref, \ + weather_animation_ref, \ + time_ref + @staticmethod def get_controller(xml_tree, catalogs): """ @@ -1008,10 +1001,27 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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)) + + sun_azimuth_angle_ref, \ + sun_altitude_angle_ref, \ + cloudiness_ref, \ + fog_distance_ref, \ + precepitation_type_ref, \ + precepitation_intensity_ref, \ + weather_animation_ref, \ + time_ref = OpenScenarioParser.get_weather_refs_from_env_action(global_action, catalogs) + + weather_behavior = ChangeWeather(sun_azimuth_angle_ref, + sun_altitude_angle_ref, + cloudiness_ref, + fog_distance_ref, + precepitation_type_ref, + precepitation_intensity_ref, + weather_animation_ref, + time_ref) + + friction_ref = OpenScenarioParser.get_friction_refs_from_env_action(global_action, catalogs) + friction_behavior = ChangeRoadFriction(friction_ref) env_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=maneuver_name) From 5fce64549f547afd52ed80b677ac0bfc5c64534d Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 4 Jan 2022 13:38:08 +0200 Subject: [PATCH 07/19] OSC TrafficSignalStateAction: moved parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 76 +++++++++++++++---- srunner/tools/openscenario_parser.py | 68 ++++------------- 2 files changed, 77 insertions(+), 67 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index faa6746d9..64671a549 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -270,17 +270,17 @@ class ChangeWeather(AtomicBehavior): weather_animation (bool | ParameterRef(bool)): If the weather should follow time of the day. time (string | ParameterRef(string)): Time of the day at the beginning of simulation. name (string): Name of the behavior. - Defaults to 'UpdateWeather'. + Defaults to 'ChangeWeather'. Attributes: - _sun_azimuth_angle (float | ParameterRef(float)): Azimuth of the sun in radians. - _sun_altitude_angle (float | ParameterRef(float)): Elevation of the sun in radians. - _sun_intensity (float | ParameterRef(float)): Intensity of sun illumination. - _fog_distance (float | ParameterRef(float)): Distance of the beginning fog from vehicle. - _precepitation_type (string | ParameterRef(string)): Dry or rain. - _precepitation_intensity (float | ParameterRef(float)): Intensity of rain. - _weather_animation (bool | ParameterRef(bool)): If the weather should follow time of the day. - _time (string | ParameterRef(string)): Time of the day at the beginning of simulation. + _sun_azimuth_angle_ref (float | ParameterRef(float)): Azimuth of the sun in radians. + _sun_altitude_angle_ref (float | ParameterRef(float)): Elevation of the sun in radians. + _sun_intensity_ref (float | ParameterRef(float)): Intensity of sun illumination. + _fog_distance_ref (float | ParameterRef(float)): Distance of the beginning fog from vehicle. + _precepitation_type_ref (string | ParameterRef(string)): Dry or rain. + _precepitation_intensity_ref (float | ParameterRef(float)): Intensity of rain. + _weather_animation_ref (bool | ParameterRef(bool)): If the weather should follow time of the day. + _time_ref (string | ParameterRef(string)): Time of the day at the beginning of simulation. """ def __init__(self, @@ -2469,11 +2469,17 @@ 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 """ + tl_states = { + "GREEN": carla.TrafficLightState.Green, + "YELLOW": carla.TrafficLightState.Yellow, + "RED": carla.TrafficLightState.Red, + "OFF": carla.TrafficLightState.Off, + } def __init__(self, actor, state, name="TrafficLightStateSetter"): """ @@ -2481,7 +2487,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__)) @@ -2489,12 +2495,20 @@ def update(self): """ Change the state of the traffic light """ - if self._actor is None: + actor = TrafficLightStateSetter.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 TrafficLightStateSetter.tl_states: + raise KeyError("CARLA only supports Green, Red, Yellow or Off") + traffic_light_state = TrafficLightStateSetter.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... @@ -2502,6 +2516,38 @@ def update(self): return new_status + @staticmethod + def get_traffic_light_from_osc_name(name): + """ + Returns a carla.TrafficLight instance that matches the name given + """ + traffic_light = None + + # Given by id + if name.startswith("id="): + tl_id = int(name[3:]) + for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): + if carla_tl.id == tl_id: + traffic_light = carla_tl + break + # Given by position + elif name.startswith("pos="): + tl_pos = name[4:] + pos = tl_pos.split(",") + for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): + carla_tl_location = carla_tl.get_transform().location + distance = carla_tl_location.distance(carla.Location(float(pos[0]), + float(pos[1]), + carla_tl_location.z)) + if distance < 2.0: + traffic_light = carla_tl + break + + if traffic_light is None: + raise AttributeError("Unknown traffic light {}".format(name)) + + return traffic_light + class ActorSource(AtomicBehavior): diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 0f04143ed..a51c3a8e9 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -108,48 +108,10 @@ class OpenScenarioParser(object): "miscellaneous": "miscellaneous" } - tl_states = { - "GREEN": carla.TrafficLightState.Green, - "YELLOW": carla.TrafficLightState.Yellow, - "RED": carla.TrafficLightState.Red, - "OFF": carla.TrafficLightState.Off, - } - global_osc_parameters = {} use_carla_coordinate_system = False osc_filepath = None - @staticmethod - def get_traffic_light_from_osc_name(name): - """ - Returns a carla.TrafficLight instance that matches the name given - """ - traffic_light = None - - # Given by id - if name.startswith("id="): - tl_id = int(name[3:]) - for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): - if carla_tl.id == tl_id: - traffic_light = carla_tl - break - # Given by position - elif name.startswith("pos="): - tl_pos = name[4:] - pos = tl_pos.split(",") - for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): - carla_tl_location = carla_tl.get_transform().location - distance = carla_tl_location.distance(carla.Location(float(pos[0]), - float(pos[1]), - carla_tl_location.z)) - if distance < 2.0: - traffic_light = carla_tl - break - - if traffic_light is None: - raise AttributeError("Unknown traffic light {}".format(name)) - - return traffic_light @staticmethod def set_osc_filepath(filepath): @@ -290,7 +252,7 @@ def get_friction_refs_from_env_action(xml_tree, catalogs): catalogs: XML Catalogs that could contain the EnvironmentAction returns: - friction (float) + friction (ParameterRef(float)) """ if xml_tree.findall('.//EnvironmentAction'): @@ -307,7 +269,6 @@ def get_friction_refs_from_env_action(xml_tree, catalogs): road_condition = environment.find("RoadCondition") friction_ref = ParameterRef(road_condition.attrib.get('frictionScaleFactor')) - return friction_ref @@ -322,7 +283,14 @@ def get_weather_refs_from_env_action(xml_tree, catalogs): catalogs: XML Catalogs that could contain the EnvironmentAction returns: - Weather (srunner.scenariomanager.weather_sim.Weather) + sun_azimuth_angle_ref (ParameterRef(float)): Azimuth of the sun in radians. + sun_altitude_angle_ref (ParameterRef(float)): Elevation of the sun in radians. + sun_intensity_ref (ParameterRef(float)): Intensity of sun illumination. + fog_distance_ref (ParameterRef(float)): Distance of the beginning fog from vehicle. + precepitation_type_ref (ParameterRef(string)): Dry or rain. + precepitation_intensity_ref (ParameterRef(float)): Intensity of rain. + weather_animation_ref (ParameterRef(bool)): If the weather should follow time of the day. + time_ref (ParameterRef(string)): Time of the day at the beginning of simulation. """ if xml_tree.findall('.//EnvironmentAction'): @@ -947,12 +915,12 @@ def convert_condition_to_atomic(condition, actor_list): tl_condition = value_condition.find('TrafficSignalCondition') name_condition = str(ParameterRef(tl_condition.attrib.get('name'))) - traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition) + traffic_light = TrafficLightStateSetter.get_traffic_light_from_osc_name(name_condition) tl_state = str(ParameterRef(tl_condition.attrib.get('state'))).upper() - if tl_state not in OpenScenarioParser.tl_states: + if tl_state not in TrafficLightStateSetter.tl_states: raise KeyError("CARLA only supports Green, Red, Yellow or Off") - state_condition = OpenScenarioParser.tl_states[tl_state] + state_condition = TrafficLightStateSetter.tl_states[tl_state] atomic = WaitForTrafficLightState( traffic_light, state_condition, name=condition_name) @@ -988,18 +956,14 @@ 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 = str(ParameterRef(traffic_light_action.attrib.get('name'))) - traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition) - - tl_state = str(ParameterRef(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: sun_azimuth_angle_ref, \ From 521ab45bdf50749a39b119e7672a0daf472977d8 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 4 Jan 2022 14:10:47 +0200 Subject: [PATCH 08/19] OSC CustomCommandAction: move parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 22 ++++++++----------- srunner/tools/openscenario_parser.py | 12 ++++++++-- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 64671a549..a24a857ed 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -41,8 +41,6 @@ from srunner.scenariomanager.weather_sim import Weather from srunner.tools.scenario_helper import detect_lane_obstacle from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane -from srunner.tools.scenario_param_ref import ParameterRef - import srunner.tools as sr_tools @@ -156,13 +154,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/" @@ -184,16 +182,14 @@ def update(self): Start script """ path = None - script_components = self._script.split(' ') interpreted_components = [] - for component in script_components: - interpreted_components.append(str(ParameterRef(component))) - script_components = interpreted_components - interpreted_script = ' '.join(script_components) + for component in self._script: + interpreted_components.append(str(component)) + interpreted_script = ' '.join(interpreted_components) - if len(script_components) > 1: - path = script_components[1] + if len(interpreted_components) > 1: + path = interpreted_components[1] if not os.path.isfile(path): path = os.path.join(self._base_path, path) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index a51c3a8e9..d3bcbbf07 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -963,7 +963,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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: sun_azimuth_angle_ref, \ @@ -1014,7 +1014,15 @@ 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') From 6bdd347324ebbe6f7e19d6fe5da913b810e4aa22 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 4 Jan 2022 16:43:40 +0200 Subject: [PATCH 09/19] OSC LongitudinalDistanceAction: moved parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 30 +++++++++++++++---- srunner/tools/openscenario_parser.py | 30 +++++++++++-------- 2 files changed, 42 insertions(+), 18 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index a24a857ed..d9445ac33 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -3222,7 +3222,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 """ @@ -3233,15 +3233,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/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index d3bcbbf07..24c7ef5e8 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1070,14 +1070,10 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): elif private_action.find('LongitudinalDistanceAction') is not None: long_dist_action = private_action.find("LongitudinalDistanceAction") - obj = str(ParameterRef(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: @@ -1085,13 +1081,21 @@ 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 = float(ParameterRef(constraints.attrib.get('maxSpeed', None)) - ) if constraints is not None else None - continues = bool(strtobool(str(ParameterRef(long_dist_action.attrib.get('continuous'))))) - freespace = bool(strtobool(str(ParameterRef(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, - name=maneuver_name) + + 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: From 0367b494d4a5111a3abd93e8861c28f9fc35bd24 Mon Sep 17 00:00:00 2001 From: lauri Date: Tue, 4 Jan 2022 22:09:29 +0200 Subject: [PATCH 10/19] OSC LaneChangeAction: moved parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 85 +++++++++++++++---- srunner/tools/openscenario_parser.py | 45 +++++----- 2 files changed, 90 insertions(+), 40 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index d9445ac33..f10d88226 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1035,51 +1035,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. @@ -1090,6 +1138,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: diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 24c7ef5e8..4e5de7e76 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1081,7 +1081,7 @@ 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_ref = ParameterRef(constraints.attrib.get('maxSpeed', None) ) if constraints is not None else None continues_ref = ParameterRef(long_dist_action.attrib.get('continuous')) @@ -1099,30 +1099,31 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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 = str(ParameterRef(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(str(ParameterRef(lat_maneuver.attrib.get('continuous', "true"))))) From 5239227393e8a08f7bd0ec1b8b5d66be7d6c5e61 Mon Sep 17 00:00:00 2001 From: lauri Date: Wed, 5 Jan 2022 13:38:01 +0200 Subject: [PATCH 11/19] OSC LaneOffsetAction: moved parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 46 +++++++++++++++---- srunner/tools/openscenario_parser.py | 28 +++++------ 2 files changed, 50 insertions(+), 24 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index f10d88226..1a993e4d8 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1241,16 +1241,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 + offset (float | ParameterRef(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 + 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 @@ -1261,21 +1269,42 @@ 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']: + relative_actor = _actor + break + if relative_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format(relative_actor_name)) + self._relative_actor = relative_actor + def initialise(self): """ Set _start_time and get (actor, controller) pair from Blackboard. @@ -1285,6 +1314,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: diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 4e5de7e76..b651b0322 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1125,34 +1125,30 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): raise NotImplementedError("LaneChangeTarget: AbsoluteTargetLane is not implemented") elif private_action.find('LaneOffsetAction') is not None: + lat_maneuver = private_action.find('LaneOffsetAction') - continuous = bool(strtobool(str(ParameterRef(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 = str(ParameterRef(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: From 744871fd371c820057202b5d910573fc3b25475c Mon Sep 17 00:00:00 2001 From: lauri Date: Thu, 6 Jan 2022 12:46:08 +0200 Subject: [PATCH 12/19] OSC SynchronizeAction: moved parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 99 +++++++++---------- srunner/tools/openscenario_parser.py | 72 ++++++++++---- 2 files changed, 97 insertions(+), 74 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 1a993e4d8..9c16d76d0 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -726,22 +726,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 """ @@ -749,17 +751,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. @@ -767,6 +796,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: @@ -1867,6 +1897,8 @@ def update(self): """ Dynamic control update for actor velocity """ + raise AttributeError("Are we even there") + new_status = py_trees.common.Status.RUNNING distance_reference = calculate_distance(CarlaDataProvider.get_location(self._actor_reference), @@ -2549,13 +2581,6 @@ class TrafficLightStateSetter(AtomicBehavior): The behavior terminates after trying to set the new state """ - tl_states = { - "GREEN": carla.TrafficLightState.Green, - "YELLOW": carla.TrafficLightState.Yellow, - "RED": carla.TrafficLightState.Red, - "OFF": carla.TrafficLightState.Off, - } - def __init__(self, actor, state, name="TrafficLightStateSetter"): """ Init @@ -2570,13 +2595,14 @@ def update(self): """ Change the state of the traffic light """ - actor = TrafficLightStateSetter.get_traffic_light_from_osc_name(str(self._actor)) + 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 TrafficLightStateSetter.tl_states: + 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 = TrafficLightStateSetter.tl_states[tl_state] + traffic_light_state = sr_tools.openscenario_parser.OpenScenarioParser.tl_states[tl_state] if traffic_light is None: return py_trees.common.Status.FAILURE @@ -2591,39 +2617,6 @@ def update(self): return new_status - @staticmethod - def get_traffic_light_from_osc_name(name): - """ - Returns a carla.TrafficLight instance that matches the name given - """ - traffic_light = None - - # Given by id - if name.startswith("id="): - tl_id = int(name[3:]) - for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): - if carla_tl.id == tl_id: - traffic_light = carla_tl - break - # Given by position - elif name.startswith("pos="): - tl_pos = name[4:] - pos = tl_pos.split(",") - for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): - carla_tl_location = carla_tl.get_transform().location - distance = carla_tl_location.distance(carla.Location(float(pos[0]), - float(pos[1]), - carla_tl_location.z)) - if distance < 2.0: - traffic_light = carla_tl - break - - if traffic_light is None: - raise AttributeError("Unknown traffic light {}".format(name)) - - return traffic_light - - class ActorSource(AtomicBehavior): """ diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index b651b0322..f24f67451 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -108,10 +108,48 @@ class OpenScenarioParser(object): "miscellaneous": "miscellaneous" } + tl_states = { + "GREEN": carla.TrafficLightState.Green, + "YELLOW": carla.TrafficLightState.Yellow, + "RED": carla.TrafficLightState.Red, + "OFF": carla.TrafficLightState.Off, + } + global_osc_parameters = {} use_carla_coordinate_system = False osc_filepath = None + @staticmethod + def get_traffic_light_from_osc_name(name): + """ + Returns a carla.TrafficLight instance that matches the name given + """ + traffic_light = None + + # Given by id + if name.startswith("id="): + tl_id = int(name[3:]) + for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): + if carla_tl.id == tl_id: + traffic_light = carla_tl + break + # Given by position + elif name.startswith("pos="): + tl_pos = name[4:] + pos = tl_pos.split(",") + for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): + carla_tl_location = carla_tl.get_transform().location + distance = carla_tl_location.distance(carla.Location(float(pos[0]), + float(pos[1]), + carla_tl_location.z)) + if distance < 2.0: + traffic_light = carla_tl + break + + if traffic_light is None: + raise AttributeError("Unknown traffic light {}".format(name)) + + return traffic_light @staticmethod def set_osc_filepath(filepath): @@ -1157,37 +1195,29 @@ 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 (str(ParameterRef(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( - str(ParameterRef(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 = float(ParameterRef(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: From 9249b63f4d4a34ad045a4e4303cf420f80436f54 Mon Sep 17 00:00:00 2001 From: lauri Date: Fri, 7 Jan 2022 10:28:00 +0200 Subject: [PATCH 13/19] OSC ActivateControllerAction: move parameter resolution to runtime --- srunner/scenariomanager/scenarioatomics/atomic_behaviors.py | 4 ++-- srunner/tools/openscenario_parser.py | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 9c16d76d0..5b5b3dabd 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1759,7 +1759,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 @@ -1783,7 +1783,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: diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index f24f67451..8b11c620c 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1220,9 +1220,11 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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(str(ParameterRef(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') From fa1ed9e36b6e255a341d73d49b49990eed396f20 Mon Sep 17 00:00:00 2001 From: lauri Date: Fri, 7 Jan 2022 11:06:07 +0200 Subject: [PATCH 14/19] OSC ControllerAction: move parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 24 ++++++++++++++++--- srunner/scenarios/open_scenario.py | 9 +++---- srunner/tools/openscenario_parser.py | 4 ++-- 3 files changed, 28 insertions(+), 9 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 5b5b3dabd..b96c3edeb 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -409,14 +409,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): """ diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 258840483..414b6842f 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -269,14 +269,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 8b11c620c..25f5706b3 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1228,8 +1228,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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') From b49ad0ea71eb5cd709455a0364695c627417fb6f Mon Sep 17 00:00:00 2001 From: lauri Date: Fri, 7 Jan 2022 11:44:24 +0200 Subject: [PATCH 15/19] OSC RoutingAction: moved parameter resolution to runtime --- .../scenarioatomics/atomic_behaviors.py | 18 +++++++++++++++++- srunner/tools/openscenario_parser.py | 4 ++-- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index b96c3edeb..97588c586 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -939,11 +939,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. @@ -951,15 +960,21 @@ 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. @@ -969,6 +984,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: diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 25f5706b3..e73a3d178 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1240,8 +1240,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: From a3068208f165e66738731fd2084be57b850fed08 Mon Sep 17 00:00:00 2001 From: lauri Date: Fri, 7 Jan 2022 12:10:23 +0200 Subject: [PATCH 16/19] OSC EnvironmentAction: refactoring --- .../scenarioatomics/atomic_behaviors.py | 102 ++++++------------ srunner/scenarios/open_scenario.py | 22 +--- srunner/tools/openscenario_parser.py | 92 +++++++--------- 3 files changed, 68 insertions(+), 148 deletions(-) diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 97588c586..dd7f4aa3d 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -248,94 +248,44 @@ def update(self): return new_status + 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: - sun_azimuth_angle (float | ParameterRef(float)): Azimuth of the sun in radians. - sun_altitude_angle (float | ParameterRef(float)): Elevation of the sun in radians. - sun_intensity (float | ParameterRef(float)): Intensity of sun illumination. - fog_distance (float | ParameterRef(float)): Distance of the beginning fog from vehicle. - precepitation_type (string | ParameterRef(string)): Dry or rain. - precepitation_intensity (float | ParameterRef(float)): Intensity of rain. - weather_animation (bool | ParameterRef(bool)): If the weather should follow time of the day. - time (string | ParameterRef(string)): Time of the day at the beginning of simulation. name (string): Name of the behavior. - Defaults to 'ChangeWeather'. - + 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: - _sun_azimuth_angle_ref (float | ParameterRef(float)): Azimuth of the sun in radians. - _sun_altitude_angle_ref (float | ParameterRef(float)): Elevation of the sun in radians. - _sun_intensity_ref (float | ParameterRef(float)): Intensity of sun illumination. - _fog_distance_ref (float | ParameterRef(float)): Distance of the beginning fog from vehicle. - _precepitation_type_ref (string | ParameterRef(string)): Dry or rain. - _precepitation_intensity_ref (float | ParameterRef(float)): Intensity of rain. - _weather_animation_ref (bool | ParameterRef(bool)): If the weather should follow time of the day. - _time_ref (string | ParameterRef(string)): Time of the day at the beginning of simulation. - """ - - def __init__(self, - sun_azimuth_angle, - sun_altitude_angle, - sun_intensity, - fog_distance, - precepitation_type, - precepitation_intensity, - weather_animation, - time, - name="ChangeWeather"): + _weather (srunner.scenariomanager.weather_sim.Weather): Weather settings. + """ + + def __init__(self, global_action, catalogs, name="ChangeWeather"): """ Setup parameters """ super(ChangeWeather, self).__init__(name) + self._global_action = global_action + self._catalogs = catalogs + self._weather = None - self._sun_azimuth_angle_ref = sun_azimuth_angle - self._sun_altitude_angle_ref = sun_altitude_angle - self._sun_intensity_ref = sun_intensity - self._fog_distance_ref = fog_distance - self._precepitation_type_ref = precepitation_type - self._precepitation_intensity_ref = precepitation_intensity - self._weather_animation_ref = weather_animation - self._time_ref = time + 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 """ - - carla_weather = carla.WeatherParameters() - carla_weather.sun_azimuth_angle = math.degrees(float(self._sun_azimuth_angle_ref)) - carla_weather.sun_altitude_angle = math.degrees(float(self._sun_altitude_angle_ref)) - carla_weather.cloudiness = 100 - float(self._sun_intensity_ref) * 100 - carla_weather.fog_distance = float(self._fog_distance_ref) - if carla_weather.fog_distance < 1000: - carla_weather.fog_density = 100 - carla_weather.precipitation = 0 - carla_weather.precipitation_deposits = 0 - carla_weather.wetness = 0 - carla_weather.wind_intensity = 30.0 - if self._precepitation_type_ref == "rain": - carla_weather.precipitation = float(self._precepitation_intensity_ref) * 100 - carla_weather.precipitation_deposits = 100 # if it rains, make the road wet - carla_weather.wetness = carla_weather.precipitation - elif self._precepitation_type_ref == "snow": - raise AttributeError("CARLA does not support snow precipitation") - - weather_animation = bool(self._weather_animation_ref) - time = str(self._time_ref) - dtime = datetime.datetime.strptime(time, "%Y-%m-%dT%H:%M:%S") - - weather = Weather(carla_weather, dtime, weather_animation) - - py_trees.blackboard.Blackboard().set("CarlaWeather", weather, overwrite=True) + py_trees.blackboard.Blackboard().set("CarlaWeather", self._weather, overwrite=True) return py_trees.common.Status.SUCCESS @@ -347,20 +297,28 @@ class ChangeRoadFriction(AtomicBehavior): The behavior immediately terminates with SUCCESS after updating the friction. Args: - friction (float | ParameterRef(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 | ParameterRef(float)): Friction coefficient. """ - def __init__(self, friction_ref, name="ChangeRoadFriction"): + def __init__(self, global_action, catalogs, name="ChangeRoadFriction"): """ Setup parameters """ super(ChangeRoadFriction, self).__init__(name) - self._friction_ref = friction_ref + 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): """ @@ -375,7 +333,7 @@ def update(self): friction_bp = CarlaDataProvider.get_world().get_blueprint_library().find('static.trigger.friction') extent = carla.Location(1000000.0, 1000000.0, 1000000.0) - friction_bp.set_attribute('friction', str(self._friction_ref)) + friction_bp.set_attribute('friction', str(self._friction)) friction_bp.set_attribute('extent_x', str(extent.x)) friction_bp.set_attribute('extent_y', str(extent.y)) friction_bp.set_attribute('extent_z', str(extent.z)) diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 414b6842f..929dd4ec3 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -226,26 +226,8 @@ def _create_environment_behavior(self): env_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior") - sun_azimuth_angle_ref, \ - sun_altitude_angle_ref, \ - sun_intensity_ref, \ - fog_distance_ref, \ - precepitation_type_ref, \ - precepitation_intensity_ref, \ - weather_animation_ref, \ - time_ref = OpenScenarioParser.get_weather_refs_from_env_action(self.config.init, self.config.catalogs) - - weather_update = ChangeWeather(sun_azimuth_angle_ref, - sun_altitude_angle_ref, - sun_intensity_ref, - fog_distance_ref, - precepitation_type_ref, - precepitation_intensity_ref, - weather_animation_ref, - time_ref) - - friction_ref = OpenScenarioParser.get_friction_refs_from_env_action(self.config.init, self.config.catalogs) - road_friction = ChangeRoadFriction( friction_ref ) + 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)) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index e73a3d178..8f9c8395c 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -280,17 +280,15 @@ def assign_catalog_parameters(entry_instance, catalog_reference): return entry_instance @staticmethod - def get_friction_refs_from_env_action(xml_tree, catalogs): + 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 (ParameterRef(float)) + friction (float) """ if xml_tree.findall('.//EnvironmentAction'): @@ -305,30 +303,24 @@ def get_friction_refs_from_env_action(xml_tree, catalogs): catalog_reference = set_environment.find("CatalogReference") environment = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference) - road_condition = environment.find("RoadCondition") - friction_ref = ParameterRef(road_condition.attrib.get('frictionScaleFactor')) - return friction_ref + friction = 1.0 + + road_condition = environment.iter("RoadCondition") + for condition in road_condition: + friction = float(ParameterRef(condition.attrib.get('frictionScaleFactor'))) + return friction @staticmethod - def get_weather_refs_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: - sun_azimuth_angle_ref (ParameterRef(float)): Azimuth of the sun in radians. - sun_altitude_angle_ref (ParameterRef(float)): Elevation of the sun in radians. - sun_intensity_ref (ParameterRef(float)): Intensity of sun illumination. - fog_distance_ref (ParameterRef(float)): Distance of the beginning fog from vehicle. - precepitation_type_ref (ParameterRef(string)): Dry or rain. - precepitation_intensity_ref (ParameterRef(float)): Intensity of rain. - weather_animation_ref (ParameterRef(bool)): If the weather should follow time of the day. - time_ref (ParameterRef(string)): Time of the day at the beginning of simulation. + Weather (srunner.scenariomanager.weather_sim.Weather) """ if xml_tree.findall('.//EnvironmentAction'): @@ -343,29 +335,35 @@ def get_weather_refs_from_env_action(xml_tree, catalogs): catalog_reference = set_environment.find("CatalogReference") environment = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference) - time_of_day = environment.find("TimeOfDay") weather = environment.find("Weather") sun = weather.find("Sun") + + carla_weather = carla.WeatherParameters() + 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(ParameterRef(fog.attrib.get('visualRange', 'inf'))) + if carla_weather.fog_distance < 1000: + carla_weather.fog_density = 100 + carla_weather.precipitation = 0 + carla_weather.precipitation_deposits = 0 + carla_weather.wetness = 0 + carla_weather.wind_intensity = 30.0 precepitation = weather.find("Precipitation") + 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 ParameterRef(precepitation.attrib.get('type')) == "snow": + raise AttributeError("CARLA does not support snow precipitation") + + time_of_day = environment.find("TimeOfDay") + 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") - sun_azimuth_angle_ref = ParameterRef(sun.attrib.get('azimuth', 0)) - sun_altitude_angle_ref = ParameterRef(sun.attrib.get('elevation', 0)) - sun_intensity_ref = ParameterRef(sun.attrib.get('intensity', 0)) - fog_distance_ref = ParameterRef(fog.attrib.get('visualRange', 'inf')) - precepitation_type_ref = ParameterRef(precepitation.attrib.get('precipitationType')) - precepitation_intensity_ref = ParameterRef(precepitation.attrib.get('intensity')) - weather_animation_ref = ParameterRef(time_of_day.attrib.get("animation")) - time_ref = ParameterRef(time_of_day.attrib.get("dateTime")) - - return sun_azimuth_angle_ref, \ - sun_altitude_angle_ref, \ - sun_intensity_ref, \ - fog_distance_ref, \ - precepitation_type_ref, \ - precepitation_intensity_ref, \ - weather_animation_ref, \ - time_ref + return Weather(carla_weather, dtime, weather_animation) @staticmethod def get_controller(xml_tree, catalogs): @@ -1004,26 +1002,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): elif global_action.find('EnvironmentAction') is not None: - sun_azimuth_angle_ref, \ - sun_altitude_angle_ref, \ - cloudiness_ref, \ - fog_distance_ref, \ - precepitation_type_ref, \ - precepitation_intensity_ref, \ - weather_animation_ref, \ - time_ref = OpenScenarioParser.get_weather_refs_from_env_action(global_action, catalogs) - - weather_behavior = ChangeWeather(sun_azimuth_angle_ref, - sun_altitude_angle_ref, - cloudiness_ref, - fog_distance_ref, - precepitation_type_ref, - precepitation_intensity_ref, - weather_animation_ref, - time_ref) - - friction_ref = OpenScenarioParser.get_friction_refs_from_env_action(global_action, catalogs) - friction_behavior = ChangeRoadFriction(friction_ref) + 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) From d0d7724c936f5d90c7ca8978230dd1529ffaa878 Mon Sep 17 00:00:00 2001 From: lauri Date: Fri, 7 Jan 2022 12:37:35 +0200 Subject: [PATCH 17/19] Code style improvements --- .../openscenario_configuration.py | 6 +- .../scenarioatomics/atomic_behaviors.py | 45 ++-- srunner/scenarios/open_scenario.py | 2 +- srunner/tools/openscenario_parser.py | 234 +++++++++++++++--- srunner/tools/scenario_param_ref.py | 180 -------------- 5 files changed, 227 insertions(+), 240 deletions(-) delete mode 100644 srunner/tools/scenario_param_ref.py diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index 9995a2b4e..f5f6e41bf 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -21,8 +21,7 @@ from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData, ScenarioConfiguration # pylint: enable=line-too-long from srunner.scenariomanager.carla_data_provider import CarlaDataProvider # workaround -from srunner.tools.openscenario_parser import OpenScenarioParser -from srunner.tools.scenario_param_ref import ParameterRef +from srunner.tools.openscenario_parser import OpenScenarioParser, ParameterRef class OpenScenarioConfiguration(ScenarioConfiguration): @@ -99,7 +98,8 @@ def _check_version(self): Ensure correct OpenSCENARIO version is used """ header = self.xml_tree.find("FileHeader") - if not (str(ParameterRef(header.attrib.get('revMajor'))) == "1" and str(ParameterRef(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): diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index dd7f4aa3d..00ae80fbe 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -21,7 +21,6 @@ import os import time import subprocess -import datetime import numpy as np from numpy import random @@ -38,7 +37,6 @@ from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.actorcontrols.actor_control import ActorControl from srunner.scenariomanager.timer import GameTime -from srunner.scenariomanager.weather_sim import Weather from srunner.tools.scenario_helper import detect_lane_obstacle from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane @@ -248,7 +246,6 @@ def update(self): return new_status - class ChangeWeather(AtomicBehavior): """ @@ -314,7 +311,7 @@ def __init__(self, global_action, catalogs, name="ChangeRoadFriction"): 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( @@ -367,7 +364,8 @@ class ChangeActorControl(AtomicBehavior): _actor_control (ActorControl): Instance of the actor control. """ - def __init__(self, actor, osc_controller_action=None, catalogs=None, 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. """ @@ -393,7 +391,6 @@ def initialise(self): super(ChangeActorControl, self).initialise() - def update(self): """ Write (actor, controler) pair to Blackboard, or update the controller @@ -753,7 +750,7 @@ def _get_parameter_values(self): 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']): + if master_actor_name == actor_ins.attributes['role_name']: self._master_actor = actor_ins break if self._master_actor is None: @@ -931,7 +928,8 @@ def __init__(self, actor, waypoints=None, osc_route_action=None, catalogs=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) + self._waypoints = sr_tools.openscenario_parser.OpenScenarioParser.get_route( + self._route_action, self._catalogs) def initialise(self): """ @@ -1109,7 +1107,7 @@ class ChangeActorLateralMotion(AtomicBehavior): """ def __init__(self, actor, target_lane_rel=1, dimension="distance", dimension_value=25, distance_other_lane=100, - name="ChangeActorLateralMotion"): + name="ChangeActorLateralMotion"): """ Setup parameters """ @@ -1124,7 +1122,7 @@ def __init__(self, actor, target_lane_rel=1, dimension="distance", dimension_val 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 @@ -1139,7 +1137,7 @@ def _get_parameter_values(self): """ 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": @@ -1263,12 +1261,12 @@ class ChangeActorLaneOffset(AtomicBehavior): Args: actor (carla.Actor): Controlled actor. - offset (float | ParameterRef(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. + 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. + 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 @@ -1277,7 +1275,7 @@ class ChangeActorLaneOffset(AtomicBehavior): _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. + _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 @@ -1292,7 +1290,7 @@ class ChangeActorLaneOffset(AtomicBehavior): OFFSET_THRESHOLD = 0.1 def __init__(self, actor, offset, relative_actor_name=None, continuous=True, actor_list=None, - name="ChangeActorWaypoints"): + name="ChangeActorWaypoints"): """ Setup parameters """ @@ -1321,11 +1319,10 @@ def _get_parameter_values(self): 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']: - relative_actor = _actor + self._relative_actor = _actor break - if relative_actor is None: - raise AttributeError("Cannot find actor '{}' for condition".format(relative_actor_name)) - self._relative_actor = relative_actor + if self._relative_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format(self._relative_actor_name)) def initialise(self): """ @@ -1889,8 +1886,6 @@ def update(self): """ Dynamic control update for actor velocity """ - raise AttributeError("Are we even there") - new_status = py_trees.common.Status.RUNNING distance_reference = calculate_distance(CarlaDataProvider.get_location(self._actor_reference), @@ -2573,6 +2568,7 @@ class TrafficLightStateSetter(AtomicBehavior): The behavior terminates after trying to set the new state """ + def __init__(self, actor, state, name="TrafficLightStateSetter"): """ Init @@ -2588,7 +2584,7 @@ def update(self): Change the state of the traffic light """ actor = sr_tools.openscenario_parser.OpenScenarioParser.get_traffic_light_from_osc_name( - str(self._actor)) + str(self._actor)) traffic_light = actor if "traffic_light" in actor.type_id else None tl_state = str(self._state).upper() @@ -2609,6 +2605,7 @@ def update(self): return new_status + class ActorSource(AtomicBehavior): """ diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 929dd4ec3..85b2b1560 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -227,7 +227,7 @@ def _create_environment_behavior(self): policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior") weather_update = ChangeWeather(self.config.init, self.config.catalogs) - road_friction = ChangeRoadFriction( 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)) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 8f9c8395c..457892e77 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -17,6 +17,7 @@ import math import operator from os import environ +import re import py_trees import carla @@ -72,7 +73,6 @@ from srunner.scenariomanager.timer import TimeOut, SimulationTimeCondition from srunner.tools.py_trees_port import oneshot_behavior from srunner.tools.scenario_helper import get_offset_transform, get_troad_from_transform -from srunner.tools.scenario_param_ref import ParameterRef def oneshot_with_check(variable_name, behaviour, name=None): @@ -88,6 +88,175 @@ def oneshot_with_check(variable_name, behaviour, name=None): return oneshot_behavior(variable_name, behaviour, name) +class ParameterRef: + """ + This class stores osc parameter reference in its original form. + Returns the converted value whenever it is used. + """ + + def __init__(self, reference_text) -> None: + # TODO: (for OSC1.1) add methods(lexer and math_interpreter) to + # recognize and interpret math expression from reference_text + self.reference_text = str(reference_text) + + def is_literal(self) -> bool: + """ + Returns: True when text is a literal/number + """ + return self._is_matching(pattern=r"(-)?\d+(\.\d*)?") + + def is_parameter(self) -> bool: + """ + Returns: True when text is a parameter + """ + return self._is_matching(pattern=r"[$][A-Za-z_][\w]*") + + def _is_matching(self, pattern: str) -> bool: + """ + Returns: True when pattern is matching with text + """ + match = re.search(pattern, self.reference_text) + if match is not None: + matching_string = match.group() + 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 + """ + if self.is_literal(): + value = self.reference_text + elif self.is_parameter(): + value = CarlaDataProvider.get_osc_global_param_value(self.reference_text) + if value is None: + raise Exception("Parameter '{}' is not defined".format(self.reference_text[1:])) + else: + value = None + return value + + def __float__(self) -> float: + value = self.get_interpreted_value() + if value is not None: + return float(value) + else: + raise Exception("could not convert '{}' to float".format(self.reference_text)) + + def __int__(self) -> int: + value = self.get_interpreted_value() + if value is not None: + return int(float(value)) + else: + raise Exception("could not convert '{}' to int".format(self.reference_text)) + + 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__() + + def __add__(self, other) -> bool: + return other + self.__float__() + + def __rsub__(self, other) -> bool: + return other - self.__float__() + + def __sub__(self, other) -> bool: + return self.__float__() - other + + def __rmul__(self, other) -> bool: + return other * self.__float__() + + def __mul__(self, other) -> bool: + return other * self.__float__() + + def __truediv__(self, other) -> bool: + return self.__float__() / other + + def __rtruediv__(self, other) -> bool: + return other / self.__float__() + + def __eq__(self, other) -> bool: + value = self._cast_type(other) + return other == value + + def __ne__(self, other) -> bool: + value = self._cast_type(other) + return other != value + + def __ge__(self, other) -> bool: + value = self._cast_type(other) + return value >= other + + def __le__(self, other) -> bool: + value = self._cast_type(other) + return value <= other + + def __gt__(self, other) -> bool: + value = self._cast_type(other) + return value > other + + def __lt__(self, other) -> bool: + value = self._cast_type(other) + return value < other + + def __GE__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value >= other + + def __LE__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value <= other + + def __GT__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value > other + + def __LT__(self, other) -> bool: # pylint: disable=invalid-name + value = self._cast_type(other) + return value < other + + def __iadd__(self, other) -> bool: + return self.__float__() + other + + def __isub__(self, other) -> bool: + return self.__float__() - other + + def __abs__(self): + return abs(self.__float__()) + + class OpenScenarioParser(object): """ @@ -109,10 +278,10 @@ class OpenScenarioParser(object): } tl_states = { - "GREEN": carla.TrafficLightState.Green, - "YELLOW": carla.TrafficLightState.Yellow, - "RED": carla.TrafficLightState.Red, - "OFF": carla.TrafficLightState.Off, + "GREEN": carla.TrafficLightState.Green, + "YELLOW": carla.TrafficLightState.Yellow, + "RED": carla.TrafficLightState.Red, + "OFF": carla.TrafficLightState.Off, } global_osc_parameters = {} @@ -364,7 +533,7 @@ def get_weather_from_env_action(xml_tree, catalogs): dtime = datetime.datetime.strptime(time, "%Y-%m-%dT%H:%M:%S") return Weather(carla_weather, dtime, weather_animation) - + @staticmethod def get_controller(xml_tree, catalogs): """ @@ -951,12 +1120,12 @@ def convert_condition_to_atomic(condition, actor_list): tl_condition = value_condition.find('TrafficSignalCondition') name_condition = str(ParameterRef(tl_condition.attrib.get('name'))) - traffic_light = TrafficLightStateSetter.get_traffic_light_from_osc_name(name_condition) + traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition) tl_state = str(ParameterRef(tl_condition.attrib.get('state'))).upper() - if tl_state not in TrafficLightStateSetter.tl_states: + if tl_state not in OpenScenarioParser.tl_states: raise KeyError("CARLA only supports Green, Red, Yellow or Off") - state_condition = TrafficLightStateSetter.tl_states[tl_state] + state_condition = OpenScenarioParser.tl_states[tl_state] atomic = WaitForTrafficLightState( traffic_light, state_condition, name=condition_name) @@ -996,7 +1165,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): tl_state_ref = ParameterRef(traffic_light_action.attrib.get('state')) atomic = TrafficLightStateSetter( - name_condition_ref, tl_state_ref, name=maneuver_name + "_" + str(traffic_light_action.attrib.get('name'))) + 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") @@ -1101,19 +1271,19 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): constraints = long_dist_action.find('DynamicConstraints') max_speed_ref = ParameterRef(constraints.attrib.get('maxSpeed', None) - ) if constraints is not None else 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) + 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: @@ -1123,7 +1293,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): lat_maneuver = private_action.find('LaneChangeAction') 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( @@ -1132,12 +1302,12 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): "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) + 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") @@ -1162,11 +1332,11 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): 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) + relative_offset_ref, + relative_actor_name_ref, + continuous=continuous_ref, + actor_list=actor_list, + name=maneuver_name) else: raise AttributeError("Unknown target offset") else: @@ -1221,7 +1391,7 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): # @TODO: How to handle relative positions here? This might chance at runtime?! route_action = private_action.find('AssignRouteAction') atomic = ChangeActorWaypoints(actor, osc_route_action=route_action, catalogs=catalogs, - name=maneuver_name) + 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: diff --git a/srunner/tools/scenario_param_ref.py b/srunner/tools/scenario_param_ref.py deleted file mode 100644 index 84c348fe8..000000000 --- a/srunner/tools/scenario_param_ref.py +++ /dev/null @@ -1,180 +0,0 @@ -#!/usr/bin/env python - -""" -This module provides a class for retrieving parameter values of -scenario configuration files based on OpenSCENARIO -""" - -import re -from distutils.util import strtobool - -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider - - -class ParameterRef: - """ - This class stores osc parameter reference in its original form. - Returns the converted value whenever it is used. - """ - - def __init__(self, reference_text) -> None: - # TODO: (for OSC1.1) add methods(lexer and math_interpreter) to - # recognize and interpret math expression from reference_text - self.reference_text = str(reference_text) - - def is_literal(self) -> bool: - """ - Returns: True when text is a literal/number - """ - return self._is_matching(pattern=r"(-)?\d+(\.\d*)?") - - def is_parameter(self) -> bool: - """ - Returns: True when text is a parameter - """ - return self._is_matching(pattern=r"[$][A-Za-z_][\w]*") - - def _is_matching(self, pattern: str) -> bool: - """ - Returns: True when pattern is matching with text - """ - match = re.search(pattern, self.reference_text) - if match is not None: - matching_string = match.group() - 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 - """ - if self.is_literal(): - value = self.reference_text - elif self.is_parameter(): - value = CarlaDataProvider.get_osc_global_param_value(self.reference_text) - if value is None: - raise Exception("Parameter '{}' is not defined".format(self.reference_text[1:])) - else: - value = None - return value - - def __float__(self) -> float: - value = self.get_interpreted_value() - if value is not None: - return float(value) - else: - raise Exception("could not convert '{}' to float".format(self.reference_text)) - - def __int__(self) -> int: - value = self.get_interpreted_value() - if value is not None: - return int(float(value)) - else: - raise Exception("could not convert '{}' to int".format(self.reference_text)) - - 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__() - - def __add__(self, other) -> bool: - return other + self.__float__() - - def __rsub__(self, other) -> bool: - return other - self.__float__() - - def __sub__(self, other) -> bool: - return self.__float__() - other - - def __rmul__(self, other) -> bool: - return other * self.__float__() - - def __mul__(self, other) -> bool: - return other * self.__float__() - - def __truediv__(self, other) -> bool: - return self.__float__() / other - - def __rtruediv__(self, other) -> bool: - return other / self.__float__() - - def __eq__(self, other) -> bool: - value = self._cast_type(other) - return other == value - - def __ne__(self, other) -> bool: - value = self._cast_type(other) - return other != value - - def __ge__(self, other) -> bool: - value = self._cast_type(other) - return value >= other - - def __le__(self, other) -> bool: - value = self._cast_type(other) - return value <= other - - def __gt__(self, other) -> bool: - value = self._cast_type(other) - return value > other - - def __lt__(self, other) -> bool: - value = self._cast_type(other) - return value < other - - def __GE__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value >= other - - def __LE__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value <= other - - def __GT__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value > other - - def __LT__(self, other) -> bool: # pylint: disable=invalid-name - value = self._cast_type(other) - return value < other - - def __iadd__(self, other) -> bool: - return self.__float__() + other - - def __isub__(self, other) -> bool: - return self.__float__() - other - - def __abs__(self): - return abs(self.__float__()) From 1b9906cb6b30b819ab2f7909741e01b27ffa1dce Mon Sep 17 00:00:00 2001 From: lasuomela Date: Fri, 7 Jan 2022 14:52:58 +0200 Subject: [PATCH 18/19] Update CHANGELOG.md --- Docs/CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) 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 From 5871fdcbbb13dc7134c70073a0568d0546eebf34 Mon Sep 17 00:00:00 2001 From: lasuomela Date: Fri, 7 Jan 2022 15:04:04 +0200 Subject: [PATCH 19/19] Update openscenario_support.md --- Docs/openscenario_support.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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") |