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