class ScenarioManager(object):
"""
Basic scenario manager class. This class holds all functionality
required to start, and analyze a scenario.
The user must not modify this class.
To use the ScenarioManager:
1. Create an object via manager = ScenarioManager()
2. Load a scenario via manager.load_scenario()
3. Trigger the execution of the scenario manager.run_scenario()
This function is designed to explicitly control start and end of
the scenario execution
4. Trigger a result evaluation with manager.analyze_scenario()
5. If needed, cleanup with manager.stop_scenario()
"""
def __init__(self, debug_mode=False, sync_mode=False, timeout=2.0):
"""
Setups up the parameters, which will be filled at load_scenario()
"""
self.scenario = None
self.scenario_tree = None
self.scenario_class = None
self.ego_vehicles = None
self.other_actors = None
self._debug_mode = debug_mode
self._agent = None
self._sync_mode = sync_mode
self._running = False
self._timestamp_last_run = 0.0
self._timeout = timeout
self._watchdog = Watchdog(float(self._timeout))
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
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
GameTime.restart()
def cleanup(self):
"""
This function triggers a proper termination of a scenario
"""
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
CarlaDataProvider.cleanup()
def load_scenario(self, scenario, agent=None):
"""
Load a new scenario
"""
self._reset()
self._agent = AgentWrapper(agent) if agent else None
if self._agent is not None:
self._sync_mode = True
self.scenario_class = scenario
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicles = scenario.ego_vehicles
self.other_actors = scenario.other_actors
# To print the scenario tree uncomment the next line
# py_trees.display.render_dot_tree(self.scenario_tree)
if self._agent is not None:
self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
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 _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._sync_mode and self._running and self._watchdog.get_status():
CarlaDataProvider.get_world().tick()
def get_running_status(self):
"""
returns:
bool: False if watchdog exception occured, True otherwise
"""
return self._watchdog.get_status()
def stop_scenario(self):
"""
This function is used by the overall signal handler to terminate the scenario execution
"""
self._running = False
def analyze_scenario(self, stdout, filename, junit):
"""
This function is intended to be called from outside and provide
the final statistics about the scenario (human-readable, in form of a junit
report, etc.)
"""
failure = False
timeout = False
result = "SUCCESS"
if self.scenario.test_criteria is None:
print("Nothing to analyze, this scenario has no criteria")
return True
for criterion in self.scenario.get_criteria():
if (not criterion.optional and criterion.test_status != "SUCCESS"
and criterion.test_status != "ACCEPTABLE"):
failure = True
result = "FAILURE"
elif criterion.test_status == "ACCEPTABLE":
result = "ACCEPTABLE"
if self.scenario.timeout_node.timeout and not failure:
timeout = True
result = "TIMEOUT"
output = ResultOutputProvider(self, result, stdout, filename, junit)
output.write()
return failure or timeout
class ScenarioManager(object):
"""
Basic scenario manager class. This class holds all functionality
required to start, and analyze a scenario.
The user must not modify this class.
To use the ScenarioManager:
1. Create an object via manager = ScenarioManager()
2. Load a scenario via manager.load_scenario()
3. Trigger the execution of the scenario manager.run_scenario()
This function is designed to explicitly control start and end of
the scenario execution
4. Trigger a result evaluation with manager.analyze_scenario()
5. If needed, cleanup with manager.stop_scenario()
"""
def __init__(self,
debug_mode=False,
sync_mode=False,
challenge_mode=False,
track=None,
timeout=10.0,
episode_max_time=10000):
"""
Setups up the parameters, which will be filled at load_scenario()
"""
self.scenario = None
self.scenario_tree = None
self.scenario_class = None
self.ego_vehicles = None
self.other_actors = None
self._debug_mode = debug_mode
self._challenge_mode = challenge_mode
self._track = track
self._agent = None
self._sync_mode = sync_mode
self._running = False
self._timestamp_last_run = 0.0
self._timeout = timeout
self._watchdog = Watchdog(float(self._timeout))
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
# modification: disable when running multi-process via dask
# Register the scenario tick as callback for the CARLA world
# Use the callback_id inside the signal handler to allow external interrupts
# signal.signal(signal.SIGINT, self._signal_handler)
self.episode_max_time = episode_max_time
def _signal_handler(self, signum, frame):
"""
Terminate scenario ticking when receiving a signal interrupt
"""
self._running = False
if not self.get_running_status():
raise RuntimeError("Timeout occured during scenario execution")
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
GameTime.restart()
def load_scenario(self, scenario, agent=None):
"""
Load a new scenario
"""
self._reset()
self._agent = AgentWrapper(agent,
self._challenge_mode) if agent else None
if self._agent is not None:
self._sync_mode = True
self.scenario_class = scenario
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicles = scenario.ego_vehicles
self.other_actors = scenario.other_actors
# modification registration now moves to carla_data_provider
# CarlaDataProvider.register_actors(self.ego_vehicles)
# CarlaDataProvider.register_actors(self.other_actors)
# To print the scenario tree uncomment the next line
# py_trees.display.render_dot_tree(self.scenario_tree)
if self._agent is not None:
self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode,
self._track)
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 _console_message(self):
"""
Message that will be displayed via console
"""
def get_symbol(value, desired_value, high=True):
"""
Returns a tick or a cross depending on the values
"""
tick = '\033[92m' + 'O' + '\033[0m'
cross = '\033[91m' + 'X' + '\033[0m'
multiplier = 1 if high else -1
if multiplier * value >= desired_value:
symbol = tick
else:
symbol = cross
return symbol
if self.scenario_tree.status == py_trees.common.Status.RUNNING:
# If still running, all the following is None, so no point continuing
print(
"\n> Something happened during the simulation. Was it manually shutdown?\n"
)
return
blackv = py_trees.blackboard.Blackboard()
route_completed = blackv.get("RouteCompletion")
collisions = blackv.get("Collision")
outside_route_lanes = blackv.get("OutsideRouteLanes")
stop_signs = blackv.get("RunningStop")
red_light = blackv.get("RunningRedLight")
in_route = blackv.get("InRoute")
# addition: new event
on_side_walk = blackv.get("OnSideWalk")
off_road = blackv.get("OffRoad")
wrong_lane = blackv.get("WrongLane")
# If something failed, stop
if [
x for x in (collisions, outside_route_lanes, stop_signs,
red_light, in_route) if x is None
]:
print("Nothing to analyze, this scenario has no criteria")
return
if blackv.get("RouteCompletion") >= 99:
route_completed = 100
else:
route_completed = blackv.get("RouteCompletion")
outside_route_lanes = float(outside_route_lanes)
route_symbol = get_symbol(route_completed, 100, True)
collision_symbol = get_symbol(collisions, 0, False)
outside_symbol = get_symbol(outside_route_lanes, 0, False)
red_light_symbol = get_symbol(red_light, 0, False)
stop_symbol = get_symbol(stop_signs, 0, False)
# addition: new event
on_side_walk_symbol = get_symbol(on_side_walk, 0, False)
off_road_symbol = get_symbol(off_road, 0, False)
wrong_lane_symbol = get_symbol(wrong_lane, 0, False)
if self.scenario_tree.status == py_trees.common.Status.FAILURE:
if not in_route:
message = "> FAILED: The actor deviated from the route"
else:
message = "> FAILED: The actor didn't finish the route"
elif self.scenario_tree.status == py_trees.common.Status.SUCCESS:
if route_completed == 100:
message = "> SUCCESS: Congratulations, route finished! "
else:
message = "> FAILED: The actor timed out "
else: # This should never be triggered
return
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
print("\n" + message)
print("> ")
print("> Score: ")
print("> - Route Completed [{}]: {}%".format(
route_symbol, route_completed))
print("> - Outside route lanes [{}]: {}%".format(
outside_symbol, outside_route_lanes))
print("> - Collisions [{}]: {} times".format(
collision_symbol, collisions))
print("> - Red lights run [{}]: {} times".format(
red_light_symbol, red_light))
print("> - Stop signs run [{}]: {} times".format(
stop_symbol, stop_signs))
# addition: new event
print("> - On side walk [{}]: {} times".format(
on_side_walk_symbol, on_side_walk))
print("> - Off road [{}]: {} times".format(
off_road_symbol, off_road))
print("> - Wrong lane [{}]: {} times\n".format(
wrong_lane_symbol, wrong_lane))
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 get_running_status(self):
"""
returns:
bool: False if watchdog exception occured, True otherwise
"""
return self._watchdog.get_status()
def stop_scenario(self):
"""
This function triggers a proper termination of a scenario
"""
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
CarlaDataProvider.cleanup()
class ScenarioManager(object):
"""
Basic scenario manager class. This class holds all functionality
required to start, run and stop a scenario.
The user must not modify this class.
To use the ScenarioManager:
1. Create an object via manager = ScenarioManager()
2. Load a scenario via manager.load_scenario()
3. Trigger the execution of the scenario manager.run_scenario()
This function is designed to explicitly control start and end of
the scenario execution
4. If needed, cleanup with manager.stop_scenario()
"""
def __init__(self, timeout, debug_mode=False, timely_commands=False, frame_rate=20):
"""
Setups up the parameters, which will be filled at load_scenario()
"""
self.timely_commands = timely_commands
self.frame_rate = frame_rate
self.scenario = None
self.scenario_tree = None
self.scenario_class = None
self.ego_vehicles = None
self.other_actors = None
self._debug_mode = debug_mode
self._agent = None
self._running = False
self._timestamp_last_run = 0.0
self._timeout = float(timeout)
# Used to detect if the simulation is down
watchdog_timeout = max(5, self._timeout - 2)
self._watchdog = Watchdog(watchdog_timeout)
# Avoid the agent from freezing the simulation
agent_timeout = watchdog_timeout - 1
self._agent_watchdog = Watchdog(agent_timeout)
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
self.end_game_time = None
# Register the scenario tick as callback for the CARLA world
# Use the callback_id inside the signal handler to allow external interrupts
signal.signal(signal.SIGINT, self.signal_handler)
self._delayed_cmds = []
brake_control = carla.VehicleControl()
brake_control.throttle = 0
brake_control.brake = 1
brake_control.steer = 0
brake_control.reverse = False
brake_control.hand_brake = False
brake_control.manual_gear_shift = False
self._last_cmd = (0, 0, brake_control)
def signal_handler(self, signum, frame):
"""
Terminate scenario ticking when receiving a signal interrupt
"""
self._running = False
def cleanup(self):
"""
Reset all parameters
"""
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
self.end_game_time = None
def load_scenario(self, scenario, agent, rep_number):
"""
Load a new scenario
"""
GameTime.restart()
agent.frame_delta_ms = int(1000 / self.frame_rate)
self._agent = AgentWrapper(agent)
self.scenario_class = scenario
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicles = scenario.ego_vehicles
self.other_actors = scenario.other_actors
self.repetition_number = rep_number
# To print the scenario tree uncomment the next line
# py_trees.display.render_dot_tree(self.scenario_tree)
self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
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()
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)
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()
game_time_ms = int(GameTime.get_time() * 1000)
try:
res = self._agent()
if self.timely_commands:
assert isinstance(res, tuple), 'Agent did not return the runtime'
ego_action, runtime = res
else:
if isinstance(res, tuple):
ego_action = res[0]
else:
ego_action = res
runtime = 0
heapq.heappush(self._delayed_cmds,
(game_time_ms + runtime, game_time_ms, ego_action))
# 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)
while len(self._delayed_cmds) > 0 and game_time_ms >= self._delayed_cmds[0][0]:
self._last_cmd = self._delayed_cmds[0]
heapq.heappop(self._delayed_cmds)
print("Command at {} using {}".format(self._last_cmd[0], self._last_cmd[1]))
self.ego_vehicles[0].apply_control(self._last_cmd[2])
# 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 get_running_status(self):
"""
returns:
bool: False if watchdog exception occured, True otherwise
"""
return self._watchdog.get_status()
def stop_scenario(self):
"""
This function triggers a proper termination of a scenario
"""
self._watchdog.stop()
self.end_system_time = time.time()
self.end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - self.start_system_time
self.scenario_duration_game = self.end_game_time - self.start_game_time
if self.get_running_status():
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
self.analyze_scenario()
def analyze_scenario(self):
"""
Analyzes and prints the results of the route
"""
global_result = '\033[92m'+'SUCCESS'+'\033[0m'
for criterion in self.scenario.get_criteria():
if criterion.test_status != "SUCCESS":
global_result = '\033[91m'+'FAILURE'+'\033[0m'
if self.scenario.timeout_node.timeout:
global_result = '\033[91m'+'FAILURE'+'\033[0m'
ResultOutputProvider(self, global_result)
class ScenarioManager(object):
"""
Basic scenario manager class. This class holds all functionality
required to start, and analyze a scenario.
The user must not modify this class.
To use the ScenarioManager:
1. Create an object via manager = ScenarioManager()
2. Load a scenario via manager.load_scenario()
3. Trigger the execution of the scenario manager.execute()
This function is designed to explicitly control start and end of
the scenario execution
4. Trigger a result evaluation with manager.analyze()
5. Cleanup with manager.stop_scenario()
"""
def __init__(self, debug_mode=False, challenge_mode=False, track=None, timeout=20.0):
"""
Init requires scenario as input
"""
self.scenario = None
self.scenario_tree = None
self.scenario_class = None
self.ego_vehicles = None
self.other_actors = None
self._debug_mode = debug_mode
self._challenge_mode = challenge_mode
self._track = track
self._agent = None
self._running = False
self._timestamp_last_run = 0.0
self._timeout = timeout
self._watchdog = Watchdog(float(self._timeout))
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
nvidia_smi.nvmlInit()
self.handle = nvidia_smi.nvmlDeviceGetHandleByIndex(1)
# Register the scenario tick as callback for the CARLA world
# Use the callback_id inside the signal handler to allow external interrupts
signal.signal(signal.SIGINT, self._signal_handler)
def _signal_handler(self, signum, frame):
"""
Terminate scenario ticking when receiving a signal interrupt
"""
self._running = False
if not self.get_running_status():
raise RuntimeError("Timeout occured during scenario execution")
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
GameTime.restart()
def load_scenario(self, scenario, agent=None):
"""
Load a new scenario
"""
self._reset()
self._agent = AgentWrapper(agent, self._challenge_mode) if agent else None
self.scenario_class = scenario
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicles = scenario.ego_vehicles
self.other_actors = scenario.other_actors
#print(self.other_actors)
CarlaDataProvider.register_actors(self.ego_vehicles)
CarlaDataProvider.register_actors(self.other_actors)
# To print the scenario tree uncomment the next line
# py_trees.display.render_dot_tree(self.scenario_tree)
if self._agent is not None:
self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode, self._track)
def run_scenario(self,path,path1):
"""
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()
total_risk_score = []
self._watchdog.start()
self._running = True
x = 0
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)
x+=1
if(x%60==1):
res = nvidia_smi.nvmlDeviceGetUtilizationRates(self.handle)
mem_res = nvidia_smi.nvmlDeviceGetMemoryInfo(self.handle)
#print(f'mem: {mem_res.used / (1024**2)} (GiB)') # usage in GiB
print(f'mem: {100 * (mem_res.used / mem_res.total):.3f}%') # percentage usage
cpu = psutil.cpu_percent()#cpu utilization stats
mem = psutil.virtual_memory()#virtual memory stats
print(f'gpu: {res.gpu}%, gpu-mem: {res.memory}%')
fields = ['GPU Utilization',
'GPU Memory',
'CPU Utilization',
'CPU Memory'
]
dict = [{'GPU Utilization':res.gpu, 'GPU Memory':100 * (mem_res.used / mem_res.total), 'CPU Utilization':cpu, 'CPU Memory':100 * (mem.used/mem.total)}]
file_exists = os.path.isfile(path1)
with open(path1, 'a') as csvfile:
# creating a csv dict writer object
writer = csv.DictWriter(csvfile, fieldnames = fields)
if not file_exists:
writer.writeheader()
writer.writerows(dict)
#total_risk_score.append(risk_score)
#print("--------------------------------------------------------------------------")
#print("Average Risk Score:%f"%(float(sum(total_risk_score))/len(total_risk_score)))
#print("--------------------------------------------------------------------------")
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
fields = ['Route Completed',
'Collisions'
]
route_completed, collisions = self._console_message()
dict = [{'Route Completed':route_completed, 'Collisions':collisions}]
file_exists = os.path.isfile(path)
with open(path, 'a') as csvfile:
# creating a csv dict writer object
writer = csv.DictWriter(csvfile, fieldnames = fields)
if not file_exists:
writer.writeheader()
writer.writerows(dict)
def _console_message(self):
"""
Message that will be displayed via console
"""
def get_symbol(value, desired_value, high=True):
"""
Returns a tick or a cross depending on the values
"""
tick = '\033[92m'+'O'+'\033[0m'
cross = '\033[91m'+'X'+'\033[0m'
multiplier = 1 if high else -1
if multiplier*value >= desired_value:
symbol = tick
else:
symbol = cross
return symbol
if self.scenario_tree.status == py_trees.common.Status.RUNNING:
# If still running, all the following is None, so no point continuing
print("\n> Something happened during the simulation. Was it manually shutdown?\n")
return
blackv = py_trees.blackboard.Blackboard()
route_completed = blackv.get("RouteCompletion")
collisions = blackv.get("Collision")
outside_route_lanes = blackv.get("OutsideRouteLanes")
stop_signs = blackv.get("RunningStop")
red_light = blackv.get("RunningRedLight")
in_route = blackv.get("InRoute")
# If something failed, stop
if [x for x in (collisions, outside_route_lanes, stop_signs, red_light, in_route) if x is None]:
return
if blackv.get("RouteCompletion") >= 99:
route_completed = 100
else:
route_completed = blackv.get("RouteCompletion")
outside_route_lanes = float(outside_route_lanes)
route_symbol = get_symbol(route_completed, 100, True)
collision_symbol = get_symbol(collisions, 0, False)
outside_symbol = get_symbol(outside_route_lanes, 0, False)
red_light_symbol = get_symbol(red_light, 0, False)
stop_symbol = get_symbol(stop_signs, 0, False)
if self.scenario_tree.status == py_trees.common.Status.FAILURE:
if not in_route:
message = "> FAILED: The actor deviated from the route"
else:
message = "> FAILED: The actor didn't finish the route"
elif self.scenario_tree.status == py_trees.common.Status.SUCCESS:
if route_completed == 100:
message = "> SUCCESS: Congratulations, route finished! "
else:
message = "> FAILED: The actor timed out "
else: # This should never be triggered
return
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
print("\n" + message)
print("> ")
print("> Score: ")
print("> - Route Completed [{}]: {}%".format(route_symbol, route_completed))
#print("> - Outside route lanes [{}]: {}%".format(outside_symbol, outside_route_lanes))
print("> - Collisions [{}]: {} times".format(collision_symbol, math.floor(collisions)))
#print("> - Red lights run [{}]: {} times".format(red_light_symbol, red_light))
#print("> - Stop signs run [{}]: {} times\n".format(stop_symbol, stop_signs))
return route_completed, collisions
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:# This is the entry point to the agent
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()
#return risk_score
def get_running_status(self):
"""
returns:
bool: False if watchdog exception occured, True otherwise
"""
return self._watchdog.get_status()
def stop_scenario(self):
"""
This function triggers a proper termination of a scenario
"""
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
CarlaDataProvider.cleanup()
CarlaActorPool.cleanup()
class ScenarioManager(object):
"""
Basic scenario manager class. This class holds all functionality
required to start, run and stop a scenario.
The user must not modify this class.
To use the ScenarioManager:
1. Create an object via manager = ScenarioManager()
2. Load a scenario via manager.load_scenario()
3. Trigger the execution of the scenario manager.run_scenario()
This function is designed to explicitly control start and end of
the scenario execution
4. If needed, cleanup with manager.stop_scenario()
"""
def __init__(self, timeout, debug_mode=False):
"""
Setups up the parameters, which will be filled at load_scenario()
"""
self.scenario = None
self.scenario_tree = None
self.scenario_class = None
self.ego_vehicles = None
self.other_actors = None
self._debug_mode = debug_mode
self._agent = None
self._running = False
self._timestamp_last_run = 0.0
self._timeout = float(timeout)
self._watchdog = Watchdog(
self._timeout) # Detects if the simulation is down
self._agent_watchdog = Watchdog(
self._timeout) # Stop the agent from freezing the simulation
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
self.end_game_time = None
# Register the scenario tick as callback for the CARLA world
# Use the callback_id inside the signal handler to allow external interrupts
signal.signal(signal.SIGINT, self.signal_handler)
def signal_handler(self, signum, frame):
"""
Terminate scenario ticking when receiving a signal interrupt
"""
if self._agent_watchdog and not self._agent_watchdog.get_status():
raise RuntimeError(
"Agent took longer than {}s to send its command".format(
self._timeout))
elif self._watchdog and not self._watchdog.get_status():
raise RuntimeError(
"The simulation took longer than {}s to update".format(
self._timeout))
self._running = False
def cleanup(self):
"""
Reset all parameters
"""
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
self.end_game_time = None
self._spectator = None
def load_scenario(self, scenario, agent, rep_number):
"""
Load a new scenario
"""
GameTime.restart()
self._agent = AgentWrapper(agent)
self.scenario_class = scenario
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicles = scenario.ego_vehicles
self.other_actors = scenario.other_actors
self.repetition_number = rep_number
self._spectator = CarlaDataProvider.get_world().get_spectator()
# To print the scenario tree uncomment the next line
# py_trees.display.render_dot_tree(self.scenario_tree)
self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
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()
self._watchdog.start()
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 _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
ego_trans = self.ego_vehicles[0].get_transform()
self._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 get_running_status(self):
"""
returns:
bool: False if watchdog exception occured, True otherwise
"""
return self._watchdog.get_status()
def stop_scenario(self):
"""
This function triggers a proper termination of a scenario
"""
self._watchdog.stop()
self._agent_watchdog.stop()
self.end_system_time = time.time()
self.end_game_time = GameTime.get_time()
self.scenario_duration_system = self.end_system_time - self.start_system_time
self.scenario_duration_game = self.end_game_time - self.start_game_time
if self.get_running_status():
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
self.analyze_scenario()
def analyze_scenario(self):
"""
Analyzes and prints the results of the route
"""
global_result = '\033[92m' + 'SUCCESS' + '\033[0m'
for criterion in self.scenario.get_criteria():
if criterion.test_status != "SUCCESS":
global_result = '\033[91m' + 'FAILURE' + '\033[0m'
if self.scenario.timeout_node.timeout:
global_result = '\033[91m' + 'FAILURE' + '\033[0m'
ResultOutputProvider(self, global_result)
class ScenarioManager(object):
"""
Basic scenario manager class. This class holds all functionality
required to start, and analyze a scenario.
The user must not modify this class.
To use the ScenarioManager:
1. Create an object via manager = ScenarioManager()
2. Load a scenario via manager.load_scenario()
3. Trigger the execution of the scenario manager.execute()
This function is designed to explicitly control start and end of
the scenario execution
4. Trigger a result evaluation with manager.analyze()
5. Cleanup with manager.stop_scenario()
"""
def __init__(self, debug_mode=False, timeout=2.0):
"""
Init requires scenario as input
"""
self.scenario = None
self.scenario_tree = None
self.scenario_class = None
self.ego_vehicles = None
self.other_actors = None
self._debug_mode = debug_mode
self._agent = None
self._running = False
self._timestamp_last_run = 0.0
self._timeout = timeout
self._watchdog = Watchdog(float(self._timeout))
self.scenario_duration_system = 0.0
self.scenario_duration_game = 0.0
self.start_system_time = None
self.end_system_time = None
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
GameTime.restart()
def cleanup(self):
"""
This function triggers a proper termination of a scenario
"""
if self.scenario is not None:
self.scenario.terminate()
if self._agent is not None:
self._agent.cleanup()
self._agent = None
CarlaDataProvider.cleanup()
CarlaActorPool.cleanup()
def load_scenario(self, scenario, agent=None):
"""
Load a new scenario
"""
self._reset()
self._agent = AgentWrapper(agent) if agent else None
self.scenario_class = scenario
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicles = scenario.ego_vehicles
self.other_actors = scenario.other_actors
CarlaDataProvider.register_actors(self.ego_vehicles)
CarlaDataProvider.register_actors(self.other_actors)
# To print the scenario tree uncomment the next line
# py_trees.display.render_dot_tree(self.scenario_tree)
if self._agent is not None:
self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
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")
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 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()
#print(vel)
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.perform_carla_tick(self._timeout)
#return vel,location,ego_vel,ego_location
def get_running_status(self):
"""
returns:
bool: False if watchdog exception occured, True otherwise
"""
return self._watchdog.get_status()
def stop_scenario(self):
"""
This function is used by the overall signal handler to terminate the scenario execution
"""
self._running = False
def analyze_scenario(self, stdout, filename, junit):
"""
This function is intended to be called from outside and provide
the final statistics about the scenario (human-readable, in form of a junit
report, etc.)
"""
failure = False
timeout = False
result = "SUCCESS"
if self.scenario.test_criteria is None:
return True
for criterion in self.scenario.get_criteria():
if (not criterion.optional and
criterion.test_status != "SUCCESS" and
criterion.test_status != "ACCEPTABLE"):
failure = True
result = "FAILURE"
elif criterion.test_status == "ACCEPTABLE":
result = "ACCEPTABLE"
if self.scenario.timeout_node.timeout and not failure:
timeout = True
result = "TIMEOUT"
output = ResultOutputProvider(self, result, stdout, filename, junit)
output.write()
return failure or timeout
