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