From 1fc508e8e0e78ec1c680097ba726ebdbab8bc5b9 Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Tue, 20 Apr 2021 10:28:28 +0200 Subject: [PATCH 1/3] Fixed OSC parser issue on Nonetype actor Resolved exception "AttributeError: 'NoneType' object has no attribute 'attributes'" in OpenScenarioParser. Change-Id: Icbe813145276a4b5133a604c67dc4762bc94317d --- srunner/tools/openscenario_parser.py | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 7aa355531..a45ad2a2c 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright (c) 2019-2020 Intel Corporation +# Copyright (c) 2019-2021 Intel Corporation # # This work is licensed under the terms of the MIT license. # For a copy, see . @@ -1004,7 +1004,8 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): continuous = relative_speed.attrib.get('continuous') for traffic_actor in actor_list: - if 'role_name' in traffic_actor.attributes and traffic_actor.attributes['role_name'] == obj: + if (traffic_actor is not None and 'role_name' in traffic_actor.attributes and + traffic_actor.attributes['role_name'] == obj): obj_actor = traffic_actor atomic = ChangeActorTargetSpeed(actor, @@ -1062,14 +1063,14 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): relative_offset = float(relative_target_offset.attrib.get('value', 0)) relative_actor = None + relative_actor_name = relative_target_offset.attrib.get('entityRef', None) for _actor in actor_list: - if relative_target_offset.attrib.get('entityRef', None) == _actor.attributes['role_name']: - relative_actor = _actor - break - + 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_target_offset.attrib.get('entityRef', None))) + raise AttributeError("Cannot find actor '{}' for condition".format(relative_actor_name)) atomic = ChangeActorLaneOffset(actor, relative_offset, relative_actor, continuous=continuous, name=maneuver_name) @@ -1085,9 +1086,10 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): master_actor = None for actor_ins in actor_list: - if sync_action.attrib.get('masterEntityRef', None) == actor_ins.attributes['role_name']: - master_actor = actor_ins - break + if actor_ins is not None and 'role_name' in actor_ins.attributes: + if sync_action.attrib.get('masterEntityRef', None) == actor_ins.attributes['role_name']: + master_actor = actor_ins + break if master_actor is None: raise AttributeError("Cannot find actor '{}' for condition".format( From 732e651e98a9db281d8fc157fa7106ccd6dfeb35 Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Fri, 16 Oct 2020 18:24:32 +0200 Subject: [PATCH 2/3] OSC Controllers --- .../openscenario_configuration.py | 30 +++++++++------- .../scenarioconfigs/scenario_configuration.py | 3 +- .../actorcontrols/simple_vehicle_control.py | 3 +- srunner/scenarios/open_scenario.py | 11 +++++- srunner/tools/openscenario_parser.py | 34 ++++++++++++++++++- 5 files changed, 65 insertions(+), 16 deletions(-) diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index f7a3df4d6..5afd7da3c 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -242,26 +242,30 @@ def _set_actor_information(self): value = prop.get('value') args[key] = value + actor_controller = [] + for controller in obj.iter("ObjectController"): + actor_controller.append(controller) + for catalog_reference in obj.iter("CatalogReference"): entry = OpenScenarioParser.get_catalog_entry(self.catalogs, catalog_reference) if entry.tag == "Vehicle": - self._extract_vehicle_information(entry, rolename, entry, args) + self._extract_vehicle_information(entry, rolename, entry, args, actor_controller) elif entry.tag == "Pedestrian": - self._extract_pedestrian_information(entry, rolename, entry, args) + self._extract_pedestrian_information(entry, rolename, entry, args, actor_controller) elif entry.tag == "MiscObject": - self._extract_misc_information(entry, rolename, entry, args) + self._extract_misc_information(entry, rolename, entry, args, actor_controller) else: self.logger.debug( " A CatalogReference specifies a reference that is not an Entity. Skipping...") for vehicle in obj.iter("Vehicle"): - self._extract_vehicle_information(obj, rolename, vehicle, args) + self._extract_vehicle_information(obj, rolename, vehicle, args, actor_controller) for pedestrian in obj.iter("Pedestrian"): - self._extract_pedestrian_information(obj, rolename, pedestrian, args) + self._extract_pedestrian_information(obj, rolename, pedestrian, args, actor_controller) for misc in obj.iter("MiscObject"): - self._extract_misc_information(obj, rolename, misc, args) + self._extract_misc_information(obj, rolename, misc, args, actor_controller) # Set transform for all actors # This has to be done in a multi-stage loop to resolve relative position settings @@ -285,7 +289,7 @@ def _set_actor_information(self): if actor.transform is None: all_actor_transforms_set = False - def _extract_vehicle_information(self, obj, rolename, vehicle, args): + def _extract_vehicle_information(self, obj, rolename, vehicle, args, actor_controller): """ Helper function to _set_actor_information for getting vehicle information from XML tree """ @@ -301,25 +305,26 @@ def _extract_vehicle_information(self, obj, rolename, vehicle, args): speed = self._get_actor_speed(rolename) new_actor = ActorConfigurationData( - model, None, rolename, speed, color=color, category=category, args=args) + model, None, rolename, speed, color=color, category=category, args=args, controller=actor_controller) if ego_vehicle: self.ego_vehicles.append(new_actor) else: self.other_actors.append(new_actor) - def _extract_pedestrian_information(self, obj, rolename, pedestrian, args): + def _extract_pedestrian_information(self, obj, rolename, pedestrian, args, actor_controller): """ Helper function to _set_actor_information for getting pedestrian information from XML tree """ model = pedestrian.attrib.get('model', "walker.*") speed = self._get_actor_speed(rolename) - new_actor = ActorConfigurationData(model, None, rolename, speed, category="pedestrian", args=args) + new_actor = ActorConfigurationData(model, None, rolename, speed, + category="pedestrian", args=args, controller=actor_controller) self.other_actors.append(new_actor) - def _extract_misc_information(self, obj, rolename, misc, args): + def _extract_misc_information(self, obj, rolename, misc, args, actor_controller): """ Helper function to _set_actor_information for getting vehicle information from XML tree """ @@ -330,7 +335,8 @@ def _extract_misc_information(self, obj, rolename, misc, args): model = "static.prop.chainbarrier" else: model = misc.attrib.get('name') - new_actor = ActorConfigurationData(model, None, rolename, category="misc", args=args) + new_actor = ActorConfigurationData(model, None, rolename, category="misc", + args=args, controller=actor_controller) self.other_actors.append(new_actor) diff --git a/srunner/scenarioconfigs/scenario_configuration.py b/srunner/scenarioconfigs/scenario_configuration.py index 388a7d2cf..26a2f4149 100644 --- a/srunner/scenarioconfigs/scenario_configuration.py +++ b/srunner/scenarioconfigs/scenario_configuration.py @@ -19,7 +19,7 @@ class ActorConfigurationData(object): """ def __init__(self, model, transform, rolename='other', speed=0, autopilot=False, - random=False, color=None, category="car", args=None): + random=False, color=None, category="car", args=None, controller=None): self.model = model self.rolename = rolename self.transform = transform @@ -29,6 +29,7 @@ def __init__(self, model, transform, rolename='other', speed=0, autopilot=False, self.color = color self.category = category self.args = args + self.controller = controller @staticmethod def parse_from_node(node, rolename): diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 473e78cd1..222fbd0b2 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -282,7 +282,8 @@ def _set_new_velocity(self, next_location): self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): target_speed = 0 - if target_speed < current_speed: + if target_speed < current_speed and math.fabs(target_speed - current_speed) > 0.01: + print(target_speed, current_speed) self._actor.set_light_state(carla.VehicleLightState.Brake) if self._max_deceleration is not None: target_speed = max(target_speed, current_speed - (current_time - diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index ec481e8d1..0c24008b4 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -225,12 +225,21 @@ def _create_init_behavior(self): if private.attrib.get('entityRef', None) == actor.rolename: for private_action in private.iter("PrivateAction"): for controller_action in private_action.iter('ControllerAction'): - module, args = OpenScenarioParser.get_controller( + module, args = OpenScenarioParser.get_controller_from_action( controller_action, self.config.catalogs) controller_atomic = ChangeActorControl( carla_actor, control_py_module=module, args=args, scenario_file_path=os.path.dirname(self.config.filename)) + if actor.controller is not None: + if controller_atomic is not None: + print("Warning: A controller was already assigned to actor {}".format(actor.model)) + else: + for controller in actor.controller: + print(actor.rolename) + module, args = OpenScenarioParser.get_controller(controller, self.config.catalogs) + controller_atomic = ChangeActorControl(carla_actor, control_py_module=module, args=args) + if controller_atomic is None: controller_atomic = ChangeActorControl(carla_actor, control_py_module=None, args={}) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index a45ad2a2c..79baabb41 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -365,7 +365,7 @@ def get_weather_from_env_action(xml_tree, catalogs): return Weather(carla_weather, dtime, weather_animation) @staticmethod - def get_controller(xml_tree, catalogs): + def get_controller_from_action(xml_tree, catalogs): """ Extract the object controller from the OSC XML or a catalog @@ -403,6 +403,38 @@ def get_controller(xml_tree, catalogs): return module, args + @staticmethod + def get_controller(xml_tree, catalogs): + """ + Extract the object controller from the OSC XML or a catalog + + Args: + xml_tree: Containing the controller information, + or the reference to the catalog it is defined in. + catalogs: XML Catalogs that could contain the EnvironmentAction + + returns: + module: Python module containing the controller implementation + args: Dictonary with (key, value) parameters for the controller + """ + + properties = None + if xml_tree.find('Controller') is not None: + properties = xml_tree.find('Controller').find('Properties') + elif xml_tree.find("CatalogReference") is not None: + catalog_reference = xml_tree.find("CatalogReference") + properties = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference).find('Properties') + + module = None + args = {} + for prop in properties: + if prop.attrib.get('name') == "module": + module = prop.attrib.get('value') + else: + args[prop.attrib.get('name')] = prop.attrib.get('value') + + return module, args + @staticmethod def get_route(xml_tree, catalogs): """ From 53d41b876eedfbdeb8371deec2ae9fdf9d9cdb24 Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Tue, 22 Dec 2020 15:37:47 +0100 Subject: [PATCH 3/3] Rework ActivateControllerAction OSC action Change-Id: I91034ba6728af16582bbfc4c992f2186b3ea1908 --- .../actorcontrols/actor_control.py | 18 ++++++ .../actorcontrols/basic_control.py | 26 ++++++++ .../actorcontrols/simple_vehicle_control.py | 7 ++- .../scenarioatomics/atomic_behaviors.py | 60 ++++++++++++++++++- srunner/tools/openscenario_parser.py | 14 +++-- 5 files changed, 119 insertions(+), 6 deletions(-) diff --git a/srunner/scenariomanager/actorcontrols/actor_control.py b/srunner/scenariomanager/actorcontrols/actor_control.py index 333618d6c..b5ebed955 100644 --- a/srunner/scenariomanager/actorcontrols/actor_control.py +++ b/srunner/scenariomanager/actorcontrols/actor_control.py @@ -173,6 +173,24 @@ def set_init_speed(self): """ self.control_instance.set_init_speed() + def change_lon_control(self, enable): + """ + Enable/Disable longitudinal control component of actor controller + + Args: + enable (boolean): Enable/Disable signal + """ + self.control_instance.change_lon_control(enable) + + def change_lat_control(self, enable): + """ + Enable/Disable lateral control component of actor controller + + Args: + enable (boolean): Enable/Disable signal + """ + self.control_instance.change_lat_control(enable) + def run_step(self): """ Execute on tick of the controller's control loop diff --git a/srunner/scenariomanager/actorcontrols/basic_control.py b/srunner/scenariomanager/actorcontrols/basic_control.py index fa59fa51c..0bb965afe 100644 --- a/srunner/scenariomanager/actorcontrols/basic_control.py +++ b/srunner/scenariomanager/actorcontrols/basic_control.py @@ -37,6 +37,12 @@ class BasicControl(object): Defaults to False. _reached_goal (boolean): Defaults to False. + _use_lon_control (boolean): + Use longitudinal component of controller + Defaults to True + _use_lat_control (boolean): + Use lateral component of controller + Defaults to True """ _actor = None @@ -47,6 +53,8 @@ class BasicControl(object): _target_speed = 0 _reached_goal = False _init_speed = False + _use_lon_control = True + _use_lat_control = True def __init__(self, actor): """ @@ -90,6 +98,24 @@ def set_init_speed(self): """ self._init_speed = True + def change_lon_control(self, enable): + """ + Enable/Disable longitudinal control component + + Args: + enable (boolean): Enable/Disable signal + """ + self._use_lon_control = enable + + def change_lat_control(self, enable): + """ + Enable/Disable lateral control component + + Args: + enable (boolean): Enable/Disable signal + """ + self._use_lat_control = enable + def check_reached_waypoint_goal(self): """ Check if the actor reached the end of the waypoint list diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 222fbd0b2..706bc38e1 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -70,6 +70,12 @@ class SimpleVehicleControl(BasicControl): Defaults to False. _proximity_threshold (float): Distance in front of actor in which obstacles are considered Defaults to infinity. + _consider_trafficlights (boolean): Enable/Disable consideration of red traffic lights + Defaults to False. + _max_deceleration (float): Deceleration value of the vehicle when braking + Defaults to None (infinity). + _max_acceleration (float): Acceleration value of the vehicle when accelerating + Defaults to None (infinity). _cv_image (CV Image): Contains the OpenCV image, in case a debug camera is attached to the actor Defaults to None. _camera (sensor.camera.rgb): Debug camera attached to actor @@ -283,7 +289,6 @@ def _set_new_velocity(self, next_location): target_speed = 0 if target_speed < current_speed and math.fabs(target_speed - current_speed) > 0.01: - print(target_speed, current_speed) self._actor.set_light_state(carla.VehicleLightState.Brake) if self._max_deceleration is not None: target_speed = max(target_speed, current_speed - (current_time - diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index bfc444e62..7f974d9d0 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -278,7 +278,7 @@ class ChangeActorControl(AtomicBehavior): Atomic to change the longitudinal/lateral control logic for an actor. The (actor, controller) pair is stored inside the Blackboard. - The behavior immediately terminates with SUCCESS after the controller. + The behavior immediately terminates with SUCCESS after the controller was changed. Args: actor (carla.Actor): Actor that should be controlled by the controller. @@ -329,6 +329,64 @@ def update(self): return py_trees.common.Status.SUCCESS +class DeActivateActorControlComponents(AtomicBehavior): + + """ + Atomic to enable/disable the longitudinal/lateral control component of an actor controller. + The (actor, controller) pair is retrieved from the Blackboard. + + The behavior immediately terminates with SUCCESS. + + Args: + actor (carla.Actor): Actor that should be controlled by the controller. + control_py_module (string): Name of the python module containing the implementation + of the controller. + args (dictionary): Additional arguments for the controller. + scenario_file_path (string): Additional path to controller implementation. + name (string): Name of the behavior. + Defaults to 'ChangeActorControl'. + + Attributes: + _actor_control (ActorControl): Instance of the actor control. + """ + + def __init__(self, actor, lon_control=None, lat_control=None, name="ChangeActorControl"): + """ + Setup actor controller. + """ + super(DeActivateActorControlComponents, self).__init__(name, actor) + + self._lon_control = lon_control + self._lat_control = lat_control + + def update(self): + """ + Write (actor, controler) pair to Blackboard, or update the controller + if actor already exists as a key. + + returns: + py_trees.common.Status.SUCCESS + """ + + actor_dict = {} + + try: + check_actors = operator.attrgetter("ActorsWithController") + actor_dict = check_actors(py_trees.blackboard.Blackboard()) + except AttributeError: + pass + + if self._actor.id in actor_dict: + if self._lon_control is not None: + actor_dict[self._actor.id].change_lon_control(self._lon_control) + if self._lat_control is not None: + actor_dict[self._actor.id].change_lat_control(self._lat_control) + else: + return py_trees.common.Status.FAILURE + + return py_trees.common.Status.SUCCESS + + class UpdateAllActorControls(AtomicBehavior): """ diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 79baabb41..b27f61fa2 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -26,12 +26,12 @@ ActorTransformSetterToOSCPosition, RunScript, ChangeWeather, - ChangeAutoPilot, ChangeRoadFriction, ChangeActorTargetSpeed, ChangeActorControl, ChangeActorWaypoints, ChangeActorLateralMotion, + DeActivateActorControlComponents, ChangeActorLaneOffset, SyncArrivalOSC, Idle) @@ -1149,13 +1149,19 @@ 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')) - atomic = ChangeAutoPilot(actor, activate, name=maneuver_name) + lon_control = None + lat_control = None + if 'longitudinal' in private_action.attrib.keys(): + lon_control = strtobool(private_action.attrib.get('longitudinal')) + if 'lateral' in private_action.attrib.keys(): + lat_control = strtobool(private_action.attrib.get('lateral')) + atomic = DeActivateActorControlComponents(actor, lon_control, lat_control, 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, - scenario_file_path=OpenScenarioParser.osc_filepath) + scenario_file_path=OpenScenarioParser.osc_filepath, + name=maneuver_name) elif private_action.find('TeleportAction') is not None: teleport_action = private_action.find('TeleportAction') position = teleport_action.find('Position')