class SimController(object):
"""SimController class controls the flow of the simulation."""
def __init__(self):
self.heart_beat_config = HeartBeatConfig()
self.sim_data = None
self.sim_obj_generator = SimObjectGenerator()
self.message_interface = SimulationMessageInterface()
self.comm_server = None
self.client_socket = None
self.comm_port_number = 10021
self.supervisor_control = SupervisorControls()
self.vehicles_manager = None
self.environment_manager = EnvironmentManager()
self.current_sim_time_ms = 0
self.robustness_function = None
self.debug_mode = 0
self.data_logger = None
self.view_follow_item = None
self.controller_comm_interface = ControllerCommunicationInterface()
def init(self, debug_mode, supervisor_params):
"""Initialize the simulation controller."""
if self.debug_mode:
print("Starting initialization")
self.comm_port_number = supervisor_params
self.supervisor_control.init(supervisor_params)
self.vehicles_manager = VehiclesManager(self.supervisor_control)
self.debug_mode = debug_mode
self.vehicles_manager.debug_mode = self.debug_mode
if self.debug_mode:
# self.robustnessFunction.set_debug_mode(self.debug_mode)
print("Initialization: OK")
def set_debug_mode(self, mode):
"""Set debug mode."""
self.debug_mode = mode
def generate_road_network(self, road_list, sim_obj_generator,
road_network_id):
"""Generate and add the road network to the simulator."""
road_network_string = sim_obj_generator.generate_road_network_string(
road_list, road_network_id)
if self.debug_mode == 2:
print road_network_string
self.supervisor_control.add_obj_to_sim_from_string(road_network_string)
def generate_vehicle(self, vhc, sim_obj_generator):
"""Generate and add the vehicle to the simulator."""
vehicle_string = sim_obj_generator.generate_vehicle_string(vhc)
if self.debug_mode == 2:
print vehicle_string
self.supervisor_control.add_obj_to_sim_from_string(vehicle_string)
def receive_and_execute_commands(self):
"""Read all incoming commands until START SIMULATION Command."""
if self.debug_mode:
print("Waiting Commands")
road_segments_to_add = []
add_road_network_to_world = False
v_u_t_to_add = []
add_v_u_t_to_world = []
dummy_vhc_to_add = []
add_dummy_vhc_to_world = []
continue_simulation = False
remote_command = None
while continue_simulation is False or self.sim_data is None:
rcv_msg = self.comm_server.receive_blocking(self.client_socket)
remote_command = self.message_interface.interpret_message(rcv_msg)
remote_response = []
if self.debug_mode:
print("Received command : {}".format(remote_command.command))
if remote_command.command == self.message_interface.START_SIM:
self.sim_data = remote_command.object
continue_simulation = True
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.RELOAD_WORLD:
continue_simulation = True
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.CONTINUE_SIM:
continue_simulation = True
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.SET_DATA_LOG_PERIOD_MS:
if self.data_logger is None:
self.data_logger = DataLogger()
self.data_logger.set_environment_manager(
self.environment_manager)
self.data_logger.set_vehicles_manager(
self.vehicles_manager)
self.data_logger.set_log_period(remote_command.object)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.SET_CONTROLLER_PARAMETER:
if self.vehicles_manager is not None:
emitter = self.vehicles_manager.get_emitter()
if emitter is not None:
self.controller_comm_interface.transmit_set_controller_parameters_message(
emitter, remote_command.object.vehicle_id,
remote_command.object.parameter_name,
remote_command.object.parameter_data)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command in (
self.message_interface.SURROUNDINGS_DEF,
self.message_interface.SURROUNDINGS_ADD):
road_segments_to_add.append(remote_command.object)
add_road_network_to_world = (
remote_command.command ==
self.message_interface.SURROUNDINGS_ADD)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command in (
self.message_interface.DUMMY_ACTORS_DEF,
self.message_interface.DUMMY_ACTORS_ADD):
dummy_vhc_to_add.append(remote_command.object)
if remote_command.command == self.message_interface.DUMMY_ACTORS_ADD:
add_dummy_vhc_to_world.append(True)
else:
add_dummy_vhc_to_world.append(False)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command in (self.message_interface.VUT_DEF,
self.message_interface.VUT_ADD):
v_u_t_to_add.append(remote_command.object)
if remote_command.command == self.message_interface.VUT_ADD:
add_v_u_t_to_world.append(True)
else:
add_v_u_t_to_world.append(False)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.SET_HEART_BEAT_CONFIG:
self.heart_beat_config = remote_command.object
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.SET_ROBUSTNESS_TYPE:
robustness_type = remote_command.object
self.robustness_function = RobustnessComputation(
robustness_type, self.supervisor_control,
self.vehicles_manager, self.environment_manager)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.SET_VIEW_FOLLOW_ITEM:
self.view_follow_item = remote_command.object
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.ADD_DATA_LOG_DESCRIPTION:
if self.data_logger is None:
self.data_logger = DataLogger()
self.data_logger.set_environment_manager(
self.environment_manager)
self.data_logger.set_vehicles_manager(
self.vehicles_manager)
self.data_logger.add_data_log_description(
remote_command.object)
if self.sim_data is not None:
self.data_logger.set_expected_simulation_time(
self.sim_data.simulation_duration_ms)
self.data_logger.set_simulation_step_size(
self.sim_data.simulation_step_size_ms)
remote_response = self.message_interface.generate_ack_message()
elif remote_command.command == self.message_interface.GET_ROBUSTNESS:
if self.robustness_function is not None:
rob = self.robustness_function.get_robustness()
else:
rob = 0.0
remote_response = self.message_interface.generate_robustness_msg(
rob)
elif remote_command.command == self.message_interface.GET_DATA_LOG_INFO:
if self.data_logger is not None:
log_info = self.data_logger.get_log_info()
else:
log_info = (0, 0)
remote_response = self.message_interface.generate_log_info_message(
log_info)
elif remote_command.command == self.message_interface.GET_DATA_LOG:
requested_log_start_index = remote_command.object[0]
requested_log_end_index = remote_command.object[1]
if self.data_logger is not None:
data_log = self.data_logger.get_log(
requested_log_start_index, requested_log_end_index)
else:
data_log = np.empty(0)
remote_response = self.message_interface.generate_data_log_message(
data_log)
if len(remote_response) > 0:
self.comm_server.send_blocking(self.client_socket,
remote_response)
# Generate simulation environment (add VUT, Dummy Actors and Surroundings)
if road_segments_to_add and self.debug_mode:
print("Number of road segments: {}".format(
len(road_segments_to_add)))
if add_road_network_to_world:
self.generate_road_network(
road_segments_to_add, self.sim_obj_generator,
self.environment_manager.get_num_of_road_networks() + 1)
if road_segments_to_add:
self.environment_manager.record_road_network(road_segments_to_add)
if v_u_t_to_add and self.debug_mode:
print("Number of VUT: {}".format(len(v_u_t_to_add)))
for i in range(len(v_u_t_to_add)):
vhc = v_u_t_to_add[i]
if add_v_u_t_to_world[i]:
self.generate_vehicle(vhc, self.sim_obj_generator)
self.vehicles_manager.record_vehicle(vhc,
self.vehicles_manager.VHC_VUT)
if dummy_vhc_to_add and self.debug_mode:
print("Number of Dummy vehicles: {}".format(len(dummy_vhc_to_add)))
for i in range(len(dummy_vhc_to_add)):
vhc = dummy_vhc_to_add[i]
if add_dummy_vhc_to_world[i]:
self.generate_vehicle(vhc, self.sim_obj_generator)
self.vehicles_manager.record_vehicle(
vhc, self.vehicles_manager.VHC_DUMMY)
if remote_command is not None and remote_command.command == self.message_interface.RELOAD_WORLD:
time.sleep(1.0)
try:
print('Closing connection!')
self.comm_server.close_connection()
except:
print('Could not close connection!')
pass
time.sleep(0.5)
self.comm_server = None
print('Reverting Simulation!')
self.supervisor_control.revert_simulation()
time.sleep(1)
def prepare_sim_environment(self):
"""Prepares the simulation environment based on the communication with simulation controller."""
if self.debug_mode:
print("Will Prepare Sim Environment")
self.supervisor_control.initialize_creating_simulation_environment()
self.comm_server = CommunicationServer(True, self.comm_port_number,
self.debug_mode)
self.client_socket = self.comm_server.get_connection()
# Read all incoming commands until START SIMULATION Command
self.receive_and_execute_commands()
# Set viewpoint
view_point_vehicle_index = 0
if self.view_follow_item is not None:
if self.view_follow_item.item_type == ItemDescription.ITEM_TYPE_VEHICLE:
view_point_vehicle_index = self.view_follow_item.item_index
if self.vehicles_manager.vehicles:
if len(self.vehicles_manager.vehicles) <= view_point_vehicle_index:
view_point_vehicle_index = 0
viewpoint = self.supervisor_control.getFromDef('VIEWPOINT')
if viewpoint is not None:
follow_point = viewpoint.getField('follow')
if follow_point is not None:
follow_point.setSFString(
self.vehicles_manager.
vehicles[view_point_vehicle_index].name.getSFString())
# Set time parameters for the objects where necessary.
if self.data_logger is not None:
self.data_logger.set_expected_simulation_time(
self.sim_data.simulation_duration_ms)
self.data_logger.set_simulation_step_size(
self.sim_data.simulation_step_size_ms)
self.vehicles_manager.set_time_step(
self.sim_data.simulation_step_size_ms / 1000.0)
# Reflect changes to the simulation environment
self.supervisor_control.finalize_creating_simulation_environment()
def run(self):
"""The overall execution of the simulation."""
# Prepare Simulation Environment
self.prepare_sim_environment()
# Start simulation
print 'Simulation Duration = {}, step size = {}'.format(
self.sim_data.simulation_duration_ms,
self.sim_data.simulation_step_size_ms)
if self.sim_data.simulation_execution_mode == self.sim_data.SIM_TYPE_RUN:
self.supervisor_control.set_simulation_mode(
self.supervisor_control.SIMULATION_MODE_RUN)
elif self.sim_data.simulation_execution_mode == self.sim_data.SIM_TYPE_FAST_NO_GRAPHICS:
self.supervisor_control.set_simulation_mode(
self.supervisor_control.SIMULATION_MODE_FAST)
else:
self.supervisor_control.set_simulation_mode(
self.supervisor_control.SIMULATION_MODE_REAL_TIME)
# Execute simulation
while self.current_sim_time_ms < self.sim_data.simulation_duration_ms:
cur_sim_time_s = self.current_sim_time_ms / 1000.0
self.vehicles_manager.simulate_vehicles(cur_sim_time_s)
if self.robustness_function is not None:
self.robustness_function.compute_robustness(cur_sim_time_s)
if self.data_logger is not None:
self.data_logger.log_data(self.current_sim_time_ms)
if (self.heart_beat_config is not None
and self.heart_beat_config.sync_type
in (HeartBeatConfig.WITH_SYNC,
HeartBeatConfig.WITHOUT_SYNC)
and self.current_sim_time_ms %
self.heart_beat_config.period_ms == 0):
heart_beat_message = self.message_interface.generate_heart_beat_message(
self.current_sim_time_ms, HeartBeat.SIMULATION_RUNNING)
self.comm_server.send_blocking(self.client_socket,
heart_beat_message)
if self.heart_beat_config.sync_type == HeartBeatConfig.WITH_SYNC:
# This means it has to wait for a new command.
self.receive_and_execute_commands()
self.supervisor_control.step_simulation(
self.sim_data.simulation_step_size_ms)
self.current_sim_time_ms = self.current_sim_time_ms + self.sim_data.simulation_step_size_ms
# End Simulation
self.supervisor_control.set_simulation_mode(
self.supervisor_control.SIMULATION_MODE_PAUSE)
heart_beat_message = self.message_interface.generate_heart_beat_message(
self.current_sim_time_ms, HeartBeat.SIMULATION_STOPPED)
self.comm_server.send_blocking(self.client_socket, heart_beat_message)
for i in range(
100
): # Maximum 100 messages are accepted after the simulation. Against lock / memory grow etc.
if self.comm_server is not None:
self.receive_and_execute_commands()
