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)
