Rework ActivateControllerAction OSC action

fabianoboril · fabianoboril · commit b6ef71dbd950 · 2020-12-22T15:43:13.000+01:00
diff --git a/.pylintrc b/.pylintrc
@@ -1,6 +1,6 @@
 [MESSAGES CONTROL]
 max-line-length=120
-disable=no-self-use,anomalous-backslash-in-string,too-many-arguments,too-few-public-methods,too-many-instance-attributes,redefined-variable-type,unused-argument,bad-continuation,too-many-lines,too-many-branches,locally-disabled,too-many-locals,too-many-statements,duplicate-code,too-many-nested-blocks,fixme
+disable=no-self-use,anomalous-backslash-in-string,too-many-arguments,too-few-public-methods,too-many-instance-attributes,redefined-variable-type,unused-argument,bad-continuation,too-many-lines,too-many-branches,locally-disabled,too-many-locals,too-many-statements,duplicate-code,too-many-nested-blocks,fixme,useless-object-inheritance
 ignored-modules=carla,carla.command,agents.navigation.basic_agent,agents.navigation.roaming_agent,agents.tools.misc,agents.navigation.local_planner,agents.navigation.global_route_planner,agents.navigation.global_route_planner_dao,shutil,carla_msgs,nav_msgs,sensor_msgs,std_msgs,tf,cv_bridge,geometry_msgs,rosgraph_msgs,rospy
 variable-rgx=[a-z0-9_]{1,40}$
 function-rgx=[a-z0-9_]{1,40}$
diff --git a/Docs/CHANGELOG.md b/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
diff --git a/Docs/openscenario_support.md b/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>
diff --git a/srunner/scenariomanager/actorcontrols/actor_control.py b/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
diff --git 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
@@ -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
diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/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
@@ -277,11 +285,10 @@ def _set_new_velocity(self, next_location):
 
         if self._consider_traffic_lights:
             if (self._actor.is_at_traffic_light() and
-                self._actor.get_traffic_light_state() == carla.TrafficLightState.Red):
+                    self._actor.get_traffic_light_state() == carla.TrafficLightState.Red):
                 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
@@ -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/scenarios/open_scenario.py b/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,14 +323,15 @@ 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(
-                                                oneshot_with_check(variable_name=# See note in get_xml_path
-                                                                get_xml_path(story, sequence) + '>' + \
-                                                                get_xml_path(maneuver, child),
-                                                                behaviour=maneuver_behavior))
+                                                oneshot_with_check(variable_name=  # See note in get_xml_path
+                                                                   get_xml_path(story, sequence) + '>' + \
+                                                                   get_xml_path(maneuver, child),
+                                                                   behaviour=maneuver_behavior))
 
                                     if child.tag == "StartTrigger":
                                         # There is always one StartConditions block per Event
@@ -340,14 +345,14 @@ def _create_behavior(self):
                                 event_sequence.add_child(parallel_actions)
                                 maneuver_parallel.add_child(
                                     oneshot_with_check(variable_name=get_xml_path(story, sequence) + '>' +
-                                                    get_xml_path(maneuver, event),  # See get_xml_path
-                                                    behaviour=event_sequence))
+                                                       get_xml_path(maneuver, event),  # See get_xml_path
+                                                       behaviour=event_sequence))
                             maneuver_parallel = StoryElementStatusToBlackboard(
                                 maneuver_parallel, "MANEUVER", maneuver.attrib.get('name'))
                             single_sequence_iteration.add_child(
                                 oneshot_with_check(variable_name=get_xml_path(story, sequence) + '>' +
-                                                maneuver.attrib.get('name'),  # See get_xml_path
-                                                behaviour=maneuver_parallel))
+                                                   maneuver.attrib.get('name'),  # See get_xml_path
+                                                   behaviour=maneuver_parallel))
 
                         # OpenSCENARIO refers to Sequences as Scenes in this instance
                         single_sequence_iteration = StoryElementStatusToBlackboard(
@@ -360,7 +365,7 @@ def _create_behavior(self):
                     if sequence_behavior.children:
                         parallel_sequences.add_child(
                             oneshot_with_check(variable_name=get_xml_path(story, sequence),
-                                            behaviour=sequence_behavior))
+                                               behaviour=sequence_behavior))
 
                 if parallel_sequences.children:
                     parallel_sequences = StoryElementStatusToBlackboard(
@@ -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")
@@ -390,8 +396,8 @@ def _create_behavior(self):
                     story_behavior.add_child(act_sequence)
 
             stories_behavior.add_child(oneshot_with_check(variable_name=get_xml_path(story, story) + '>' +
-                                                story_name,  # See get_xml_path
-                                                behaviour=story_behavior))
+                                                          story_name,  # See get_xml_path
+                                                          behaviour=story_behavior))
 
         # Build behavior tree
         behavior = py_trees.composites.Parallel(
diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py
