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, supervisor_controller):
self.EMITTER_NAME = "emitter"
self.debug_mode = 0
self.vehicles = []
self.vehicle_dictionary = {}
self.VUT_dictionary = {}
self.dummy_vhc_dictionary = {}
self.total_vhc_count = 0
self.has_sent_controller_config = False
self.supervisorControl = supervisor_controller
self.time_step = 0.01
self.current_sim_time = 0.0
self.step_counter = 0
self.num_of_steps_for_jerk_calc = 10
self.acc_arr = [0.0] * (self.num_of_steps_for_jerk_calc + 3)
self.cur_jerk_compute_index = 0
self.controller_comm_interface = ControllerCommunicationInterface()
class simple_acceleration_follower(BaseCarController):
def __init__(self, (car_model, self_vhc_id, target_acc_list,
target_time_list)):
BaseCarController.__init__(self, car_model)
self.EMITTER_DEVICE_NAME = "emitter"
self.RECEIVER_DEVICE_NAME = "receiver"
self.COMPASS_DEVICE_NAME = "vut_compass"
self.STEP_TIME = 10
self.TOUCH_SENSOR_NAME = "touch sensor"
self.MAX_NEGATIVE_THROTTLE_CHANGE = -float(self.STEP_TIME) / 3000.0
self.MAX_POSITIVE_THROTTLE_CHANGE = float(
self.STEP_TIME
) / 1000.0 # I want to give full throttle from 0 in 1 sec
self.STANLEY_K = 1.4 # 0.25
self.STANLEY_K2 = 1.0 # 0.3
self.STANLEY_K3 = 1.0
self.ref_orientation = 0.0
self.received_message = []
self.SELF_VHC_ID = int(self_vhc_id)
self.touch_sensor = None
self.compass_device = None
self.receiver_device = None
self.emitter_device = None
self.GPS_DEVICE_NAME = "vut_gps"
self.GPS_DEVICE_PERIOD = 10
self.prev_long_control = 0.0
self.prev_lat_control = 0.0
self.last_throttle = 0.0
self.LONG_PID_P = 0.06125 # 1.5
self.LONG_PID_I = 0.000005 # 0.1
self.LONG_PID_D = 0.1 # 8.5
self.PID_INTEGRATOR_MIN = -500.0
self.PID_INTEGRATOR_MAX = 500.0
self.STEP_TIME = 10
self.prev_speed = None
self.MAX_NEGATIVE_THROTTLE_CHANGE = -float(self.STEP_TIME) / 100.0
self.MAX_POSITIVE_THROTTLE_CHANGE = float(self.STEP_TIME) / 100.0
self.longitudinal_pid = GenericPIDController()
self.lateral_controller = GenericStanleyController()
import ast
print('list: {}'.format(target_acc_list))
self.target_acc_list = ast.literal_eval(target_acc_list)
print('Target Acc: {}'.format(self.target_acc_list))
self.target_time_list = ast.literal_eval(target_time_list)
self.contr_comm_interface = ControllerCommunicationInterface()
class VehiclesManager(object):
"""VehiclesManager keeps track of the vehicles in the simulation environment."""
VHC_DUMMY = 0
VHC_VUT = 1
def __init__(self, supervisor_controller):
self.EMITTER_NAME = "emitter"
self.debug_mode = 0
self.vehicles = []
self.vehicle_dictionary = {}
self.VUT_dictionary = {}
self.dummy_vhc_dictionary = {}
self.total_vhc_count = 0
self.has_sent_controller_config = False
self.supervisorControl = supervisor_controller
self.time_step = 0.01
self.current_sim_time = 0.0
self.step_counter = 0
self.num_of_steps_for_jerk_calc = 10
self.acc_arr = [0.0] * (self.num_of_steps_for_jerk_calc + 3)
self.cur_jerk_compute_index = 0
self.controller_comm_interface = ControllerCommunicationInterface()
def record_vehicle(self, vehicle_object, vehicle_type):
"""Add the vehicle in the records. vehicle_type can be VUT / Dummy"""
self.vehicles.append(vehicle_object)
self.vehicles[self.total_vhc_count].node = \
self.supervisorControl.get_obj_node(self.vehicles[self.total_vhc_count])
self.vehicles[self.total_vhc_count].translation = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count], "translation")
self.vehicles[self.total_vhc_count].rotation = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count], "rotation")
self.vehicles[self.total_vhc_count].name = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count], "name")
self.vehicles[self.total_vhc_count].front_right_wheel_angular_velocity = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count],
"front_right_wheel_angular_velocity")
self.vehicles[self.total_vhc_count].front_left_wheel_angular_velocity = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count],
"front_left_wheel_angular_velocity")
self.vehicles[self.total_vhc_count].rear_right_wheel_angular_velocity = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count],
"rear_right_wheel_angular_velocity")
self.vehicles[self.total_vhc_count].rear_left_wheel_angular_velocity = \
self.supervisorControl.get_obj_field(self.vehicles[self.total_vhc_count],
"rear_left_wheel_angular_velocity")
self.vehicles[self.total_vhc_count].current_position = \
self.supervisorControl.get_obj_position_3D(self.vehicles[self.total_vhc_count])
# We use current vehicle index as its id as well:
self.vehicle_dictionary[self.vehicles[
self.total_vhc_count].def_name] = self.total_vhc_count
if vehicle_type == self.VHC_VUT:
self.VUT_dictionary[self.vehicles[
self.total_vhc_count].def_name] = self.total_vhc_count
if vehicle_type == self.VHC_DUMMY:
self.dummy_vhc_dictionary[self.vehicles[
self.total_vhc_count].def_name] = self.total_vhc_count
self.total_vhc_count += 1
def update_vehicle_states(self, vhc):
"""Update the current states of the vehicle."""
vhc.current_position = self.supervisorControl.get_obj_position_3D(vhc)
if vhc.previous_position is None:
curr_velocity = 0.0
else:
curr_velocity = (
(vhc.current_position[0] - vhc.previous_position[0]) /
(self.time_step * float(self.num_of_steps_for_jerk_calc)))
curr_acc = (curr_velocity - vhc.previous_velocity) / (
self.time_step * float(self.num_of_steps_for_jerk_calc))
self.acc_arr[self.cur_jerk_compute_index] = curr_acc
if self.step_counter > self.num_of_steps_for_jerk_calc + 3:
cur_acc_avg = (
self.acc_arr[self.cur_jerk_compute_index] + self.acc_arr[
(self.cur_jerk_compute_index - 1) % len(self.acc_arr)] +
self.acc_arr[(self.cur_jerk_compute_index - 1) %
len(self.acc_arr)]) / 3.0
prev_acc_avg = (self.acc_arr[self.cur_jerk_compute_index -
self.num_of_steps_for_jerk_calc] +
self.acc_arr[(self.cur_jerk_compute_index -
self.num_of_steps_for_jerk_calc - 1)
% len(self.acc_arr)] +
self.acc_arr[(self.cur_jerk_compute_index -
self.num_of_steps_for_jerk_calc - 2)
% len(self.acc_arr)]) / 3.0
curr_jerk = (cur_acc_avg - prev_acc_avg) / (
self.time_step * float(self.num_of_steps_for_jerk_calc))
else:
curr_jerk = 0.0
self.cur_jerk_compute_index = (self.cur_jerk_compute_index + 1) % (
self.num_of_steps_for_jerk_calc + 3)
vhc.previous_position = vhc.current_position[:]
vhc.previous_velocity = curr_velocity
vhc.previous_acceleration = curr_acc
vhc.current_acceleration = curr_acc
vhc.current_jerk = curr_jerk
vhc.current_velocity = self.supervisorControl.get_obj_velocity(vhc)
# print 'computed velocity: {} read velocity: {}'.format(temp_velocity, vhc.current_velocity[0])
# print 'vhc.current_velocity:{} previous_velocity: {}'.format(vhc.current_velocity, previous_velocity)
# np.subtract(vhc.current_velocity[0:3], previous_velocity)/self.time_step
# vhc.current_acceleration = list(vhc.current_acceleration)
# (vhc.current_acceleration[0] - prev_acc[0])/self.time_step
# print 'vhc: {}, current_acceleration = {} prev_acc = {} current_jerk = {}'.format(vhc.def_name,
# vhc.current_acceleration[0], prev_acc[0], vhc.current_jerk)
# vhc.current_jerk = list(vhc.current_jerk)
vhc.speed = math.sqrt(vhc.current_velocity[0]**2 +
vhc.current_velocity[1]**2 +
vhc.current_velocity[2]**2)
vhc.state_record_time = self.current_sim_time
def update_all_vehicles_states(self):
"""Updates the state of the all vehicles."""
for vhc in self.vehicles:
self.update_vehicle_states(vhc)
def get_reference_value(self, ref_index, ref_field, current_sim_time):
"""Get value of the reference field of the indexed vehicle at the given time."""
if ref_index == 0: # reference is time
ret_val = current_sim_time
else:
vhc = self.vehicles[ref_index - 1]
if ref_field == 0:
ret_val = vhc.speed
elif ref_field == 1:
pos = self.supervisorControl.get_obj_position_3D(vhc)
ret_val = pos[0]
elif ref_field == 2:
pos = self.supervisorControl.get_obj_position_3D(vhc)
ret_val = pos[1]
elif ref_field == 3:
pos = self.supervisorControl.get_obj_position_3D(vhc)
ret_val = pos[2]
else:
ret_val = 0.0
return ret_val
def transmit_all_vhc_positions(self, emitter):
"""Transmit all vehicle positions through emitter."""
for vhc in self.vehicles:
self.controller_comm_interface.transmit_vehicle_position_message(
emitter, vhc.id, vhc.current_position)
def transmit_init_controller_params(self, emitter):
"""Transmit the neural network controller parameters."""
if self.has_sent_controller_config is False:
for vhc in self.vehicles:
for c_param in vhc.controller_parameters:
self.controller_comm_interface.transmit_set_controller_parameters_message(
emitter=emitter,
vhc_id=c_param.vehicle_id,
parameter_name=c_param.parameter_name,
parameter_data=c_param.parameter_data)
time.sleep(0.1)
self.has_sent_controller_config = True
def apply_manual_position_control(self, vhc_id):
"""Manually control the position of the vehicle."""
vhc = self.vehicles[self.dummy_vhc_dictionary[vhc_id]]
pos = self.supervisorControl.get_obj_position_3D(vhc)
for s in vhc.signal:
reference_value = self.get_reference_value(s.ref_index,
s.ref_field,
self.current_sim_time)
signal_value = s.get_signal_value_corresponding_to_value_of_reference(
reference_value, STaliroSignal.INTERPOLATION_TYPE_NONE)
# print("reference_value: {} s.ref_index: {} s.ref_field: {} signal_value: {}".format(reference_value,
# s.ref_index, s.ref_field, signal_value));
if s.signal_type == s.SIGNAL_TYPE_SPEED:
# When I change speed aggressively, the vehicle rolls over. Look into this.
# spd = self.get_velocity(vhc)
# spd[0] = signal_value
# self.set_velocity(vhc, spd)
pos[0] = pos[0] + signal_value * self.time_step
# print("SIGNAL_TYPE_SPEED : signal_value : {} pos: {}".format(signal_value, pos));
self.supervisorControl.set_obj_position_3D(vhc, pos)
if s.signal_type == s.SIGNAL_TYPE_Y_POSITION:
pos[2] = signal_value
# print("SIGNAL_TYPE_Y_POSITION : signal_value: {} pos: {}".format(signal_value, pos));
self.supervisorControl.set_obj_position_3D(vhc, pos)
def set_time_step(self, time_step):
"""Set the time_step."""
self.time_step = time_step
self.num_of_steps_for_jerk_calc = int(math.ceil(0.05 / time_step))
print('Num of steps for jerk calculation = {}'.format(
self.num_of_steps_for_jerk_calc))
self.acc_arr = [0.0] * (self.num_of_steps_for_jerk_calc + 3)
def get_emitter(self):
"""Returns the supervisor emitter"""
supervisor_emitter = self.supervisorControl.get_emitter(
self.EMITTER_NAME)
return supervisor_emitter
def simulate_vehicles(self, current_sim_time_s):
"""Simulation vehicles for one time step."""
self.current_sim_time = current_sim_time_s
control_type = 0
supervisor_emitter = self.get_emitter()
self.update_all_vehicles_states()
self.transmit_all_vhc_positions(supervisor_emitter)
self.transmit_init_controller_params(supervisor_emitter)
if control_type == 0:
for vhc_id in self.dummy_vhc_dictionary:
self.apply_manual_position_control(vhc_id)
self.step_counter += 1
def __init__(self):
self.debug_mode = 0
self.controller_comm_interface = ControllerCommunicationInterface()
class SimulationMessageInterface(object):
"""SimulationMessageInterface class understands the command types,
and acts as an interpreter between the application and the communication server."""
START_SIM = 1
RELOAD_WORLD = 2
SET_HEART_BEAT_CONFIG = 3
CONTINUE_SIM = 4
SET_ROBUSTNESS_TYPE = 5
ADD_DATA_LOG_DESCRIPTION = 6
SET_VIEW_FOLLOW_ITEM = 7
GET_ROBUSTNESS = 8
GET_DATA_LOG_INFO = 9
GET_DATA_LOG = 10
SET_CONTROLLER_PARAMETER = 11
SET_DATA_LOG_PERIOD_MS = 12
SURROUNDINGS_ADD = 100
SURROUNDINGS_DEF = 101
S_ROAD = 102
STRAIGHT_ROAD = 103
DUMMY_ACTORS_ADD = 120
DUMMY_ACTORS_DEF = 121
D_VHC = 122
VUT_ADD = 130
VUT_DEF = 131
VUT_VHC = 132
ROBUSTNESS = 201
HEART_BEAT = 202
DATA_LOG_INFO = 203
DATA_LOG = 204
ACK = 250
def __init__(self):
self.debug_mode = 0
self.controller_comm_interface = ControllerCommunicationInterface()
def interpret_message(self, msg):
"""Extracts the command and object information from the given raw msg."""
obj = None
(command, ) = struct.unpack('B', msg[0:struct.calcsize('B')])
cur_msg_index = struct.calcsize('B')
if self.debug_mode:
print("SimulationMessageInterface : msg length:{}".format(len(msg)))
if command == self.ACK:
obj = None
elif command == self.CONTINUE_SIM:
obj = None
elif command == self.HEART_BEAT:
(sim_status,) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
(sim_time_ms, ) = struct.unpack('I', msg[cur_msg_index:cur_msg_index + struct.calcsize('I')])
obj = HeartBeat(simulation_status=sim_status, simulation_time_ms=sim_time_ms)
elif command == self.SET_CONTROLLER_PARAMETER:
obj = self.interpret_set_controller_parameter_command(msg)
elif command == self.SET_DATA_LOG_PERIOD_MS:
(obj, ) = struct.unpack('I', msg[cur_msg_index:cur_msg_index + struct.calcsize('I')])
elif command in (self.SURROUNDINGS_DEF, self.SURROUNDINGS_ADD):
(item_type, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
if item_type == self.S_ROAD:
if self.debug_mode:
print("Road")
obj = WebotsRoad()
(obj.position[0],
obj.position[1],
obj.position[2],
obj.rotation[0],
obj.rotation[1],
obj.rotation[2],
obj.rotation[3],
obj.length,
obj.width,
road_type,
obj.number_of_lanes,
obj.right_border_bounding_object,
obj.left_border_bounding_object) = \
struct.unpack('dddddddddBB??', msg[cur_msg_index:cur_msg_index + struct.calcsize('dddddddddBB??')])
cur_msg_index += struct.calcsize('dddddddddBB??')
if self.debug_mode:
print("SimulationMessageInterface: rd length: {}".format(obj.length))
if road_type == self.STRAIGHT_ROAD:
obj.road_type = "StraightRoadSegment"
else:
print("SimulationMessageInterface: Unknown SURROUNDINGS DEF {}".format(item_type))
elif command in (self.DUMMY_ACTORS_DEF, self.DUMMY_ACTORS_ADD, self.VUT_DEF, self.VUT_ADD):
(item_type, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
if (((command in (self.DUMMY_ACTORS_DEF, self.DUMMY_ACTORS_ADD)) and item_type == self.D_VHC)
or ((command in (self.VUT_DEF, self.VUT_ADD)) and item_type == self.VUT_VHC)):
obj = WebotsVehicle()
(obj.current_position[0],
obj.current_position[1],
obj.current_position[2],
obj.current_rotation[0],
obj.current_rotation[1],
obj.current_rotation[2],
obj.current_rotation[3],
obj.id,
vhc_model,
obj.controller) = struct.unpack('dddddddB25s30s',
msg[cur_msg_index:cur_msg_index + struct.calcsize('dddddddB25s30s')])
cur_msg_index += struct.calcsize('dddddddB25s30s')
vhc_model = vhc_model.rstrip(' \t\r\n\0') # Remove null characters at the end
vhc_model = vhc_model.strip() # Remove space characters
obj.set_vehicle_model(vhc_model)
if command in (self.DUMMY_ACTORS_DEF, self.DUMMY_ACTORS_ADD):
obj.def_name = "DVHC_" + str(obj.id)
else:
obj.def_name = "VUT_" + str(obj.id)
obj.controller = obj.controller.rstrip('\0')
if self.debug_mode:
print("SimulationMessageInterface : Adding vhc: Controller: {}, length: {}".format(
obj.controller, len(obj.controller)))
# Read Vehicle Parameters to be used in proto settings
(num_of_params, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
if self.debug_mode:
print("SimulationMessageInterface: Adding vhc: numOf vehicle Params {}".format(num_of_params))
for i in range(num_of_params):
(length_of_setting, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
param_name_str = ''
if length_of_setting > 0:
param_name_str += msg[cur_msg_index:cur_msg_index + length_of_setting]
cur_msg_index += length_of_setting
(length_of_setting, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
param_str = ''
if length_of_setting > 0:
param_str += msg[cur_msg_index:cur_msg_index + length_of_setting]
cur_msg_index += length_of_setting
obj.vehicle_parameters.append((param_name_str, param_str))
# Read Controller Arguments additional to vehicle type
(num_of_contr_arguments, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
if self.debug_mode:
print("SimulationMessageInterface: Adding vhc: num_of_contr_arguments {}".format(
num_of_contr_arguments))
for i in range(num_of_contr_arguments):
(length_of_setting, ) = struct.unpack('I', msg[cur_msg_index:cur_msg_index + struct.calcsize('I')])
cur_msg_index += struct.calcsize('I')
if length_of_setting > 0:
argument_str = msg[cur_msg_index:cur_msg_index + length_of_setting]
cur_msg_index += length_of_setting
obj.controller_arguments.append(argument_str)
# Read signals
(num_of_signals, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
obj.signal = []
if self.debug_mode:
print("SimulationMessageInterface: Adding vhc: num_of_signals {}".format(num_of_signals))
for i in range(0, num_of_signals):
(signal_type, interpolation_type, signal_ref_index, signal_ref_field, signal_val_count) = \
struct.unpack('BBBBh', msg[cur_msg_index:cur_msg_index + struct.calcsize('BBBBh')])
cur_msg_index += struct.calcsize('BBBBh')
signal_values = []
reference_values = []
for j in range(0, signal_val_count):
(sig_val, ) = struct.unpack('d', msg[cur_msg_index:cur_msg_index + struct.calcsize('d')])
signal_values.append(sig_val)
cur_msg_index += struct.calcsize('d')
for j in range(0, signal_val_count):
ref_val = struct.unpack('d', msg[cur_msg_index:cur_msg_index + struct.calcsize('d')])
reference_values.append(ref_val)
cur_msg_index += struct.calcsize('d')
obj.signal.append(STaliroSignal(signal_type,
interpolation_type,
signal_ref_index,
signal_ref_field,
signal_values,
reference_values))
if self.debug_mode:
print("SimulationMessageInterface: Added Signal")
# Read Sensors
(num_of_sensors, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
if self.debug_mode:
print("SimulationMessageInterface: Adding vhc: num_of_sensors {}".format(num_of_sensors))
cur_msg_index += struct.calcsize('B')
obj.sensor_array = [WebotsSensor() for i in range(num_of_sensors)]
for i in range(0, num_of_sensors):
(obj.sensor_array[i].sensor_location, ) = \
struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
(len_of_type, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
obj.sensor_array[i].sensor_type = msg[cur_msg_index:cur_msg_index + len_of_type]
cur_msg_index += len_of_type
(len_of_field_name, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
field_index = 0
while len_of_field_name > 0:
temp_field_name = msg[cur_msg_index:cur_msg_index + len_of_field_name]
cur_msg_index += len_of_field_name
(len_of_field_val, ) = \
struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
temp_field_val = msg[cur_msg_index:cur_msg_index + len_of_field_val]
cur_msg_index += len_of_field_val
obj.sensor_array[i].add_sensor_field(temp_field_name, temp_field_val)
(len_of_field_name, ) = \
struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
field_index += 1
# Read Controller Parameters (NOT arguments!)
(num_of_control_params, ) = struct.unpack('B', msg[cur_msg_index:cur_msg_index + struct.calcsize('B')])
cur_msg_index += struct.calcsize('B')
obj.controller_parameters = []
if self.debug_mode:
print("SimulationMessageInterface: Adding vhc : num_of_control_params {}".format(
num_of_control_params))
for i in range(0, num_of_control_params):
(controller_param, param_msg_size) = \
self.controller_comm_interface.interpret_controller_parameter_message(msg[cur_msg_index:])
cur_msg_index += param_msg_size
controller_param.set_vehicle_id(obj.id)
obj.controller_parameters.append(controller_param)
if self.debug_mode:
print("SimulationMessageInterface: Added Controller Parameter.")
else:
print('SimulationMessageInterface: UNEXPECTED DUMMY ACTOR')
elif command == self.SET_ROBUSTNESS_TYPE:
(obj, ) = struct.unpack('I', msg[cur_msg_index:cur_msg_index + struct.calcsize('I')])
elif command == self.ADD_DATA_LOG_DESCRIPTION:
obj = ItemDescription()
(obj.item_type, obj.log_item_index, obj.item_state_index) = \
struct.unpack('BBB', msg[cur_msg_index:cur_msg_index + struct.calcsize('BBB')])
elif command == self.START_SIM:
obj = SimData()
(obj.simulation_duration_ms, obj.simulation_step_size_ms, obj.simulation_execution_mode) = \
struct.unpack('IIB', msg[cur_msg_index:cur_msg_index + struct.calcsize('IIB')])
print("SimulationMessageInterface: Simulation Duration: {} step size: {} type: {}".format(
obj.simulation_duration_ms,
obj.simulation_step_size_ms,
obj.simulation_execution_mode))
elif command == self.RELOAD_WORLD:
obj = None
print("SimulationMessageInterface: Revert world")
elif command == self.GET_ROBUSTNESS:
obj = None
elif command == self.GET_DATA_LOG_INFO:
obj = None
elif command == self.GET_DATA_LOG:
(log_start_index, log_end_index) = \
struct.unpack('II', msg[cur_msg_index:cur_msg_index + struct.calcsize('II')])
obj = (log_start_index, log_end_index)
elif command == self.DATA_LOG_INFO:
(num_log, size_of_each_log) = struct.unpack('II', msg[cur_msg_index:cur_msg_index + struct.calcsize('II')])
obj = (num_log, size_of_each_log)
elif command == self.DATA_LOG:
(num_data, ) = struct.unpack('I', msg[cur_msg_index:cur_msg_index + struct.calcsize('I')])
cur_msg_index += struct.calcsize('I')
obj = np.fromstring(msg[cur_msg_index:], dtype='d%s' % num_data)
elif command == self.SET_HEART_BEAT_CONFIG:
obj = HeartBeatConfig()
(obj.sync_type, obj.period_ms) = \
struct.unpack('II', msg[cur_msg_index:cur_msg_index + struct.calcsize('II')])
print("Heart Beat Type: {} Period: {}".format(obj.sync_type, obj.period_ms))
elif command == self.SET_VIEW_FOLLOW_ITEM:
obj = ItemDescription()
(obj.item_type, obj.item_index) = \
struct.unpack('BB', msg[cur_msg_index:cur_msg_index + struct.calcsize('BB')])
elif command == self.ROBUSTNESS:
(obj, ) = struct.unpack('d', msg[cur_msg_index:cur_msg_index + struct.calcsize('d')])
else:
print("SimulationMessageInterface: Unknown COMMAND {}".format(command))
ret_cmd = SimulationCommand(command, obj)
return ret_cmd
def generate_ack_message(self):
"""Creates ACKNOWLEDGEMENT message to be sent."""
msg = struct.pack('B', self.ACK)
return msg
def generate_continue_sim_command(self):
command = struct.pack('B', self.CONTINUE_SIM)
return command
def generate_robustness_msg(self, robustness):
"""Creates robustness message with the given robustness value."""
msg = struct.pack('B', self.ROBUSTNESS)
msg += struct.pack('d', robustness)
return msg
def generate_set_heart_beat_config_command(self, sync_type, heart_beat_period_ms):
command = struct.pack('B', self.SET_HEART_BEAT_CONFIG)
command += struct.pack('II', sync_type, heart_beat_period_ms)
return command
def generate_set_data_log_period_ms_command(self, data_log_period_ms):
command = struct.pack('B', self.SET_DATA_LOG_PERIOD_MS)
command += struct.pack('I', data_log_period_ms)
return command
def generate_heart_beat_message(self, current_simulation_time_ms, simulation_status):
"""Creates heartbeat message with the given simulation time."""
msg = struct.pack('B', self.HEART_BEAT)
msg += struct.pack('B', simulation_status)
msg += struct.pack('I', current_simulation_time_ms)
return msg
def generate_start_simulation_command(self, duration, step_size, sim_type):
command = struct.pack('B', self.START_SIM)
command += struct.pack('IIB', duration, step_size, sim_type)
return command
def generate_restart_simulation_command(self):
command = struct.pack('B', self.RELOAD_WORLD)
return command
def generate_set_robustness_type_command(self, robustness_type):
command = struct.pack('B', self.SET_ROBUSTNESS_TYPE)
command += struct.pack('I', robustness_type)
return command
def generate_set_view_follow_item_command(self, item_type, item_index):
command = struct.pack('B', self.SET_VIEW_FOLLOW_ITEM)
command += struct.pack('BB', item_type, item_index)
return command
def generate_add_data_log_description_command(self, item_type, item_index, item_state_index):
command = struct.pack('B', self.ADD_DATA_LOG_DESCRIPTION)
command += struct.pack('BBB', item_type, item_index, item_state_index)
return command
def generate_get_log_info_command(self):
command = struct.pack('B', self.GET_DATA_LOG_INFO)
return command
def generate_log_info_message(self, log_info):
command = struct.pack('B', self.DATA_LOG_INFO)
command += struct.pack('II', log_info[0], log_info[1])
return command
def generate_get_data_log_command(self, start_index, end_index):
command = struct.pack('B', self.GET_DATA_LOG)
command += struct.pack('II', start_index, end_index)
return command
def generate_data_log_message(self, data_log):
command = struct.pack('B', self.DATA_LOG)
command += struct.pack('I', data_log.size)
command += data_log.astype('d').tostring()
return command
def generate_add_road_to_simulation_command(self, road_object, is_create):
if is_create:
cmd = self.SURROUNDINGS_ADD
else:
cmd = self.SURROUNDINGS_DEF
msg = struct.pack('B', cmd)
msg += struct.pack('B', self.S_ROAD)
msg += struct.pack('dddddddddBB??',
road_object.position[0],
road_object.position[1],
road_object.position[2],
road_object.rotation[0],
road_object.rotation[1],
road_object.rotation[2],
road_object.rotation[3],
road_object.length,
road_object.width,
self.STRAIGHT_ROAD,
road_object.number_of_lanes,
road_object.right_border_bounding_object,
road_object.left_border_bounding_object)
return msg
def generate_add_vehicle_to_simulation_command(self, vehicle_object, is_dummy, is_create):
if is_dummy and is_create:
cmd = self.DUMMY_ACTORS_ADD
vhc_type = self.D_VHC
elif is_dummy and not is_create:
cmd = self.DUMMY_ACTORS_DEF
vhc_type = self.D_VHC
elif not is_dummy and is_create:
cmd = self.VUT_ADD
vhc_type = self.VUT_VHC
else:
cmd = self.VUT_DEF
vhc_type = self.VUT_VHC
msg = struct.pack('B', cmd)
msg += struct.pack('B', vhc_type)
# Vehicle main structure:
msg += struct.pack('dddddddB25s30s', vehicle_object.current_position[0],
vehicle_object.current_position[1],
vehicle_object.current_position[2],
vehicle_object.current_rotation[0],
vehicle_object.current_rotation[1],
vehicle_object.current_rotation[2],
vehicle_object.current_rotation[3],
vehicle_object.id,
vehicle_object.vehicle_model,
vehicle_object.controller)
# Vehicle Parameters to be used in proto settings. As string.
num_params = len(vehicle_object.vehicle_parameters)
msg += struct.pack('B', num_params)
for (par_name, par_val) in vehicle_object.vehicle_parameters:
msg += struct.pack('B', len(par_name))
msg += struct.pack("%ds" % (len(par_name),), par_name)
msg += struct.pack('B', len(par_val))
msg += struct.pack("%ds" % (len(par_val),), par_val)
# Controller Arguments other than vehicle type. As string.
num_of_controller_arguments = len(vehicle_object.controller_arguments)
msg += struct.pack('B', num_of_controller_arguments)
for s in vehicle_object.controller_arguments:
msg += struct.pack('I', len(s))
msg += struct.pack("%ds" % (len(s),), s)
# Signals related to the vehicle:
num_signals = len(vehicle_object.signal)
msg += struct.pack('B', num_signals)
for s in vehicle_object.signal:
msg += struct.pack('BBBBh',
s.signal_type,
s.interpolation_type,
s.ref_index,
s.ref_field,
len(s.signal_values))
for j in range(len(s.signal_values)):
msg += struct.pack('d', s.signal_values[j])
for j in range(len(s.ref_values)):
msg += struct.pack('d', s.ref_values[j])
# Sensors related to the vehicle:
num_sensors = len(vehicle_object.sensor_array)
msg += struct.pack('B', num_sensors)
for s in vehicle_object.sensor_array:
msg += struct.pack('B', s.sensor_location)
msg += struct.pack('B', len(s.sensor_type))
msg += struct.pack("%ds" % (len(s.sensor_type),), s.sensor_type)
for f in s.sensor_fields:
msg += struct.pack('B', len(f.field_name))
msg += struct.pack("%ds" % (len(f.field_name),), f.field_name)
msg += struct.pack('B', len(f.field_val))
msg += struct.pack("%ds" % (len(f.field_val),), f.field_val)
msg += struct.pack('B', 0) # Length of next field name is 0. This finishes the sensor message
# Controller parameters related to the vehicle:
num_of_controller_parameters = len(vehicle_object.controller_parameters)
msg += struct.pack('B', num_of_controller_parameters)
for c_param in vehicle_object.controller_parameters:
msg += self.controller_comm_interface.generate_controller_parameter_message(c_param.parameter_name,
c_param.parameter_data)
return msg
def generate_set_controller_parameter_command(self, vhc_id=0, parameter_name='N/A', parameter_data=None):
command = struct.pack('B', self.SET_CONTROLLER_PARAMETER)
command += struct.pack('I', vhc_id)
command += self.controller_comm_interface.generate_controller_parameter_message(parameter_name=parameter_name,
parameter_data=parameter_data)
return command
def interpret_set_controller_parameter_command(self, message):
cur_msg_index = struct.calcsize('B')
(vhc_id, ) = struct.unpack('I', message[cur_msg_index:cur_msg_index+struct.calcsize('I')])
cur_msg_index += struct.calcsize('I')
(data, data_length) = \
self.controller_comm_interface.interpret_controller_parameter_message(message[cur_msg_index:])
data.vehicle_id = vhc_id
return data
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()