Extended OSC Controllers [DO_NOT_MERGE]

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)
 

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):

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

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

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
@@ -282,7 +288,7 @@ 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:
             self._actor.set_light_state(carla.VehicleLightState.Brake)
             if self._max_deceleration is not None:
                 target_speed = max(target_speed, current_speed - (current_time -

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):
 
     """

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={})