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 79baabb41..b27f61fa2 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) @@ -1149,13 +1149,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')