class BallFound(py_trees.behaviour.Behaviour): def __init__(self, name="BallFound"): super(BallFound, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self.blackboard = Blackboard() def setup(self, unused_timeout=15): self.logger.debug("%s.setup()" % (self.__class__.__name__)) return True def initialise(self): self.logger.debug("%s.initialise()" % (self.__class__.__name__)) def update(self): self.logger.debug("%s.update()" % (self.__class__.__name__)) robot = self.blackboard.get("robot") ball = self.blackboard.get("ball") th = direction(robot, ball) d = distance(robot, ball) if math.fabs(th) > 1 and d > 10: new_status = py_trees.common.Status.FAILURE else: new_status = py_trees.common.Status.SUCCESS return new_status def terminate(self, new_status): self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
class ApproachBin(py_trees.behaviour.Behaviour): def __init__(self, name="ApproachBin"): super(ApproachBin, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self.blackboard = Blackboard() def setup(self, unused_timeout=15): self.logger.debug("%s.setup()" % (self.__class__.__name__)) return True def initialise(self): self.logger.debug("%s.initialise()" % (self.__class__.__name__)) def update(self): self.logger.debug("%s.update()" % (self.__class__.__name__)) robot = self.blackboard.get("robot") ball = self.blackboard.get("ball") bin = self.blackboard.get("bin") dx = bin.x - robot.x dy = bin.y - robot.y d = math.sqrt(dx*dx+dy*dy) robot.x += int(10/d*dx) robot.y += int(10/d*dy) ball.x += int(10/d*dx) ball.y += int(10/d*dy) new_status = py_trees.common.Status.RUNNING return new_status def terminate(self, new_status): self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
class BinClose(py_trees.behaviour.Behaviour): def __init__(self, name="BinClose"): super(BinClose, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self.blackboard = Blackboard() def setup(self, unused_timeout=15): self.logger.debug("%s.setup()" % (self.__class__.__name__)) return True def initialise(self): self.logger.debug("%s.initialise()" % (self.__class__.__name__)) def update(self): self.logger.debug("%s.update()" % (self.__class__.__name__)) robot = self.blackboard.get("robot") bin = self.blackboard.get("bin") d = distance(robot, bin) if d < 10: new_status = py_trees.common.Status.SUCCESS else: new_status = py_trees.common.Status.FAILURE return new_status def terminate(self, new_status): self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
class MoveUntilNewBiome(Behaviour): def __init__(self, name="MoveUntilNewBiome"): super(MoveUntilNewBiome, self).__init__(name) self.bb = Blackboard() def setup(self, timeout, robot=None, speeds=None): if robot: self.robot = robot if speeds: self.speeds = speeds return True def initialise(self): pass # Move forward until the color of the floor changes enough to be # recognized as a new floor color def update(self): if new_biome(self.robot.get_color(), self.bb.get("color_floor")): if self.status == Status.RUNNING: return Status.SUCCESS else: if self.status != Status.RUNNING: speed_left, speed_right = self.speeds self.robot.move(speed_left, speed_right) return Status.RUNNING return Status.FAILURE # Stops the robot and signals the analyzer to analyze the floor color def terminate(self, new_status): if new_status == Status.SUCCESS: self.bb.set("biome_check", True) self.robot.stop()
def terminate(self, new_status): if new_status == Status.SUCCESS: bb = Blackboard() bb.set( "biome_type", analyze_type(bb.get("biome_type"), self.color_original, self.color_left, self.color_right)) bb.set("biome_check", False)
def update(self): bb = Blackboard() if bb.get("color_floor"): return Status.SUCCESS color = self.robot.get_color() result = bb.set("color_floor", color) if result: return Status.RUNNING else: return Status.FAILURE
def test_static_get_set(): console.banner("Blackboard get/set") print("Set foo: 5") Blackboard.set("foo", 5) print("Unset foo") Blackboard.unset("foo") with nose.tools.assert_raises_regexp(KeyError, "foo"): print(" - Expecting a KeyError") unused_value = Blackboard.get("foo") print("Get bar") with nose.tools.assert_raises_regexp(KeyError, "bar"): print(" - Expecting a KeyError") unused_value = Blackboard.get("bar") print("Set motley: Motley()") Blackboard.set("motley", Motley()) print("Set motley.nested: nooo") Blackboard.set("motley.nested", "nooo") assert (Blackboard.get("motley.nested") == "nooo") print("Get motley.foo") with nose.tools.assert_raises_regexp(KeyError, "nested attributes"): print(" - Expecting a KeyError") unused_value = Blackboard.get("motley.foo") print("Set motley.other: floosie") Blackboard.set("motley.other", "floosie") assert (Blackboard.get("motley.other") == "floosie") print("Get missing") with nose.tools.assert_raises_regexp(KeyError, "missing"): print(" - Expecting a KeyError") unused_value = Blackboard.get("missing")
def test_blackboard_static_names(): console.banner("Test Absolute Names with Static Methods") print("Set 'foo'") Blackboard.set("foo", "foo") print("Get 'foo' [{}]".format(Blackboard.get("foo"))) assert (Blackboard.get("foo") == "foo") assert (Blackboard.get("/foo") == "foo") print("Set '/bar'") Blackboard.set("/bar", "bar") print("Get 'bar' [{}]".format(Blackboard.get("bar"))) assert (Blackboard.get("bar") == "bar") assert (Blackboard.get("/bar") == "bar")
class FindBin(py_trees.behaviour.Behaviour): def __init__(self, name="FindBin"): super(FindBin, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self.blackboard = Blackboard() def setup(self, unused_timeout=15): self.logger.debug("%s.setup()" % (self.__class__.__name__)) return True def initialise(self): self.logger.debug("%s.initialise()" % (self.__class__.__name__)) def update(self): self.logger.debug("%s.update()" % (self.__class__.__name__)) robot = self.blackboard.get("robot") robot.th += 0.5 new_status = py_trees.common.Status.RUNNING return new_status def terminate(self, new_status): self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
def update(self): bb = Blackboard() if bb.get("biome_check") and self.status != Status.RUNNING: self.color_original = self.robot.get_color() return Status.RUNNING elif self.status == Status.RUNNING: if not self.robot.is_running(): if self.turning: if not self.color_left: self.color_left = self.robot.get_color() self.robot.turn_for(30, 0, 2) if not self.color_right: self.color_right = self.robot.get_color() self.robot.turn_for(0, 30, 1) else: self.turning = False return Status.SUCCESS elif not self.left_done: self.robot.turn_for(0, 30, 1) self.turning = True return Status.RUNNING else: return Status.INVALID
class WaypointFollower(AtomicBehavior): """ This is an atomic behavior to follow waypoints indefinitely while maintaining a given speed or if given a waypoint plan, follows the given plan Important parameters: - actor: CARLA actor to execute the behavior - target_speed: Desired speed of the actor in m/s - plan[optional]: Waypoint plan the actor should follow - avoid_collision[optional]: Enable/Disable(=default) collision avoidance A parallel termination behavior has to be used. """ def __init__(self, actor, target_speed=None, plan=None, blackboard_queue_name=None, avoid_collision=False, name="FollowWaypoints"): """ Set up actor and local planner """ super(WaypointFollower, self).__init__(name) self._actor_list = [] self._actor_list.append(actor) self._target_speed = target_speed self._local_planner_list = [] self._plan = plan self._blackboard_queue_name = blackboard_queue_name if blackboard_queue_name is not None: self._queue = Blackboard().get(blackboard_queue_name) self._args_lateral_dict = { 'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.0, 'dt': 0.05 } self._avoid_collision = avoid_collision def initialise(self): """ Delayed one-time initialization """ for actor in self._actor_list: self._apply_local_planner(actor) return True def _apply_local_planner(self, actor): if self._target_speed is None: self._target_speed = CarlaDataProvider.get_velocity(actor) * 3.6 else: self._target_speed = self._target_speed * 3.6 local_planner = LocalPlanner( # pylint: disable=undefined-variable actor, opt_dict={ 'target_speed': self._target_speed, 'lateral_control_dict': self._args_lateral_dict}) if self._plan is not None: local_planner.set_global_plan(self._plan) self._local_planner_list.append(local_planner) def update(self): """ Run local planner, obtain and apply control to actor """ new_status = py_trees.common.Status.RUNNING if self._blackboard_queue_name is not None: while not self._queue.empty(): actor = self._queue.get() if actor is not None and actor not in self._actor_list: self._actor_list.append(actor) self._apply_local_planner(actor) for actor, local_planner in zip(self._actor_list, self._local_planner_list): if actor is not None and actor.is_alive and local_planner is not None: control = local_planner.run_step(debug=False) if self._avoid_collision and detect_lane_obstacle(actor): control.throttle = 0.0 control.brake = 1.0 actor.apply_control(control) return new_status def terminate(self, new_status): """ On termination of this behavior, the throttle, brake and steer should be set back to 0. """ control = carla.VehicleControl() control.throttle = 0.0 control.brake = 0.0 control.steer = 0.0 for actor, local_planner in zip(self._actor_list, self._local_planner_list): if actor is not None and actor.is_alive: actor.apply_control(control) if local_planner is not None: local_planner.reset_vehicle() local_planner = None super(WaypointFollower, self).terminate(new_status)
class TrafficJamChecker(AtomicBehavior): """ Atomic behavior that performs the following actions: 1. Instantiates a set of vehicles managed by a server autopilot 2. Check for possible traffic jams 3. Destroy the NPC actors (in autopilot mode) involved in the traffic jam This behavior stops when blackboard.get('master_scenario_command') == scenarios_stop_request """ SOFT_NUMBER_BLOCKS = 10 # 10 seconds HARD_NUMBER_BLOCKS = 30 # 30 seconds MINIMUM_DISTANCE = 5.0 # meters def __init__(self, debug=False, name="TrafficJamChecker"): super(TrafficJamChecker, self).__init__(name) self.debug = debug self.blackboard = Blackboard() self.world = CarlaDataProvider.get_world() self.map = CarlaDataProvider.get_map() self.list_intersection_waypoints = [] # remove initial collisions during setup list_actors = list(CarlaActorPool.get_actors()) for _, actor in list_actors: if actor.attributes['role_name'] == 'autopilot': if detect_lane_obstacle(actor, margin=0.2): CarlaActorPool.remove_actor_by_id(actor.id) # prepare a table to check for stalled vehicles during the execution of the scenario self.table_blocked_actors = {} current_game_time = GameTime.get_time() for actor_id, actor in CarlaActorPool.get_actors(): if actor.attributes['role_name'] == 'autopilot': actor.set_autopilot(True) self.table_blocked_actors[actor_id] = { 'location': actor.get_location(), 'time': current_game_time } self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def update(self): master_scenario_command = self.blackboard.get( 'master_scenario_command') if master_scenario_command and master_scenario_command == 'scenarios_stop_request': new_status = py_trees.common.Status.SUCCESS return new_status else: new_status = py_trees.common.Status.RUNNING current_game_time = GameTime.get_time() list_actors_to_destroy = [] for actor_id, actor in CarlaActorPool.get_actors(): if actor.attributes['role_name'] == 'autopilot': block_info = self.table_blocked_actors[actor_id] current_location = actor.get_location() distance = current_location.distance(block_info['location']) # if vehicle is moving we reset the current time if distance >= self.MINIMUM_DISTANCE: self.table_blocked_actors[actor_id][ 'location'] = current_location self.table_blocked_actors[actor_id][ 'time'] = current_game_time # if the vehicle is on a trigger box than it should have the time reset if actor.is_at_traffic_light(): self.table_blocked_actors[actor_id][ 'location'] = current_location self.table_blocked_actors[actor_id][ 'time'] = current_game_time if self.debug: self.world.debug.draw_point(current_location, size=1.3, color=carla.Color( 0, 255, 0), life_time=5) # if vehicle has been static for a long time we get rid of it if (current_game_time - self.table_blocked_actors[actor_id]['time'] ) > self.HARD_NUMBER_BLOCKS: list_actors_to_destroy.append(actor_id) if self.debug: self.world.debug.draw_point(current_location, size=1.3, color=carla.Color( 255, 0, 0), life_time=5) # if the vehicle has been static for a short period of time... elif (current_game_time - self.table_blocked_actors[actor_id]['time'] ) > self.SOFT_NUMBER_BLOCKS: # check if this vehicle is at an intersection current_waypoint = self.map.get_waypoint(current_location) # is it blocked at an intersection? Then we need to get rid of it! if current_waypoint.is_intersection: if self.debug: self.world.debug.draw_point(current_location, size=1.3, color=carla.Color( 0, 0, 255), life_time=5) list_actors_to_destroy.append(actor_id) for actor_id in list_actors_to_destroy: CarlaActorPool.remove_actor_by_id(actor_id) self.table_blocked_actors[actor_id] = None return new_status
class TrafficLightManipulator(AtomicBehavior): """ Atomic behavior that manipulates traffic lights around the ego_vehicle This scenario stops when blackboard.get('master_scenario_command') == scenarios_stop_request Important parameters: - ego_vehicle: CARLA actor that controls this behavior This behavior stops when blackboard.get('master_scenario_command') == scenarios_stop_request """ MAX_DISTANCE_TRAFFIC_LIGHT = 15 RANDOM_VALUE_INTERVENTION = 0.4 RED = carla.TrafficLightState.Red GREEN = carla.TrafficLightState.Green INT_CONF_OPP = { 'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': RED, 'opposite': GREEN } INT_CONF_LFT = { 'ego': GREEN, 'ref': GREEN, 'left': GREEN, 'right': RED, 'opposite': RED } INT_CONF_RGT = { 'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': GREEN, 'opposite': RED } INTERSECTION_CONFIGURATIONS = [INT_CONF_OPP, INT_CONF_LFT, INT_CONF_RGT] def __init__(self, ego_vehicle, debug=False, name="TrafficLightManipulator"): super(TrafficLightManipulator, self).__init__(name) self.ego_vehicle = ego_vehicle self.debug = debug self.blackboard = Blackboard() self.target_traffic_light = None self.annotations = None self.reset_annotations = None self.intervention = False self.logger.debug("%s.__init__()" % (self.__class__.__name__)) def update(self): master_scenario_command = self.blackboard.get( 'master_scenario_command') if master_scenario_command and master_scenario_command == 'scenarios_stop_request': new_status = py_trees.common.Status.SUCCESS return new_status else: new_status = py_trees.common.Status.RUNNING # find a suitable target if not self.target_traffic_light: traffic_light = CarlaDataProvider.get_next_traffic_light( self.ego_vehicle, use_cached_location=False) if not traffic_light: # nothing else to do in this iteration... return new_status base_transform = traffic_light.get_transform() area_loc = carla.Location( base_transform.transform( traffic_light.trigger_volume.location)) distance_to_traffic_light = area_loc.distance( self.ego_vehicle.get_location()) if self.debug: print("[{}] distance={}".format(traffic_light.id, distance_to_traffic_light)) if distance_to_traffic_light < self.MAX_DISTANCE_TRAFFIC_LIGHT: self.target_traffic_light = traffic_light self.intervention = random.random( ) > self.RANDOM_VALUE_INTERVENTION if self.intervention: if self.debug: print( "--- We are going to affect the following intersection" ) loc = self.target_traffic_light.get_location() CarlaDataProvider.get_world().debug.draw_point( loc + carla.Location(z=1.0), size=0.5, color=carla.Color(255, 255, 0), life_time=50000) self.annotations = CarlaDataProvider.annotate_trafficlight_in_group( self.target_traffic_light) else: if not self.reset_annotations: if self.intervention: # the light has not been manipulated yet choice = random.choice(self.INTERSECTION_CONFIGURATIONS) self.reset_annotations = CarlaDataProvider.update_light_states( self.target_traffic_light, self.annotations, choice, freeze=True) else: # the traffic light has been manipulated... base_transform = self.target_traffic_light.get_transform() area_loc = carla.Location( base_transform.transform( self.target_traffic_light.trigger_volume.location)) distance_to_traffic_light = area_loc.distance( self.ego_vehicle.get_location()) if self.debug: print("++ distance={}".format(distance_to_traffic_light)) if distance_to_traffic_light > self.MAX_DISTANCE_TRAFFIC_LIGHT: if self.reset_annotations: CarlaDataProvider.reset_lights(self.reset_annotations) self.target_traffic_light = None self.reset_annotations = None self.annotations = None self.intervention = False return new_status
class WaypointFollower(AtomicBehavior): """ This is an atomic behavior to follow waypoints while maintaining a given speed. If no plan is provided, the actor will follow its foward waypoints indefinetely. Otherwise, the behavior will end with SUCCESS upon reaching the end of the plan. If no target velocity is provided, the actor continues with its current velocity. Args: actor (carla.Actor): CARLA actor to execute the behavior. target_speed (float, optional): Desired speed of the actor in m/s. Defaults to None. plan ([carla.Location] or [(carla.Waypoint, carla.agent.navigation.local_planner)], optional): Waypoint plan the actor should follow. Defaults to None. blackboard_queue_name (str, optional): Blackboard variable name, if additional actors should be created on-the-fly. Defaults to None. avoid_collision (bool, optional): Enable/Disable(=default) collision avoidance for vehicles/bikes. Defaults to False. name (str, optional): Name of the behavior. Defaults to "FollowWaypoints". Attributes: actor (carla.Actor): CARLA actor to execute the behavior. name (str, optional): Name of the behavior. _target_speed (float, optional): Desired speed of the actor in m/s. Defaults to None. _plan ([carla.Location] or [(carla.Waypoint, carla.agent.navigation.local_planner)]): Waypoint plan the actor should follow. Defaults to None. _blackboard_queue_name (str): Blackboard variable name, if additional actors should be created on-the-fly. Defaults to None. _avoid_collision (bool): Enable/Disable(=default) collision avoidance for vehicles/bikes. Defaults to False. _actor_dict: Dictonary of all actors, and their corresponding plans (e.g. {actor: plan}). _local_planner_dict: Dictonary of all actors, and their corresponding local planners. Either "Walker" for pedestrians, or a carla.agent.navigation.LocalPlanner for other actors. _args_lateral_dict: Parameters for the PID of the used carla.agent.navigation.LocalPlanner. _unique_id: Unique ID of the behavior based on timestamp in nanoseconds. Note: OpenScenario: The WaypointFollower atomic must be called with an individual name if multiple consecutive WFs. Blackboard variables with lists are used for consecutive WaypointFollower behaviors. Termination of active WaypointFollowers in initialise of AtomicBehavior because any following behavior must terminate the WaypointFollower. """ def __init__(self, actor, target_speed=None, plan=None, blackboard_queue_name=None, avoid_collision=False, name="FollowWaypoints"): """ Set up actor and local planner """ super(WaypointFollower, self).__init__(name, actor) self._actor_dict = {} self._actor_dict[actor] = None self._target_speed = target_speed self._local_planner_dict = {} self._local_planner_dict[actor] = None self._plan = plan self._blackboard_queue_name = blackboard_queue_name if blackboard_queue_name is not None: self._queue = Blackboard().get(blackboard_queue_name) self._args_lateral_dict = {'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.0, 'dt': 0.05} self._avoid_collision = avoid_collision self._unique_id = 0 def initialise(self): """ Delayed one-time initialization Checks if another WaypointFollower behavior is already running for this actor. If this is the case, a termination signal is sent to the running behavior. """ super(WaypointFollower, self).initialise() self._unique_id = int(round(time.time() * 1e9)) try: # check whether WF for this actor is already running and add new WF to running_WF list check_attr = operator.attrgetter("running_WF_actor_{}".format(self._actor.id)) running = check_attr(py_trees.blackboard.Blackboard()) active_wf = copy.copy(running) active_wf.append(self._unique_id) py_trees.blackboard.Blackboard().set( "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True) except AttributeError: # no WF is active for this actor py_trees.blackboard.Blackboard().set("terminate_WF_actor_{}".format(self._actor.id), [], overwrite=True) py_trees.blackboard.Blackboard().set( "running_WF_actor_{}".format(self._actor.id), [self._unique_id], overwrite=True) for actor in self._actor_dict: self._apply_local_planner(actor) return True def _apply_local_planner(self, actor): """ Convert the plan into locations for walkers (pedestrians), or to a waypoint list for other actors. For non-walkers, activate the carla.agent.navigation.LocalPlanner module. """ if self._target_speed is None: self._target_speed = CarlaDataProvider.get_velocity(actor) else: self._target_speed = self._target_speed if isinstance(actor, carla.Walker): self._local_planner_dict[actor] = "Walker" if self._plan is not None: if isinstance(self._plan[0], carla.Location): self._actor_dict[actor] = self._plan else: self._actor_dict[actor] = [element[0].transform.location for element in self._plan] else: local_planner = LocalPlanner( # pylint: disable=undefined-variable actor, opt_dict={ 'target_speed': self._target_speed * 3.6, 'lateral_control_dict': self._args_lateral_dict}) if self._plan is not None: if isinstance(self._plan[0], carla.Location): plan = [] for location in self._plan: waypoint = CarlaDataProvider.get_map().get_waypoint(location, project_to_road=True, lane_type=carla.LaneType.Any) plan.append((waypoint, RoadOption.LANEFOLLOW)) local_planner.set_global_plan(plan) else: local_planner.set_global_plan(self._plan) self._local_planner_dict[actor] = local_planner self._actor_dict[actor] = self._plan def update(self): """ Compute next control step for the given waypoint plan, obtain and apply control to actor """ new_status = py_trees.common.Status.RUNNING check_term = operator.attrgetter("terminate_WF_actor_{}".format(self._actor.id)) terminate_wf = check_term(py_trees.blackboard.Blackboard()) check_run = operator.attrgetter("running_WF_actor_{}".format(self._actor.id)) active_wf = check_run(py_trees.blackboard.Blackboard()) # Termination of WF if the WFs unique_id is listed in terminate_wf # only one WF should be active, therefore all previous WF have to be terminated if self._unique_id in terminate_wf: terminate_wf.remove(self._unique_id) if self._unique_id in active_wf: active_wf.remove(self._unique_id) py_trees.blackboard.Blackboard().set( "terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True) py_trees.blackboard.Blackboard().set( "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True) new_status = py_trees.common.Status.SUCCESS return new_status if self._blackboard_queue_name is not None: while not self._queue.empty(): actor = self._queue.get() if actor is not None and actor not in self._actor_dict: self._apply_local_planner(actor) success = True for actor in self._local_planner_dict: local_planner = self._local_planner_dict[actor] if actor else None if actor is not None and actor.is_alive and local_planner is not None: # Check if the actor is a vehicle/bike if not isinstance(actor, carla.Walker): control = local_planner.run_step(debug=False) if self._avoid_collision and detect_lane_obstacle(actor): control.throttle = 0.0 control.brake = 1.0 actor.apply_control(control) # Check if the actor reached the end of the plan # @TODO replace access to private _waypoints_queue with public getter if local_planner._waypoints_queue: # pylint: disable=protected-access success = False # If the actor is a pedestrian, we have to use the WalkerAIController # The walker is sent to the next waypoint in its plan else: actor_location = CarlaDataProvider.get_location(actor) success = False if self._actor_dict[actor]: location = self._actor_dict[actor][0] direction = location - actor_location direction_norm = math.sqrt(direction.x**2 + direction.y**2) control = actor.get_control() control.speed = self._target_speed control.direction = direction / direction_norm actor.apply_control(control) if direction_norm < 1.0: self._actor_dict[actor] = self._actor_dict[actor][1:] if self._actor_dict[actor] is None: success = True else: control = actor.get_control() control.speed = self._target_speed control.direction = CarlaDataProvider.get_transform(actor).rotation.get_forward_vector() actor.apply_control(control) if success: new_status = py_trees.common.Status.SUCCESS return new_status def terminate(self, new_status): """ On termination of this behavior, the controls should be set back to 0. """ for actor in self._local_planner_dict: if actor is not None and actor.is_alive: control, _ = get_actor_control(actor) actor.apply_control(control) local_planner = self._local_planner_dict[actor] if local_planner is not None and local_planner != "Walker": local_planner.reset_vehicle() local_planner = None self._local_planner_dict = {} self._actor_dict = {} super(WaypointFollower, self).terminate(new_status)