def __init__(self, actor, args=None):
super(SimpleVehicleControl, self).__init__(actor)
self._generated_waypoint_list = []
self._last_update = None
self._consider_traffic_lights = False
self._consider_obstacles = False
self._proximity_threshold = float('inf')
self._max_deceleration = None
self._max_acceleration = None
self._obstacle_sensor = None
self._obstacle_distance = float('inf')
self._obstacle_actor = None
self._visualizer = None
if args and 'consider_obstacles' in args and strtobool(
args['consider_obstacles']):
self._consider_obstacles = strtobool(args['consider_obstacles'])
bp = CarlaDataProvider.get_world().get_blueprint_library().find(
'sensor.other.obstacle')
bp.set_attribute('distance', '250')
if args and 'proximity_threshold' in args:
self._proximity_threshold = float(args['proximity_threshold'])
bp.set_attribute(
'distance',
str(max(float(args['proximity_threshold']), 250)))
bp.set_attribute('hit_radius', '1')
bp.set_attribute('only_dynamics', 'True')
self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor(
bp,
carla.Transform(
carla.Location(x=self._actor.bounding_box.extent.x,
z=1.0)),
attach_to=self._actor)
self._obstacle_sensor.listen(
lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda
if args and 'consider_trafficlights' in args and strtobool(
args['consider_trafficlights']):
self._consider_traffic_lights = strtobool(
args['consider_trafficlights'])
if args and 'max_deceleration' in args:
self._max_deceleration = float(args['max_deceleration'])
if args and 'max_acceleration' in args:
self._max_acceleration = float(args['max_acceleration'])
if args and 'attach_camera' in args and strtobool(
args['attach_camera']):
self._visualizer = Visualizer(self._actor)
def _reset(self):
"""
Reset all parameters
"""
self._running = False
self._timestamp_last_run = 0.0
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
if self._callback_id:
CarlaDataProvider.get_world().remove_on_tick(self._callback_id)
self._callback_id = None
GameTime.restart()
def _tick_scenario(self, timestamp):
"""
Run next tick of scenario and the agent and tick the world.
"""
if self._timestamp_last_run < timestamp.elapsed_seconds and self._running:
self._timestamp_last_run = timestamp.elapsed_seconds
self._watchdog.update()
# Update game time and actor information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
self._watchdog.pause()
try:
self._agent_watchdog.resume()
self._agent_watchdog.update()
ego_action = self._agent()
self._agent_watchdog.pause()
# Special exception inside the agent that isn't caused by the agent
except SensorReceivedNoData as e:
raise RuntimeError(e)
except Exception as e:
raise AgentError(e)
self._watchdog.resume()
self.ego_vehicles[0].apply_control(ego_action)
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode:
print("\n")
py_trees.display.print_ascii_tree(self.scenario_tree,
show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
spectator = CarlaDataProvider.get_world().get_spectator()
ego_trans = self.ego_vehicles[0].get_transform()
spectator.set_transform(
carla.Transform(ego_trans.location + carla.Location(z=50),
carla.Rotation(pitch=-90)))
if self._running and self.get_running_status():
CarlaDataProvider.get_world().tick(self._timeout)
def _tick_scenario(self, timestamp):
"""
Run next tick of scenario
This function is a callback for world.on_tick()
Important:
- It has to be ensured that the scenario has not yet completed/failed
and that the time moved forward.
- A thread lock should be used to avoid that the scenario tick is performed
multiple times in parallel.
"""
if self._timestamp_last_run < timestamp.elapsed_seconds:
self._timestamp_last_run = timestamp.elapsed_seconds
self._watchdog.update()
# Update game time and actor information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
if self._agent is not None:
ego_action = self._agent()
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode > 1:
print("\n")
py_trees.display.print_ascii_tree(self.scenario_tree,
show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
if self._challenge_mode:
spectator = CarlaDataProvider.get_world().get_spectator()
ego_trans = self.ego_vehicles[0].get_transform()
spectator.set_transform(
carla.Transform(ego_trans.location + carla.Location(z=50),
carla.Rotation(pitch=-90)))
if self._agent is not None:
self.ego_vehicles[0].apply_control(ego_action)
if self._agent and self._running and self._watchdog.get_status():
CarlaDataProvider.get_world().tick()
def update(self):
"""
Check if new weather settings are available on the blackboard, and if yes fetch these
into the _weather attribute.
Apply the weather settings from _weather to CARLA.
Note:
To minimize CARLA server interactions, the weather is only updated, when the blackboard
is updated, or if the weather animation flag is true. In the latter case, the update
frequency is 1 Hz.
returns:
py_trees.common.Status.RUNNING
"""
weather = None
try:
check_weather = operator.attrgetter("CarlaWeather")
weather = check_weather(py_trees.blackboard.Blackboard())
except AttributeError:
pass
if weather:
self._weather = weather
delattr(py_trees.blackboard.Blackboard(), "CarlaWeather")
CarlaDataProvider.get_world().set_weather(
self._weather.carla_weather)
py_trees.blackboard.Blackboard().set("Datetime",
self._weather.datetime,
overwrite=True)
if self._weather and self._weather.animation:
new_time = GameTime.get_time()
delta_time = new_time - self._current_time
if delta_time > 1:
self._weather.update(delta_time)
self._current_time = new_time
CarlaDataProvider.get_world().set_weather(
self._weather.carla_weather)
py_trees.blackboard.Blackboard().set("Datetime",
self._weather.datetime,
overwrite=True)
return py_trees.common.Status.RUNNING
def _set_route(self, hop_resolution=1.0):
world = CarlaDataProvider.get_world()
dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = CarlaDataProvider._spawn_points
# start, target = random.choice(spawn_points, size=2)
start_idx, target_idx = random.choice(len(spawn_points), size=2)
# DEBUG
# start_idx, target_idx = 57, 87
# start_idx, target_idx = 2, 18
# print (start_idx, target_idx)
start = spawn_points[start_idx]
target = spawn_points[target_idx]
route = grp.trace_route(start.location, target.location)
self.route = [(w.transform, c) for w, c in route]
CarlaDataProvider.set_ego_vehicle_route([(w.transform.location, c)
for w, c in route])
gps_route = location_route_to_gps(self.route, *_get_latlon_ref(world))
self.config.agent.set_global_plan(gps_route, self.route)
self.timeout = self._estimate_route_timeout()
def run_scenario(self):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
print("ScenarioManager: Running scenario {}".format(
self.scenario_tree.name))
self.start_system_time = time.time()
start_game_time = GameTime.get_time()
self._watchdog.start()
self._running = True
while self._running:
timestamp = None
world = CarlaDataProvider.get_world()
if world:
snapshot = world.get_snapshot()
if snapshot:
timestamp = snapshot.timestamp
if timestamp:
self._tick_scenario(timestamp)
self._watchdog.stop()
self.cleanup()
self.end_system_time = time.time()
end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - \
self.start_system_time
self.scenario_duration_game = end_game_time - start_game_time
if self.scenario_tree.status == py_trees.common.Status.FAILURE:
print("ScenarioManager: Terminated due to failure")
def __init__(self,
reference_actor,
actor,
distance,
comparison_operator=operator.lt,
distance_type="cartesianDistance",
freespace=False,
name="TriggerDistanceToVehicle"):
"""
Setup trigger distance
"""
super(InTriggerDistanceToVehicle, self).__init__(name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
self._reference_actor = reference_actor
self._actor = actor
self._distance = distance
self._distance_type = distance_type
self._freespace = freespace
self._comparison_operator = comparison_operator
if distance_type == "longitudinal":
self._global_rp = GlobalRoutePlanner(
CarlaDataProvider.get_world().get_map(), 1.0)
else:
self._global_rp = None
def __init__(self,
traffic_light_id,
state,
name="TrafficLightStateSetter"):
"""
Init
"""
super(TrafficLightStateSetter, self).__init__(name)
self._actor = None
actor_list = CarlaDataProvider.get_world().get_actors()
for actor in actor_list:
if actor.id == int(traffic_light_id):
self._actor = actor
break
new_state = carla.TrafficLightState.Unknown
if state.upper() == "GREEN":
new_state = carla.TrafficLightState.Green
elif state.upper() == "RED":
new_state = carla.TrafficLightState.Red
elif state.upper() == "YELLOW":
new_state = carla.TrafficLightState.Yellow
elif state.upper() == "OFF":
new_state = carla.TrafficLightState.Off
self._new_traffic_light_state = new_state
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
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 run_scenario(self):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
self.start_system_time = time.time()
self.start_game_time = GameTime.get_time()
# Detects if the simulation is down
self._watchdog = Watchdog(self._timeout)
self._watchdog.start()
# Stop the agent from freezing the simulation
self._agent_watchdog = Watchdog(self._timeout)
self._agent_watchdog.start()
self._running = True
while self._running:
timestamp = None
world = CarlaDataProvider.get_world()
if world:
snapshot = world.get_snapshot()
if snapshot:
timestamp = snapshot.timestamp
if timestamp:
self._tick_scenario(timestamp)
def __init__(self,
actor,
other_actor,
time,
side_lane,
comparison_operator=operator.lt,
name="InTimeToArrivalToVehicleSideLane"):
"""
Setup parameters
"""
self._other_actor = other_actor
self._side_lane = side_lane
self._world = CarlaDataProvider.get_world()
self._map = CarlaDataProvider.get_map(self._world)
other_location = other_actor.get_transform().location
other_waypoint = self._map.get_waypoint(other_location)
if self._side_lane == 'right':
other_side_waypoint = other_waypoint.get_left_lane()
elif self._side_lane == 'left':
other_side_waypoint = other_waypoint.get_right_lane()
else:
raise Exception("cut_in_lane must be either 'left' or 'right'")
other_side_location = other_side_waypoint.transform.location
super(InTimeToArrivalToVehicleSideLane,
self).__init__(actor, time, other_side_location,
comparison_operator, name)
self.logger.debug("%s.__init__()" % (self.__class__.__name__))
def get_geometric_linear_intersection(ego_location, other_location):
"""
Obtain a intersection point between two actor's location by using their waypoints (wp)
@return point of intersection of the two vehicles
"""
wp_ego_1 = CarlaDataProvider.get_map().get_waypoint(ego_location)
wp_ego_2 = wp_ego_1.next(1)[0]
ego_1_loc = wp_ego_1.transform.location
ego_2_loc = wp_ego_2.transform.location
wp_other_1 = CarlaDataProvider.get_world().get_map().get_waypoint(
other_location)
wp_other_2 = wp_other_1.next(1)[0]
other_1_loc = wp_other_1.transform.location
other_2_loc = wp_other_2.transform.location
s = np.vstack([(ego_1_loc.x, ego_1_loc.y), (ego_2_loc.x, ego_2_loc.y),
(other_1_loc.x, other_1_loc.y),
(other_2_loc.x, other_2_loc.y)])
h = np.hstack((s, np.ones((4, 1))))
line1 = np.cross(h[0], h[1])
line2 = np.cross(h[2], h[3])
x, y, z = np.cross(line1, line2)
if z == 0:
return None
return carla.Location(x=x / z, y=y / z, z=0)
def run_scenario(self):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
print("ScenarioManager: Running scenario {}".format(
self.scenario_tree.name))
self.start_system_time = time.time()
start_game_time = GameTime.get_time()
self._watchdog.start()
self._running = True
while self._running:
timestamp = None
world = CarlaDataProvider.get_world()
if world:
snapshot = world.get_snapshot()
if snapshot:
timestamp = snapshot.timestamp
if timestamp:
self._tick_scenario(timestamp)
self._watchdog.stop()
self.end_system_time = time.time()
end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - \
self.start_system_time
self.scenario_duration_game = end_game_time - start_game_time
self._console_message()
def get_geometric_linear_intersection(ego_actor, other_actor):
"""
Obtain a intersection point between two actor's location by using their waypoints (wp)
@return point of intersection of the two vehicles
"""
wp_ego_1 = CarlaDataProvider.get_map().get_waypoint(
ego_actor.get_location())
wp_ego_2 = wp_ego_1.next(1)[0]
x_ego_1 = wp_ego_1.transform.location.x
y_ego_1 = wp_ego_1.transform.location.y
x_ego_2 = wp_ego_2.transform.location.x
y_ego_2 = wp_ego_2.transform.location.y
wp_other_1 = CarlaDataProvider.get_world().get_map().get_waypoint(
other_actor.get_location())
wp_other_2 = wp_other_1.next(1)[0]
x_other_1 = wp_other_1.transform.location.x
y_other_1 = wp_other_1.transform.location.y
x_other_2 = wp_other_2.transform.location.x
y_other_2 = wp_other_2.transform.location.y
s = np.vstack([(x_ego_1, y_ego_1), (x_ego_2, y_ego_2),
(x_other_1, y_other_1), (x_other_2, y_other_2)])
h = np.hstack((s, np.ones((4, 1))))
line1 = np.cross(h[0], h[1])
line2 = np.cross(h[2], h[3])
x, y, z = np.cross(line1, line2)
if z == 0:
return (float('inf'), float('inf'))
intersection = carla.Location(x=x / z, y=y / z, z=0)
return intersection
def __init__(self,
actor_type_list,
transform,
threshold,
blackboard_queue_name,
actor_limit=999,
name="ActorSource",
# ======== additional args ========
init_speed=0., # initial speed of spawned vehicle
randomize=False,
):
"""
Setup class members
"""
super(ActorSource, self).__init__(name)
self._world = CarlaDataProvider.get_world()
self._actor_types = actor_type_list
self._spawn_point = transform
self._threshold = threshold + 5. # todo fix this with accurate vehicle length
self._queue = Blackboard().get(blackboard_queue_name)
self._actor_limit = actor_limit
self._last_blocking_actor = None
# ========== additional attributes ==========
self.init_speed = init_speed
self.randomize = randomize
self.distance_gap = None
def run_scenario(self):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
print("ScenarioManager: Running scenario {}".format(
self.scenario_tree.name))
self.start_system_time = time.time()
start_game_time = GameTime.get_time()
self._watchdog.start()
self._running = True
parent_folder = os.environ['SAVE_FOLDER']
string = pathlib.Path(os.environ['ROUTES']).stem
save_path = pathlib.Path(parent_folder) / string
step = 0
# hack: control the time of running
# debug:
s = time.time()
with (save_path / 'measurements_loc.csv').open("a") as f_out:
while self._running:
timestamp = None
world = CarlaDataProvider.get_world()
if world:
snapshot = world.get_snapshot()
if snapshot:
timestamp = snapshot.timestamp
if timestamp:
self._tick_scenario(timestamp)
# addition
if step == 0:
f_out.write('x,y,yaw\n')
# if step % 10 == 0:
loc = self.ego_vehicles[0].get_location()
ori = self.ego_vehicles[0].get_transform().rotation
f_out.write(
str(loc.x) + ',' + str(loc.y) + ',' + str(ori.yaw) + '\n')
step += 1
# hack: control the time of running
# debug:
if time.time() - s > self.episode_max_time:
print('maximum time reaches, end running')
self._running = False
break
self._watchdog.stop()
self.end_system_time = time.time()
end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - \
self.start_system_time
self.scenario_duration_game = end_game_time - start_game_time
self._console_message()
def run_step(self, input_data, timestamp):
"""
Execute one step of navigation.
"""
if not self._agent:
# Search for the ego actor
hero_actor = None
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes[
'role_name'] == 'hero':
hero_actor = actor
break
if not hero_actor:
return carla.VehicleControl()
# Add an agent that follows the route to the ego
self._agent = BasicAgent(hero_actor, 30)
plan = []
prev_wp = None
for transform, _ in self._global_plan_world_coord:
wp = CarlaDataProvider.get_map().get_waypoint(
transform.location)
if prev_wp:
plan.extend(self._agent.trace_route(prev_wp, wp))
prev_wp = wp
self._agent.set_global_plan(plan)
return carla.VehicleControl()
else:
return self._agent.run_step()
def setup_sensors(self, vehicle, debug_mode=False):
"""
Create the sensors defined by the user and attach them to the ego-vehicle
:param vehicle: ego vehicle
:return:
"""
bp_library = CarlaDataProvider.get_world().get_blueprint_library()
for sensor_spec in self._agent.sensors():
# These are the sensors spawned on the carla world
bp = bp_library.find(str(sensor_spec['type']))
if sensor_spec['type'].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(sensor_spec['width']))
bp.set_attribute('image_size_y', str(sensor_spec['height']))
bp.set_attribute('fov', str(sensor_spec['fov']))
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.lidar'):
bp.set_attribute('range', str(sensor_spec['range']))
bp.set_attribute('rotation_frequency', str(sensor_spec['rotation_frequency']))
bp.set_attribute('channels', str(sensor_spec['channels']))
bp.set_attribute('upper_fov', str(sensor_spec['upper_fov']))
bp.set_attribute('lower_fov', str(sensor_spec['lower_fov']))
bp.set_attribute('points_per_second', str(sensor_spec['points_per_second']))
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.gnss'):
sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation()
# create sensor
sensor_transform = carla.Transform(sensor_location, sensor_rotation)
sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle)
# setup callback
sensor.listen(CallBack(sensor_spec['id'], sensor, self._agent.sensor_interface))
self._sensors_list.append(sensor)
while not self._agent.all_sensors_ready():
if debug_mode:
print(" waiting for one data reading from sensors...")
CarlaDataProvider.get_world().tick()
def stop_scenario(self):
"""
This function triggers a proper termination of a scenario
"""
with self._my_lock:
if self._callback_id:
CarlaDataProvider.get_world().remove_on_tick(self._callback_id)
self._callback_id = None
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
CarlaDataProvider.cleanup()
def _tick_scenario(self, timestamp):
"""
Run next tick of scenario
This function is a callback for world.on_tick()
Important:
- It has to be ensured that the scenario has not yet completed/failed
and that the time moved forward.
- A thread lock should be used to avoid that the scenario tick is performed
multiple times in parallel.
"""
with self._my_lock:
if self._running and self._timestamp_last_run < timestamp.elapsed_seconds:
self._timestamp_last_run = timestamp.elapsed_seconds
if self._debug_mode:
print("\n--------- Tick ---------\n")
# Update game time and actor information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
if self._agent is not None:
ego_action = self._agent()
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode:
print("\n")
py_trees.display.print_ascii_tree(
self.scenario_tree, show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
if self._challenge_mode:
ChallengeStatisticsManager.compute_current_statistics()
if self._agent is not None:
self.ego_vehicles[0].apply_control(ego_action)
if self._agent:
CarlaDataProvider.get_world().tick()
def __init__(self, vehicle, reading_frequency=1.0):
super().__init__(vehicle, reading_frequency)
client = CarlaDataProvider.get_client()
world = CarlaDataProvider.get_world()
town_map = CarlaDataProvider.get_map()
# Initialize map
map_utils.init(client, world, town_map, vehicle)
def __init__(self, bp_library, vehicle, reading_frequency=1.0):
self._collided = False
bp = bp_library.find('sensor.other.collision')
self.sensor = CarlaDataProvider.get_world().spawn_actor(bp, carla.Transform(), vehicle)
self.sensor.listen(lambda event: self.__class__.on_collision(weakref.ref(self), event))
self._reading_frequency = reading_frequency
self._callback = None
self._run_ps = True
self.run()
def __init__(self, actor, args=None):
super(SimpleVehicleControl, self).__init__(actor)
self._generated_waypoint_list = []
self._last_update = None
self._consider_obstacles = False
self._proximity_threshold = float('inf')
self._cv_image = None
self._camera = None
self._obstacle_sensor = None
self._obstacle_distance = float('inf')
self._obstacle_actor = None
if args and 'consider_obstacles' in args and strtobool(
args['consider_obstacles']):
self._consider_obstacles = strtobool(args['consider_obstacles'])
bp = CarlaDataProvider.get_world().get_blueprint_library().find(
'sensor.other.obstacle')
bp.set_attribute('distance', '250')
if args and 'proximity_threshold' in args:
bp.set_attribute('distance', args['proximity_threshold'])
bp.set_attribute('hit_radius', '1')
bp.set_attribute('only_dynamics', 'True')
self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor(
bp,
carla.Transform(
carla.Location(x=self._actor.bounding_box.extent.x,
z=1.0)),
attach_to=self._actor)
self._obstacle_sensor.listen(
lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda
if args and 'attach_camera' in args and strtobool(
args['attach_camera']):
bp = CarlaDataProvider.get_world().get_blueprint_library().find(
'sensor.camera.rgb')
self._camera = CarlaDataProvider.get_world().spawn_actor(
bp,
carla.Transform(carla.Location(x=0.0, z=30.0),
carla.Rotation(pitch=-60)),
attach_to=self._actor)
self._camera.listen(lambda image: self._on_camera_update(image)) # pylint: disable=unnecessary-lambda
def _tick_scenario(self, timestamp):
"""
Run next tick of scenario and the agent.
If running synchornously, it also handles the ticking of the world.
"""
if self._timestamp_last_run < timestamp.elapsed_seconds:
self._timestamp_last_run = timestamp.elapsed_seconds
self._watchdog.update()
# Update game time and actor information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
if self._agent is not None:
ego_action = self._agent()
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode > 1:
print("\n")
py_trees.display.print_ascii_tree(self.scenario_tree,
show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
if self._challenge_mode:
spectator = CarlaDataProvider.get_world().get_spectator()
ego_trans = self.ego_vehicles[0].get_transform()
spectator.set_transform(
carla.Transform(ego_trans.location + carla.Location(z=50),
carla.Rotation(pitch=-90)))
if self._agent is not None:
self.ego_vehicles[0].apply_control(ego_action)
if self._sync_mode and self._running and self._watchdog.get_status():
CarlaDataProvider.get_world().tick()
def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name,
actor_limit=7, name="ActorSource"):
"""
Setup class members
"""
super(ActorSource, self).__init__(name)
self._world = CarlaDataProvider.get_world()
self._actor_types = actor_type_list
self._spawn_point = transform
self._threshold = threshold
self._queue = Blackboard().get(blackboard_queue_name)
self._actor_limit = actor_limit
self._last_blocking_actor = None
def prepare_ego_vehicles(self, config, wait_for_ego_vehicles=False):
"""
Spawn or update the ego vehicle according to
its parameters provided in config
As the world is re-loaded for every scenario, no ego exists so far
"""
if not wait_for_ego_vehicles:
for vehicle in config.ego_vehicles:
self.ego_vehicles.append(
CarlaActorPool.setup_actor(vehicle.model,
vehicle.transform,
vehicle.rolename, True))
else:
ego_vehicle_missing = True
while ego_vehicle_missing:
self.ego_vehicles = []
ego_vehicle_missing = False
for ego_vehicle in config.ego_vehicles:
ego_vehicle_found = False
carla_vehicles = CarlaDataProvider.get_world().get_actors(
).filter('vehicle.*')
for carla_vehicle in carla_vehicles:
if carla_vehicle.attributes[
'role_name'] == ego_vehicle.rolename:
ego_vehicle_found = True
self.ego_vehicles.append(carla_vehicle)
break
if not ego_vehicle_found:
ego_vehicle_missing = True
break
for i, _ in enumerate(self.ego_vehicles):
self.ego_vehicles[i].set_transform(
config.ego_vehicles[i].transform)
# sync state
CarlaDataProvider.get_world().tick()
def _prepare_ego_vehicles(self, ego_vehicles, wait_for_ego_vehicles=False):
"""
Spawn or update the ego vehicles
"""
if not wait_for_ego_vehicles:
for vehicle in ego_vehicles:
self.ego_vehicles.append(
CarlaActorPool.setup_actor(
vehicle.model,
vehicle.transform,
vehicle.rolename,
True,
color=vehicle.color,
vehicle_category=vehicle.category))
else:
ego_vehicle_missing = True
while ego_vehicle_missing:
self.ego_vehicles = []
ego_vehicle_missing = False
for ego_vehicle in ego_vehicles:
ego_vehicle_found = False
carla_vehicles = CarlaDataProvider.get_world().get_actors(
).filter('vehicle.*')
for carla_vehicle in carla_vehicles:
if carla_vehicle.attributes[
'role_name'] == ego_vehicle.rolename:
ego_vehicle_found = True
self.ego_vehicles.append(carla_vehicle)
break
if not ego_vehicle_found:
ego_vehicle_missing = True
break
for i, _ in enumerate(self.ego_vehicles):
self.ego_vehicles[i].set_transform(ego_vehicles[i].transform)
# sync state
CarlaDataProvider.get_world().tick()
def _tick_scenario(self, timestamp):
"""
Run next tick of scenario and the agent.
If running synchornously, it also handles the ticking of the world.
"""
if self._timestamp_last_run < timestamp.elapsed_seconds and self._running:
self._timestamp_last_run = timestamp.elapsed_seconds
self._watchdog.update()
if self._debug_mode:
print("\n--------- Tick ---------\n")
# Update game time and actor information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
if self._agent is not None:
ego_action = self._agent()
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode:
print("\n")
py_trees.display.print_ascii_tree(self.scenario_tree,
show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
if self._agent is not None:
self.ego_vehicles[0].apply_control(ego_action)
if self._agent and self._running and self._watchdog.get_status():
CarlaDataProvider.get_world().tick()
def run_scenario(self):
"""
Trigger the start of the scenario and wait for it to finish/fail
"""
print("ScenarioManager: Running scenario {}".format(self.scenario_tree.name))
self.start_system_time = time.time()
start_game_time = GameTime.get_time()
self._watchdog.start()
self._running = True
# self.fields = ['location',
# 'velocity',
# 'ego_location',
# 'ego_velocity'
# ]
while self._running:
timestamp = None
world = CarlaDataProvider.get_world()
if world:
snapshot = world.get_snapshot()
if snapshot:
timestamp = snapshot.timestamp
if timestamp:
self._tick_scenario(timestamp)
# dict = [{'location':location, 'velocity':vel, 'ego_location':ego_location, 'ego_velocity':ego_vel}]
#
# file_exists = os.path.isfile(self.distance_path)
# with open(self.distance_path, 'a') as csvfile:
# # creating a csv dict writer object
# writer = csv.DictWriter(csvfile, fieldnames = self.fields)
# if not file_exists:
# writer.writeheader()
# writer.writerows(dict)
self._watchdog.stop()
self.cleanup()
self.end_system_time = time.time()
end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - \
self.start_system_time
self.scenario_duration_game = end_game_time - start_game_time
if self.scenario_tree.status == py_trees.common.Status.FAILURE:
print("ScenarioManager: Terminated due to failure")
