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):
new_status = py_trees.common.Status.RUNNING
if self._actor:
CarlaActorPool.remove_actor_by_id(self._actor.id)
self._actor = None
new_status = py_trees.common.Status.SUCCESS
return new_status
def remove_all_actors(self):
"""
Remove all actors
"""
for i, _ in enumerate(self.other_actors):
if self.other_actors[i] is not None:
if CarlaActorPool.actor_id_exists(self.other_actors[i].id):
CarlaActorPool.remove_actor_by_id(self.other_actors[i].id)
self.other_actors[i] = None
self.other_actors = []
def remove_all_actors(self):
"""
Remove all actors
"""
for i, _ in enumerate(self.other_actors):
if self.other_actors[i] is not None:
# print("removed other actor position: {}, {}, {}".format(self.other_actors[i].get_location().x,
# self.other_actors[i].get_location().y,
# self.other_actors[i].get_location().z))
if CarlaActorPool.actor_id_exists(self.other_actors[i].id):
CarlaActorPool.remove_actor_by_id(self.other_actors[i].id)
self.other_actors[i] = None
self.other_actors = []
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
