default='roborio-3223-frc.local',
help='specify ip address of robot')
return parser
parser = setup_options_parser()
args = parser.parse_args()
NetworkTable.setIPAddress(args.robot)
NetworkTable.setClientMode()
NetworkTable.initialize()
file_name = os.path.join(args.output_dir, "structure.jpg")
tmp_name = os.path.join(args.output_dir, "structure.tmp.jpg")
vision = Vision()
vision.mode = 5
vision.setup_mode_listener()
logger = DataLogger(args.log_dir)
now = time.time()
with vision:
while True:
vision.get_depths()
vision.idepth_stats()
vision.set_display()
cv2.imwrite(tmp_name, vision.display)
os.rename(tmp_name, file_name)
logger.log_data(vision.depth, vision.ir)
time.sleep(0.05)
def main():
# display detected goal and distance from it
# in a live window
parser = setup_options_parser()
args = parser.parse_args()
replaying = args.replay_dir is not None
recording = args.record
pclviewer = None
if replaying:
replayer = Replayer(args.replay_dir)
mode = "stopped"
top = 120
left = 160
cv2.namedWindow("View")
cv2.createTrackbar("mode", "View", 0, 7, lambda *args: None)
cv2.createTrackbar("area_threshold", "View", 10, 500,
lambda *args: None)
cv2.createTrackbar("frame", "View", 0, len(replayer.frame_names), lambda *args: None)
with Vision(use_sensor=False) as vision:
while True:
vision.mode = cv2.getTrackbarPos("mode", "View")
vision.area_threshold = cv2.getTrackbarPos("area_threshold", "View")
_frame_i = cv2.getTrackbarPos("frame", "View")
if 0 <= _frame_i < len(replayer.frame_names):
frame_i = _frame_i
vision.get_recorded_depths(replayer, frame_i)
vision.idepth_stats()
vision.set_display()
if mode == "stopped" and vision.mode == 4:
cv2.rectangle(
vision.display, (left, top), (left + 10, top+10),
(255, 0, 0))
if mode != "stopped" and pclviewer is not None:
pclviewer.updateCloud(vision.xyz)
cv2.imshow("View", vision.display)
wait_delay = 50
if mode == "fw" and frame_i < len(replayer.frame_names) - 1:
cv2.setTrackbarPos("frame", "View", frame_i+1)
wait_delay = replayer.offset_milis(frame_i)
elif mode == "bw" and 0 < frame_i:
cv2.setTrackbarPos("frame", "View", frame_i-1)
wait_delay = replayer.offset_milis(frame_i-1)
x = cv2.waitKey(wait_delay)
if x % 128 == 27:
break
elif ord('0') <= x <= ord('7'):
cv2.setTrackbarPos("mode", "View", x - ord('0'))
elif ord('`') == x:
cv2.setTrackbarPos("mode", "View", 0)
elif ord('s') == x:
mode = "stopped"
elif ord('f') == x:
mode = 'fw'
elif ord('b') == x:
mode = 'bw'
elif ord('p') == x:
print(replayer.file_name(frame_i))
elif ord('i') == x:
cv2.imwrite("plop.jpg", vision.display);
elif ord('z') == x and libpclproc is not None:
if pclviewer is None:
pclviewer = libpclproc.process(vision.xyz)
else:
pclviewer.close()
pclviewer = None
if pclviewer is not None and not pclviewer.wasStopped():
pclviewer.spin()
if mode == "stopped" and vision.mode == 4:
if x == 65361:
# left arrow key
left -= 2
elif x == 65362:
# up arrow key
top -= 2
elif x == 65363:
# right arrow key
left += 2
elif x == 65364:
# down arrow key
top += 2
elif x == ord('p'):
print('x: ', vision.xyz[0, top:top+10, left:left+10])
print('y: ', vision.xyz[1, top:top+10, left:left+10])
print('z: ', vision.xyz[2, top:top+10, left:left+10])
cv2.destroyWindow("View")
else:
logger = DataLogger("logs")
if recording:
logger.begin_logging()
cv2.namedWindow("View")
cv2.createTrackbar("mode", "View", 0, 7, lambda *args: None)
'''
cv2.createTrackbar("area_threshold", "View", 10, 500,
lambda *args: None)
'''
cv2.createTrackbar("angle", "View", 0, 90,
lambda *args: None)
cv2.createTrackbar("velocity", "View", 1000, 10000,
lambda *args: None)
with Vision() as vision:
while True:
vision.mode = cv2.getTrackbarPos("mode", "View")
#vision.area_threshold = cv2.getTrackbarPos("area_threshold", "View")
vision.angle = cv2.getTrackbarPos("angle", "View")
vision.get_depths()
vision.idepth_stats()
vision.set_display()
logger.log_data(vision.depth, vision.ir)
cv2.imshow("View", vision.display)
x = cv2.waitKey(50)
if x % 128 == 27:
break
elif ord('0') <= x <= ord('7'):
cv2.setTrackbarPos("mode", "View", x - ord('0'))
elif ord('`') == x:
cv2.setTrackbarPos("mode", "View", 0)
cv2.destroyWindow("View")
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()
