Rework ActivateControllerAction OSC action

Change-Id: I91034ba6728af16582bbfc4c992f2186b3ea1908

Author: fabianoboril

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
@@ -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 -

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/tools/openscenario_parser.py

@@ -26,12 +26,12 @@
                                                                       ActorTransformSetterToOSCPosition,
                                                                       RunScript,
                                                                       ChangeWeather,
-                                                                      ChangeAutoPilot,
                                                                       ChangeRoadFriction,
                                                                       ChangeActorTargetSpeed,
                                                                       ChangeActorControl,
                                                                       ChangeActorWaypoints,
                                                                       ChangeActorLateralMotion,
+                                                                      DeActivateActorControlComponents,
                                                                       ChangeActorLaneOffset,
                                                                       SyncArrivalOSC,
                                                                       Idle)
@@ -1081,31 +1081,25 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs):
                     lat_maneuver = private_action.find('LaneOffsetAction')
                     continuous = strtobool(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 = float(
                             lane_target_offset.find('AbsoluteTargetLaneOffset').attrib.get('value', 0))
                         atomic = ChangeActorLaneOffset(
                             actor, absolute_offset, continuous=continuous, name=maneuver_name)
-
                     elif lane_target_offset.find('RelativeTargetLaneOffset') is not None:
                         relative_target_offset = lane_target_offset.find('RelativeTargetLaneOffset')
                         relative_offset = float(relative_target_offset.attrib.get('value', 0))
-
                         relative_actor = None
                         for _actor in actor_list:
                             if relative_target_offset.attrib.get('entityRef', None) == _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)))
-
                         atomic = ChangeActorLaneOffset(actor, relative_offset, relative_actor,
                                                        continuous=continuous, name=maneuver_name)
-
                     else:
                         raise AttributeError("Unknown target offset")
                 else:
@@ -1147,13 +1141,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')