from multiprocessing import Process # from event_storage import EventStorage from hard_disk_storage import HardDiskStorage from memory_storage import MemoryStorage from sonar_socket2 import Receiver from sonar_temp_tcp import ClientTemp from temp_sonar import Receiver1 from sonar_udp import sender from sonar_command_tcp_once import Client import socket import struct import time import construct as c import redis import logging from logging.handlers import TimedRotatingFileHandler servo_angle = 'servo1_angle' MIN_ANGLE = 1 MAX_ANGLE = 358 SYSTEM_MODE = 'once' # once,multiple hard_disk_db = HardDiskStorage() servo_data = c.Struct( header=c.Int16ul, # 固定不便，可能用于区分不同设备 none_1=c.Int8ub, # 固定不便，可能用于区分不同设备 servo1_flag=c.Int8ub, none_2=c.Int8ub, # 固定不便，可能用于区分不同设备 servo2_flag=c.Int8ub, depth=c.Bytes(4), temp=c.Bytes(4), yaw=c.Bytes(4), # 航向 roll=c.Bytes(4), # 横滚 pitch=c.Bytes(4), # 俯仰 servo1_angle=c.Bytes(4), servo2_angle=c.Bytes(4), water_leak1=c.Bytes(4), water_leak2=c.Bytes(4), ) def get_program_status(): sql = "SELECT time_end FROM measurement_data ORDER BY id desc limit 1" res = hard_disk_db.execute_sql(sql, None) if res: if res[0]['time_end']: return "off" else: return "on" else: return "off" class MyProcess(Process): # 继承Process类 def __init__(self, name, config): super(MyProcess, self).__init__() self.name = name self.__sock = None self.__ip = config['ip'] self.__port = config['port'] self.__save_run_time = 0 # self.__converter = converter # self.__storager = MemoryStorage({'ip':'127.0.0.1','port':4001}) # self.__save_frequency = config['save_frequency'] self.__save_frequency = 300 # 最小值120 self.redis_db = None self.__connected = False self.run_count = 0 self.servo_status = None self.proc_status = get_program_status() def run(self): self.__connect() self.__connected = True self.redis_db = MemoryStorage() self.redis_db.set_value_permanent({'operation_mode': 'auto'}) self.redis_db.set_value_permanent({'operation_init_move': '0'}) self.redis_db.set_value_permanent({'operation_move_flag': '0'}) self.redis_db.set_value_permanent({'operation_move_angel': '0'}) while True: speed = 30 acceleration = 2000 operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode'] operation_init_move = self.redis_db.get_value(['operation_init_move'])['operation_init_move'] status = self.get_sonar_status() # self.redis_db.set_value_permanent({'newset_angel': curr_angel}) try: curr_angel = int(status['servo1_angle']) curr_depth = round(status['depth']) except: print('get sonar status error') continue info_dict = {'depth': curr_depth, 'servo_angle': curr_angel} self.redis_db.set_value(info_dict) print('fang --> operation_mode:', operation_mode) if operation_mode == 'manual': time_flag = 0 temp_now_time = int(time.time()) operation_save_time = self.redis_db.get_value(['operation_save_time'])['operation_save_time'] print('fang --> operation_save_time:', operation_save_time) print('fang --> temp_now_time:', temp_now_time) print('fang --> operation_init_move:', operation_init_move) if operation_save_time != None: print('fang --> cha:', temp_now_time - int(operation_save_time)) if operation_save_time != None and (temp_now_time - int(operation_save_time)) > 300: self.redis_db.set_value_permanent({'operation_mode': 'auto'}) continue dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) if operation_init_move == '0': self.send_command(location=curr_angel, speed=speed, acceleration=acceleration) self.redis_db.set_value_permanent({'operation_init_move': '1'}) operation_move_angel = self.redis_db.get_value(['operation_move_angel'])['operation_move_angel'] operation_move_flag = self.redis_db.get_value(['operation_move_flag'])['operation_move_flag'] print('fang --> operation_move_angel:', operation_move_angel) print('fang --> operation_move_flag:', operation_move_flag) print('fang --> curr_angel:', curr_angel) if operation_move_angel != None and operation_move_flag == '1': after_angel = int(operation_move_angel) + curr_angel if int(operation_move_angel) > 0: after_angel += 1 if after_angel > 358: after_angel = 358 elif after_angel < 1: after_angel = 1 print('fang --> after_angel:', after_angel) self.send_command(location=after_angel, speed=speed, acceleration=acceleration) # time.sleep(0.2) time_flag = 1 self.redis_db.set_value_permanent({'operation_move_angel': '0'}) self.redis_db.set_value_permanent({'operation_move_flag': '0'}) if time_flag == 0: time.sleep(1) print('fang --> --> --> --> --> --> --> --> -->') self.__save_run_time = 0 continue status = self.get_sonar_status() # 发查询指令，当作心跳包。 info_dict = {'depth': status['depth'], 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], 'pitch': status['pitch'], 'servo_angle': status[servo_angle], 'water_leak1': status['water_leak1'], 'water_leak2': status['water_leak2']} self.redis_db.set_value(info_dict) now_time = time.time() switch = get_program_status() print(self.proc_status, "-->", switch) if self.proc_status == 'on' and switch == 'off': dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) time.sleep(3) self.proc_status = switch continue elif self.proc_status == 'off' and switch == 'off': dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) time.sleep(3) continue elif self.proc_status == 'off' and switch == 'on': self.__save_run_time = 0 self.proc_status = switch else: pass if now_time - self.__save_run_time >= self.__save_frequency - 10: # 提前开启声纳 print('sonar_begin---------------------------', time.time()) if SYSTEM_MODE == 'once': dict = {'sonar_open': 'open', 'range': 'small', 'flag': '0'} elif SYSTEM_MODE == 'multiple': dict = {'sonar_open': 'open', 'range': 'small', 'flag': '0'} self.redis_db.set_value(dict) self.rotate_zero(speed, acceleration) print('sonar_status:', self.redis_db.get_value(['sonar_status'])['sonar_status']) if now_time - self.__save_run_time >= self.__save_frequency and self.redis_db.get_value(['sonar_status'])[ 'sonar_status'] == 'open': print('zz', self.redis_db.get_value(['sonar_status'])['sonar_status']) self.__save_run_time = time.time() if SYSTEM_MODE == 'once': print('duoji_runing---------------------------', time.time()) self.rotate_once_small(speed, acceleration, self.redis_db, MAX_ANGLE, self.__save_run_time) self.redis_db.set_value({'range': 'big'}) time.sleep(3) self.rotate_once_big(speed, acceleration, self.redis_db, MIN_ANGLE, self.__save_run_time) self.run_count = self.run_count + 1 # print('run_count',self.run_count) elif SYSTEM_MODE == 'multiple': speed = 30 angle = 10 self.rotate_multiple_small(angle, speed, acceleration, self.__save_run_time, 's') self.redis_db.set_value({'range': 'big'}) time.sleep(3) self.rotate_multiple_big(angle, speed, acceleration, self.__save_run_time, 'b') # 关闭声纳 dict = {'sonar_open': 'close'} self.redis_db.set_value(dict) time.sleep(1) # data_dict = get_sonar_status(self.__sock) # self.__conn.set_value(data_dict) # find_data = b'\xF0\x0F\xFF\xFF\xFF\xFF\x03' # self.__sock.send(find_data) # # # 接收服务器返回的数据 # response = self.__sock.recv(1024) # print(response) # #res = servo_data.parse(response) # while True: # if isinstance(self.__command, list): # for i in self.__command: # command_list = json.loads(i['command']) # self.command_polling(command_list=command_list) # time.sleep(1) # else: # self.command_polling() # time.sleep(1) # # if self.__stopped: # break # 建立socket连接 def __connect(self): if self.__sock: self.__sock.close() self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # 允许重用本地地址和端口 self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) # 在客户端开启心跳维护 self.__sock.settimeout(10) # 设置超时时间3mins try: self.__sock.connect((self.__ip, self.__port)) # logger.info(f'Connect to [{self.name}]:[{self.__ip}]:[{self.__port}] success !') self.__connected = True except Exception as e: # logger.info(f'Connect to [{self.name}]:[{self.__ip}]:[{self.__port}] failed:{e} !!!') self.__connected = False self.__reconnect() def __reconnect(self): while True: try: if self.__sock: self.__sock.close() self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) # 在客户端开启心跳维护 self.__sock.settimeout(10) # 设置超时时间，单位：秒 self.__sock.connect((self.__ip, self.__port)) self.__connected = True # logger.info(f'Reconnect to [{self.name}]:[{self.__ip}]:[{self.__port}] success !') break except Exception as e: # logger.info(f'Reconnect to [{self.name}]:[{self.__ip}]:[{self.__port}] failed:{e} !!! Continue reconnect in 5s..') self.__connected = False time.sleep(5) def __send_command(self, command): if self.__connected: try: # send_data = bytes.fromhex(command) self.__sock.send(command) response = self.__sock.recv(1024) return response except Exception as e: time.sleep(5) self.__reconnect() print('send', e) return None # logger.info(f'Send command to [{self.name}]:[{self.__ip}]:[{self.__port}] error:{e}') else: self.__reconnect() return None def calculate_checksum(self, data): checksum = 0 for byte in data: checksum += byte checksum = (~checksum) & 0xFF byte_data = bytes([checksum]) hex_string = byte_data.hex() return hex_string def data_converter(self, hex_data): servo_data = c.Struct( header=c.Int16ul, # 固定不便，可能用于区分不同设备 none_1=c.Int8ub, # 固定不便，可能用于区分不同设备 servo1_flag=c.Int8ub, none_2=c.Int8ub, # 固定不便，可能用于区分不同设备 servo2_flag=c.Int8ub, depth=c.Bytes(4), temp=c.Bytes(4), yaw=c.Bytes(4), # 航向 roll=c.Bytes(4), # 横滚 pitch=c.Bytes(4), # 俯仰 servo1_angle=c.Bytes(4), servo2_angle=c.Bytes(4), ) res = servo_data.parse(hex_data) return res def get_sonar_status(self): # find_data = b'\xF0\x0F\xFF\xFF\xFF\xFF\x03' # find_data = b'\xF0\x0F\xFF\xFF\xFF\xFF\x03' while True: try: find_data = b'\xF0\x0F\xF0\xF0\xF0\xF0\x03' # 增加漏水检测传感器指令 response = self.__send_command(find_data) result = {'depth': None, 'temp': None, 'yaw': None, 'roll': None, 'pitch': None, 'servo1_angle': None, 'servo2_angle': None, 'water_leak1': None, 'water_leak2': None} if response != None: if len(response) == 35: res = servo_data.parse(response) result['depth'] = struct.unpack('>f', res.depth)[0] result['temp'] = struct.unpack('>f', res.temp)[0] result['yaw'] = struct.unpack('>f', res.yaw)[0] result['roll'] = struct.unpack('>f', res.roll)[0] result['pitch'] = struct.unpack('>f', res.pitch)[0] result['servo1_angle'] = int(struct.unpack('>f', res.servo1_angle)[0]) result['servo2_angle'] = int(struct.unpack('>f', res.servo2_angle)[0]) if len(response) == 43: # 增加漏水检测传感器 res = servo_data.parse(response) result['depth'] = struct.unpack('>f', res.depth)[0] result['temp'] = struct.unpack('>f', res.temp)[0] result['yaw'] = struct.unpack('>f', res.yaw)[0] result['roll'] = struct.unpack('>f', res.roll)[0] result['pitch'] = struct.unpack('>f', res.pitch)[0] result['servo1_angle'] = int(struct.unpack('>f', res.servo1_angle)[0]) result['servo2_angle'] = int(struct.unpack('>f', res.servo2_angle)[0]) result['water_leak1'] = struct.unpack('>f', res.water_leak1)[0] result['water_leak2'] = struct.unpack('>f', res.water_leak2)[0] return result except Exception as e: print('fang --> get sonar error :', e) time.sleep(2) continue def send_command(self, location, speed, acceleration): header = 'f00f' servo1_location = struct.pack('>f', location).hex() servo2_location = struct.pack('>f', location).hex() servo1_speed = struct.pack('>f', speed).hex() servo2_speed = struct.pack('>f', speed).hex() servo1_acceleration = struct.pack('>f', acceleration).hex() servo2_acceleration = struct.pack('>f', acceleration).hex() data = servo1_location + servo2_location + servo1_speed + servo2_speed + servo1_acceleration + servo2_acceleration data_hex = bytes.fromhex(data) checksum = self.calculate_checksum(data_hex) send_data = header + data + checksum send_data = bytes.fromhex(send_data) # print(send_data) # 输出校验和的十六进制表示 response = self.__send_command(send_data) def rotate_zero(self, speed, acceleration): while True: status = self.get_sonar_status() if status[servo_angle] != None: if status[servo_angle] > MIN_ANGLE + 5: print('rotate_zero', status[servo_angle]) self.send_command(location=MIN_ANGLE, speed=speed, acceleration=acceleration) else: break def rotate_once_small(self, speed, acceleration, save_data, location, time_start): count = 0 num = 0 # 图像编号 angel = None save_flag = 0 send_data = True while True: status = self.get_sonar_status() # print(status[servo_angle]) operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode'] if operation_mode == 'manual': dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) break if status[servo_angle] != None: if send_data == True: # time_start = time.time() # 记录开始时间 save_flag = 1 self.send_command(location=location, speed=speed, acceleration=acceleration) send_data = False if save_flag == 1: info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'], 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], 'pitch': status['pitch'], 'img_type': 's', 'servo_angle': status[servo_angle], 'water_leak1': status['water_leak1'], 'water_leak2': status['water_leak2']} save_data.set_value(info_dict) num += 1 if angel != status[servo_angle]: angel = status[servo_angle] count = 0 else: count = count + 1 if status[servo_angle] >= MAX_ANGLE - 1 or count > 4: save_data.set_value({'flag': '0'}) logger.warning('angel error, save image stop , curr range: s . ') break def rotate_once_big(self, speed, acceleration, save_data, location, time_start): count = 0 num = 0 # 图像编号 angel = None save_flag = 0 send_data = True while True: status = self.get_sonar_status() print(status[servo_angle]) operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode'] print('fang --> rotate --> model :', operation_mode) if operation_mode == 'manual': dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) break if status[servo_angle] != None: if send_data == True: # time_start = time.time() # 记录开始时间 save_flag = 1 self.send_command(location=location, speed=speed, acceleration=acceleration) send_data = False if save_flag == 1: info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'], 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], 'pitch': status['pitch'], 'img_type': 'b', 'servo_angle': status[servo_angle], 'water_leak1': status['water_leak1'], 'water_leak2': status['water_leak2'] } save_data.set_value(info_dict) num += 1 if angel != status[servo_angle]: angel = status[servo_angle] count = 0 else: count = count + 1 if status[servo_angle] <= MIN_ANGLE + 0.5 or count > 4: save_data.set_value({'flag': '0'}) logger.warning('angel error, save image stop , curr range: b . ') break def rotate_once_little(self, speed, acceleration, save_data, location, time_start, mode): count = 0 angel = None while True: status = self.get_sonar_status() if status[servo_angle] != None: if location >= MIN_ANGLE and location <= MAX_ANGLE: self.send_command(location=location, speed=speed, acceleration=acceleration) if angel != status[servo_angle]: angel = status[servo_angle] count = 0 else: count = count + 1 if count > 2: info_dict = {'flag': '1', 'timestamp': time_start, 'num': status[servo_angle], 'depth': status['depth'], 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], 'pitch': status['pitch'], 'img_type': mode, 'servo_angle': status[servo_angle], 'water_leak1': status['water_leak1'], 'water_leak2': status['water_leak2'] } save_data.set_value(info_dict) break # def rotate_once(self,speed, acceleration,save_data): # status = self.get_sonar_status() # if status[servo_angle] != None: # if status[servo_angle] > 0.5: # self.send_command(location=0, speed=speed, acceleration=acceleration) # else: # count = 0 # num = 0 # 图像编号 # angel = None # save_image_flag = 0 # while count < 5: # status = self.get_sonar_status() # if status[servo_angle] != None and save_image_flag == 0: # if status[servo_angle] < 0.5: # time_start = time.time() # 记录开始时间 # save_image_flag = 1 # self.send_command(location=359, speed=speed, acceleration=acceleration) # #print(angel,status[servo_angle]) # if angel != status[servo_angle]: # angel = status[servo_angle] # count = 0 # else: # count = count + 1 # if save_image_flag == 2: # count = count + 2 # if status[servo_angle] != None: # if count > 2 or status[servo_angle] >= 358: # save_image_flag = 0 # save_data.set_value({'range':'big'}) # if status[servo_angle] != None: # if count > 2 or status[servo_angle] >= 358: # save_image_flag = 2 # time.sleep(5) # self.send_command(location=0, speed=speed, acceleration=acceleration) # time_now = time.time() # if save_image_flag == 1: # info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'], # 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], # 'pitch': status['pitch'], # 'servo_angle': status[servo_angle]} # save_data.set_value(info_dict) # num += 1 # print('asdasd',111111111111111111111) # elif save_image_flag == 2: # info_dict = {'flag': '1', 'timestamp': time_start, 'num': num, 'depth': status['depth'], # 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], # 'pitch': status['pitch'], # 'servo_angle': status[servo_angle]} # save_data.set_value(info_dict) # num += 1 # print('asdasd', 22222222222222222222222222222222) # else: # info_dict = {'flag': '0', 'timestamp': time_start, 'num': num, 'depth': status['depth'], # 'temp': status['temp'], 'yaw': status['yaw'], 'roll': status['roll'], # 'pitch': status['pitch'], # 'servo_angle': status[servo_angle]} # save_data.set_value(info_dict) # # time_sum = time_end - time_start # 计算的时间差为程序的执行时间，单位为秒/s # # print(time_sum) def rotate_multiple_small(self, angel, speed, acceleration, time_start, mode): t = 0 while True: operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode'] if operation_mode == 'manual': dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) break save_status = self.redis_db.get_value(['flag'])['flag'] if save_status == '0': location_angel = MIN_ANGLE + angel * t if location_angel > MAX_ANGLE: break else: self.rotate_once_little(speed, acceleration, self.redis_db, location_angel, time_start, mode) t = t + 1 def rotate_multiple_big(self, angel, speed, acceleration, time_start, mode): t = 0 while True: operation_mode = self.redis_db.get_value(['operation_mode'])['operation_mode'] if operation_mode == 'manual': dict1 = {'sonar_open': 'open', 'range': 'big', 'flag': '0'} self.redis_db.set_value_permanent(dict1) break save_status = self.redis_db.get_value(['flag'])['flag'] if save_status == '0': location_angel = MAX_ANGLE - angel * t - 10 if location_angel < MIN_ANGLE: break else: self.rotate_once_little(speed, acceleration, self.redis_db, location_angel, time_start, mode) t = t + 1 def rotate_multiple(self, client_socket, angel, speed, acceleration): # 模式二，指定间隔，达到，停顿，继续，完成，停止 status = self.get_sonar_status() if status[servo_angle] != None: if status[servo_angle] > 0.5: self.send_command(location=0, speed=speed, acceleration=acceleration) else: i = int(360 / angel) t = 0 time_start = time.time() # 记录开始时间 while t < i + 1: count = 0 speed = 300 acceleration = 2000 last_angel = None while count < 4: status = self.get_sonar_status() location_angel = min(angel * t, 359) self.send_command(location=location_angel, speed=speed, acceleration=acceleration) if last_angel != status[servo_angle]: last_angel = status[servo_angle] count = 0 else: print(t, last_angel) count = count + 1 if count > 3: t = t + 1 self.send_command(location=0, speed=speed, acceleration=acceleration) if __name__ == '__main__': servo_config = {'ip': '192.168.1.61', 'port': 4001, 'save_frequency': 0} log_handler = TimedRotatingFileHandler(filename='/home/sencott/sonar/log/test_tcp.log', when='midnight', interval=30, backupCount=5, encoding="utf8") # 配置日志记录器 logging.basicConfig(level=logging.WARNING, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s", handlers=[log_handler]) # 获取日志记录器 logger = logging.getLogger() awaken_gls10 = Process(target=sender, name='awaken_gls10', daemon=True) command_p = Client() temp = ClientTemp() save_image = Process(target=Receiver, name='save_image', daemon=True) save_temp = Process(target=Receiver1, name='save_temp', daemon=True) p = MyProcess(name='servo', config=servo_config) # 实例化进程对象 awaken_gls10.start() command_p.daemon = True command_p.start() temp.daemon = True temp.start() save_image.start() save_temp.start() p.daemon = True p.start() while True: time.sleep(1) # print('main..............................', time.time())