From 208882dfd9a4af437fa9182f9d865dc7e7516ddc Mon Sep 17 00:00:00 2001 From: Fabian Oboril Date: Tue, 22 Dec 2020 15:37:47 +0100 Subject: [PATCH] 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 | 20 +++---- 5 files changed, 119 insertions(+), 12 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 8bf52e106..42cacb578 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) @@ -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')