class Main():
'''
Creates object of all sensor types, packs their references into
a list. Creates Communication thread.
'''
def __init__(self):
'''
Creates and stores references of all slave objects.
'''
self.logger = Logger(filename='main_xavier',
title="Main Xavier",
directory=LOG_DIRECOTRY)
self.logger.start()
# cameras
self.camera_client = CameraClient(IP_ADDRESS, name_modifer="_main")
self.logger.log("Cameras created")
#communication
self.communication = Communication()
self.rpi_reference = self.communication.rpi_reference
self.logger.log("communication was established")
# sensors
self.ahrs = AHRS(self.rpi_reference)
self.depth_sensor = DepthSensor(self.rpi_reference)
self.distance_sensor = DistanceSensor(self.rpi_reference)
self.hydrophones = Hydrophones(self.rpi_reference)
self.logger.log("sensors created")
self.sensors = {
'ahrs': self.ahrs,
'depth': self.depth_sensor,
'distance': self.distance_sensor,
'hydrophones': self.hydrophones
}
#control
self.movements = Movements(self.rpi_reference, self.logger)
#self.torpedoes = Torpedoes(self.rpi_reference)
#self.manipulator = Manipulator(self.rpi_reference)
self.dropper = Dropper(self.rpi_reference)
self.logger.log("control objects created")
self.control = {
'movements': self.movements,
#torpedoes': self.torpedoes,
#'manipulator': self.manipulator,
'dropper': self.dropper
}
# task sheduler
self.task_scheduler = TaskSchedululer(self.control, self.sensors,
self.logger)
self.logger.log("Task scheduler created")
def run(self):
self.logger.log("main thread is running")
self.task_scheduler.run()
class CameraClient:
def __init__(self, host=IP_ADDRESS, port=CAMERA_SERVER_PORT, retry_no=5, name_modifer = "", logs=True):
"""
Initialize Camera Client Class
:param host: [String] Server host
:param port: [Int] Server port
:param retry_no: [Int] Number of retries
"""
self.host = host
self.port = port
self.retryNo = retry_no
# set logger file
self.logs = logs
if self.logs:
self.logger = Logger(filename='camera_client'+name_modifer, title="CameraClient", directory=LOG_DIRECOTRY, logexists='append')
self.logger.start()
if not self.__auto_retry(self.__create_socket()):
if self.logs:
self.logger.log(f"ERROR: Create socket failure")
return
if not self.__auto_retry(self.__connect_to_server()):
if self.logs:
self.logger.log(f"ERROR: Connect to server failure")
return
def __create_socket(self):
"""
Create socket for making connection possible
:return: [Bool] True if successful
"""
try:
self.socket = socket.socket()
return True
except socket.error as msg:
if self.logs:
self.logger.log(f'WARNING: Socket creation error: {msg}.')
return False
def __connect_to_server(self):
"""
Establish connection to server
:return: True if successful
"""
try:
if self.logs:
self.logger.log(f"Connecting the port {self.host}:{self.port}")
self.socket.connect((self.host, self.port))
return True
except socket.error as msg:
if self.logs:
self.logger.log(f"WARNING: Socket binding error: {msg}.")
return False
def __auto_retry(self, function):
"""
Auto-retry function
:param function: [Function] Function to try. Require False/0 if unsuccessful
:return: True if finished with retry number of tries
"""
for i in range(self.retryNo):
if function:
return True
elif i == self.retryNo:
if self.logs:
self.logger.log("ERROR: Initialize error. Check logs for more info.")
return False
if self.logs:
self.logger.log(f"Retrying. Try: {i} of {self.retryNo}.")
@property
def frame(self):
"""
Get frame from server
:return: Frame of image
"""
limit = struct.calcsize(">L")
data = b""
self.socket.send("get_frame".encode())
while len(data) < limit:
data += self.socket.recv(4096)
packed_msg_size = data[:limit]
msg_size = struct.unpack(">L", packed_msg_size)[0]
data = data[limit:]
while len(data) < msg_size:
data += self.socket.recv(4096)
frame_data = data[:msg_size]
# data = data[msg_size:]
frame = pickle.loads(frame_data, fix_imports=True, encoding="bytes")
return frame
def change_camera(self,id):
self.socket.send(("change_to:{}".format(id)).encode())
confirmation = self.socket.recv(4096).decode()
if confirmation == 'true':
return 'true'
else:
return 'false'
class ServerXavier:
def __init__(self, host=str(os.system('hostname -I')), port=CAMERA_SERVER_PORT, black_and_white=False, retry_no=5):
"""
Initialize server
:param host: [String] host address
:param port: [Int] port
:param black_and_white: [Bool] Is white and white camera image?
:param retry_no: [Int] Number of retries
"""
self.host = host
self.port = port
self.bw = black_and_white
self.retryNo = retry_no
# set logger file
self.logger = Logger(filename='server_xavier', title="ServerXavier", directory=LOG_DIRECOTRY, logexists='append')
self.logger.start()
# start up camera
front_camera_connected = False
bottom_camera_connected = False
try:
front_camera = cv2.VideoCapture(CAMERAS.FRONT_CAMERA_DEVNAME)
front_camera_connected = True
except:
self.logger.log("Front camera not connected")
try:
bottom_camera = cv2.VideoCapture(CAMERAS.BOTTOM_CAMERA_DEVNAME)
bottom_camera_connected = True
except:
self.logger.log("Bottom camera not connected")
if front_camera_connected and bottom_camera_connected:
self.camerasDict = {"front": front_camera,"bottom": bottom_camera}
self.cameraCapture = self.camerasDict["front"]
elif front_cammera_connected:
self.camerasDict = {"front": front_camera}
self.cameraCapture = self.camerasDict["front"]
elif bottom_camera_connected:
self.camerasDict = {"bottom": bottom_camera}
self.cameraCapture = self.camerasDict["front"]
else:
self.print("No camera connected")
self.logger.log("No camera connected")
if not self.__auto_retry(self.__create_socket()):
self.logger.log(f"ERROR: Create socket failure")
return
if not self.__auto_retry(self.__bind_socket()):
self.logger.log(f"ERROR: Bind socket failure")
return
self.logger.log(f"Init complete")
# variables for videoClient use
self.VIDEO_PATH = ''
self.VIDEO = None
self.VIDEO_FRAME_COUNT = 0
def __create_socket(self):
"""
Create socket for making connection possible
:return: [Bool] True if successful
"""
try:
self.socket = socket.socket()
return True
except socket.error as msg:
self.logger.log(f'WARNING: Socket creation error: {msg}.')
return False
def __bind_socket(self):
"""
Bind selected socket and listen for connections
:return: [Bool] True if successful
"""
try:
self.logger.log(f"Binding the Port {self.port}")
self.socket.bind((self.host, self.port))
self.socket.listen(5)
return True
except socket.error as msg:
self.logger.log(f"WARNING: Socket binding error: {msg}.")
return False
def __auto_retry(self, function):
"""
Auto-retry function
:param function: [Function] Function to try. Require False/0 if unsuccessful
:return: True if finished with retry number of tries
"""
for i in range(self.retryNo):
if function:
return True
elif i == self.retryNo:
self.logger.log("ERROR: Initialize error. Check logs for more info.")
return False
self.logger.log(f"Retrying. Try: {i} of {self.retryNo}.")
def socket_accept(self):
"""
Accept connection from client
:return: None
"""
conn, address = self.socket.accept()
self.logger.log(f"Connection has been established! | {address[0]}:{address[1]}")
threading.Thread(target=self.__handle_client, args=(conn,)).start()
def change_camera(self, id):
if id in self.camerasDict.keys():
self.cameraCapture = self.camerasDict[id]
return True
else:
return False
def __handle_client(self, conn):
"""
Handle client in separate function
:param conn: Client connection data
:return: None
"""
counter = 1
while True:
data = conn.recv(1024).decode()
if not data:
break
elif "change" in data:
if self.change_camera(data.split(':')[1]):
conn.send('true'.encode())
else:
conn.send('false'.decode())
elif "get_frame" in data:
conn.send(self.__frame(source=self.cameraCapture, h_flip=True))
elif "get_video" in data:
# set video variables
if counter == 1:
self.VIDEO_PATH = data.split()[1]
self.VIDEO = cv2.VideoCapture(self.VIDEO_PATH)
self.VIDEO_FRAME_COUNT = self.VIDEO.get(7)
if counter < self.VIDEO_FRAME_COUNT:
conn.send(self.__frame(source=self.VIDEO))
counter += 1
else:
self.VIDEO.release()
self.VIDEO = cv2.VideoCapture(self.VIDEO_PATH)
conn.send(self.__frame(source=self.VIDEO))
counter = 2
conn.close()
def __frame(self, source, v_flip=False, h_flip=False):
"""
Get picture frame
:param v_flip: [Bool] Is image flipped vertical
:param h_flip: [Bool] Is image flipped horizontal
:return: frame
"""
# Capture frame
ret, frame = source.read()
# Handles the mirroring of the current frame
frame = cv2.flip(frame, self.__flip(v_flip, h_flip))
if self.bw:
# Change color to black and white if decided
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Code image
result, frame = cv2.imencode('.jpg', frame)
data = pickle.dumps(frame, 0)
size = len(data)
return struct.pack(">L", size) + data
def __flip(self, v_flip, h_flip):
"""
Get flip parameter
:param v_flip: [Bool] Is image flipped vertical
:param h_flip: [Bool] Is image flipped horizontal
:return: [Int] value for cv2 flip method
"""
if h_flip and v_flip:
return -1
elif v_flip:
return 0
else:
return 1
class Main():
'''
Creates object of all sensor types, packs their references into
a list. Creates Communication thread.
'''
def __init__(self):
'''
Creates and stores references of all slave objects.
'''
self.logger = Logger(filename='main',
title="Main",
directory=DEFLOG.LOG_DIRECTORY)
#Sensors initialization
self.ahrs = None
self.depth = None
self.hydrophones = None
self.distance = None
if SENSORS.AHRS:
self.ahrs = AHRS(port=ports.AHRS_CLIENT_PORT,
main_logger=self.logger,
local_log=DEFLOG.AHRS_LOCAL_LOG,
log_timing=DEFLOG.AHRS_LOG_TIMING,
log_directory=DEFLOG.LOG_DIRECTORY,
mode=MODE)
if SENSORS.DEPTH:
self.depth = DepthSensor(port=ports.DEPTH_CLIENT_PORT,
main_logger=self.logger,
local_log=DEFLOG.DEPTH_LOCAL_LOG,
log_timing=DEFLOG.DEPTH_LOG_TIMING,
log_directory=DEFLOG.LOG_DIRECTORY)
if SENSORS.HYDROPHONES:
self.hydrophones = HydrophonesPair(
port=ports.HYDRO_CLIENT_PORT,
main_logger=self.logger,
local_log=DEFLOG.HYDROPHONES_LOCAL_LOG,
log_timing=DEFLOG.HYDROPHONES_LOG_TIMING,
log_directory=DEFLOG.LOG_DIRECTORY)
if SENSORS.DISTANCE:
self.distance = DistanceSensor(
port=ports.DISTANCE_CLIENT_PORT,
main_logger=self.logger,
local_log=DEFLOG.DISTANCE_LOCAL_LOG,
log_timing=DEFLOG.HYDROPHONES_LOG_TIMING,
log_directory=DEFLOG.LOG_DIRECTORY)
#Controls initialization
self.lights = None
self.manipulator = None
self.torpedoes = None
self.dropper = None
self.movements = Movements(port=ports.ENGINE_SLAVE_PORT,
depth_sensor_ref=self.depth,
ahrs_ref=self.ahrs,
main_logger=self.logger,
local_log=DEFLOG.MOVEMENTS_LOCAL_LOG,
log_directory=DEFLOG.LOG_DIRECTORY)
if CONTROL.LIGHTS:
self.lights = Lights(port=ports.LIGHTS_CLIENT_PORT,
main_logger=self.logger)
if CONTROL.MANIPULATOR:
self.manipulator = Manipulator(port=ports.MANIP_CLIENT_PORT,
main_logger=self.logger)
if CONTROL.TORPEDOES:
self.torpedoes = Torpedoes(
fire_client_port=ports.TORPEDO_FIRE_CLIENT_PORT,
ready_driver_port=ports.TORPEDO_READY_DRIVER_PORT,
main_logger=self.logger,
local_log=DEFLOG.TORPEDOES_LOCAL_LOG,
log_directory=DEFLOG.LOG_DIRECTORY)
if CONTROL.DROPPER:
self.dropper = Dropper(self.logger)
#Run threads, in control for local logers
self.logger.start()
if SENSORS.DEPTH:
self.depth.run()
if SENSORS.AHRS:
self.ahrs.run()
if SENSORS.HYDROPHONES:
self.hydrophones.run()
if SENSORS.DISTANCE:
self.distance.run()
self.movements.run()
if CONTROL.MANIPULATOR:
self.manipulator.run()
if CONTROL.LIGHTS:
self.manipulator.run()
if CONTROL.TORPEDOES:
self.torpedoes.run()
self.sensors_refs = {
'AHRS': self.ahrs,
'DepthSensor': self.depth,
'HydrophonesPair': self.hydrophones,
'DistanceSensor': self.distance,
'Movements': self.movements,
'Lights': self.lights,
'Manipulator': self.manipulator,
'Torpedoes': self.torpedoes,
'dropper': self.dropper
}
#Here you can add more feature classes
#Remeber then to provide proper Communication class methods
self.comm = Communication(self.sensors_refs,
RPI_ADDRESS,
main_logger=self.logger)
'''