Rework ActivateControllerAction OSC action

Author: fabianoboril

Docs/CHANGELOG.md

@@ -13,11 +13,18 @@
 ### :rocket: New Features
 * Added a sensor barrier for the agents to ensure that the simulation waits for them to render their data.
 * Added an option to produce a machine-readable JSON version of the scenario report.
-* Added a static obstacle evasion OpenSCENARIO scenario
-* Added support for OSC Routing options
-* Added support for OSC SynchronizeAction
-* Added support to place OSC controller implementation alongside the OSC scenario
 * Updated *GameTime.restart()* at *srunner/scenariomanager/timer.py* to also reset the frame number
+* OpenSCENARIO Support:
+  * Added a static obstacle evasion OpenSCENARIO scenario
+  * Added support for OSC Routing options
+  * Added support for OSC SynchronizeAction
+  * Added support to place OSC controller implementation alongside the OSC scenario
+  * Extended SimpleVehicleController
+  * Added controller using CARLA's autopilot (in replacement for ActivateControllerAction)
+  * Updated ActivateControllerAction to its specified behavior according to OSC 1.0
+  * Added support for storyboards with multiple stories
+  * Added support for ObjectControllers. Note that the controller has to be implemented in Python,
+    or be one of the provided controllers.
 ### :bug: Bug Fixes
 * Fixed metrics-manager.py failing to run with port argument
 * Fixed exception when using OSC scenarios without EnvironmentAction inside Storyboard-Init

Docs/openscenario_support.md

@@ -197,7 +197,7 @@ contains of submodules, which are not listed, the support status applies to all
 <td><code>ActivateControllerAction</code></td>
 <td>&#10060;</td>
 <td>&#9989;</td>
-<td>Can be used to activate/deactive the CARLA autopilot.</td>
+<td></td>
 <tr>
 <td><code>ControllerAction</code></td>
 <td>&#9989;</td>

srunner/scenariomanager/actorcontrols/actor_control.py

@@ -150,6 +150,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
@@ -45,6 +51,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):
         """
@@ -78,6 +86,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

@@ -55,6 +55,8 @@ class SimpleVehicleControl(BasicControl):
                                         (consider_trafficlights, true/false) - Enable consideration of traffic lights
                                         (max_deceleration, float)            - Use a reasonable deceleration value for
                                                                                this vehicle
+                                        (max_acceleration, float)            - Use a reasonable acceleration value for
+                                                                               this vehicle
                                         (attach_camera, true/false)          - Attach OpenCV display to actor
                                                                                (useful for debugging)
 
@@ -69,6 +71,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
@@ -281,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 -

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

@@ -258,12 +258,14 @@ def _create_behavior(self):
         Basic behavior do nothing, i.e. Idle
         """
 
-        stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="OSCStories")
+        stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
+                                                        name="OSCStories")
         joint_actor_list = self.other_actors + self.ego_vehicles + [None]
 
         for story in self.config.stories:
             story_name = story.get("name")
-            story_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=story_name)
+            story_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
+                                                          name=story_name)
             for act in story.iter("Act"):
 
                 act_sequence = py_trees.composites.Sequence(
@@ -289,7 +291,8 @@ def _create_behavior(self):
                             for entity in actor.iter("EntityRef"):
                                 entity_name = entity.attrib.get('entityRef', None)
                                 for k, _ in enumerate(joint_actor_list):
-                                    if joint_actor_list[k] and entity_name == joint_actor_list[k].attributes['role_name']:
+                                    if (joint_actor_list[k] and
+                                        entity_name == joint_actor_list[k].attributes['role_name']):
                                         actor_ids.append(k)
                                         break
 
@@ -298,10 +301,11 @@ def _create_behavior(self):
                                 sequence.attrib.get('name')))
                             actor_ids.append(len(joint_actor_list) - 1)
 
-                    # Collect catalog reference maneuvers in order to process them at the same time as normal maneuvers
+                    # Collect catalog reference maneuvers to process them at the same time as normal maneuvers
                         catalog_maneuver_list = []
                         for catalog_reference in sequence.iter("CatalogReference"):
-                            catalog_maneuver = OpenScenarioParser.get_catalog_entry(self.config.catalogs, catalog_reference)
+                            catalog_maneuver = OpenScenarioParser.get_catalog_entry(self.config.catalogs,
+                                                                                    catalog_reference)
                             catalog_maneuver_list.append(catalog_maneuver)
                         all_maneuvers = itertools.chain(iter(catalog_maneuver_list), sequence.iter("Maneuver"))
                         single_sequence_iteration = py_trees.composites.Parallel(
@@ -319,7 +323,8 @@ def _create_behavior(self):
                                     if child.tag == "Action":
                                         for actor_id in actor_ids:
                                             maneuver_behavior = OpenScenarioParser.convert_maneuver_to_atomic(
-                                                child, joint_actor_list[actor_id], joint_actor_list, self.config.catalogs)
+                                                child, joint_actor_list[actor_id],
+                                                joint_actor_list, self.config.catalogs)
                                             maneuver_behavior = StoryElementStatusToBlackboard(
                                                 maneuver_behavior, "ACTION", child.attrib.get('name'))
                                             parallel_actions.add_child(
@@ -370,7 +375,8 @@ def _create_behavior(self):
                 start_triggers = act.find("StartTrigger")
                 if list(start_triggers) is not None:
                     for start_condition in start_triggers:
-                        parallel_start_criteria = self._create_condition_container(start_condition, story, "StartConditions")
+                        parallel_start_criteria = self._create_condition_container(start_condition, story,
+                                                                                   "StartConditions")
                         if parallel_start_criteria.children:
                             start_conditions.add_child(parallel_start_criteria)
                 end_triggers = act.find("StopTrigger")

srunner/tools/openscenario_parser.py

@@ -32,6 +32,7 @@
                                                                       ChangeActorControl,
                                                                       ChangeActorWaypoints,
                                                                       ChangeActorLateralMotion,
+                                                                      DeActivateActorControlComponents,
                                                                       SyncArrivalOSC,
                                                                       Idle)
 # pylint: disable=unused-import
@@ -1111,13 +1112,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')