from multiprocessing import Process #from event_storage import EventStorage from memory_storage import MemoryStorage from sonar_socket2 import Receiver from sonar_udp import sender from sonar_command_tcp_once import Client import socket import struct import time import construct as c import redis servo_angle = 'servo1_angle' MIN_ANGLE = 5 MAX_ANGLE = 355 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), ) 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 #最小值60 self.redis_db = None self.__connected = False self.run_count = 0 self.servo_status = None def run(self): self.__connect() self.__connected = True self.redis_db = MemoryStorage() while True: speed = 30 acceleration = 2000 status = self.get_sonar_status() #发查询指令，当作心跳包。 print('duoji_runing') now_time = time.time() if now_time - self.__save_run_time >= self.__save_frequency - 10: #提前开启声纳 dict = {'sonar_open': 'open','range':'small'} self.redis_db.set_value(dict) self.rotate_zero(speed,acceleration) if now_time - self.__save_run_time >= self.__save_frequency: self.__save_run_time = 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) # 关闭声纳 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' response = self.__send_command(find_data) result = {'depth':None,'temp':None,'yaw':None,'roll':None,'pitch':None,'servo1_angle':None,'servo2_angle':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'] = struct.unpack('>f', res.servo1_angle)[0] result['servo2_angle'] = struct.unpack('>f', res.servo2_angle)[0] return result 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 + 0.5: print(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 while True: status = self.get_sonar_status() if status[servo_angle] != None: if status[servo_angle] < MIN_ANGLE + 0.5: # time_start = time.time() # 记录开始时间 save_flag = 1 self.send_command(location=location, speed=speed, acceleration=acceleration) 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]} 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 > 3: save_data.set_value({'flag': '0'}) break def rotate_once_big(self, speed, acceleration, save_data, location, time_start): count = 0 num = 0 # 图像编号 angel = None save_flag = 0 while True: status = self.get_sonar_status() if status[servo_angle] != None: if status[servo_angle] >= MAX_ANGLE - 1: # time_start = time.time() # 记录开始时间 save_flag = 1 self.send_command(location=location, speed=speed, acceleration=acceleration) 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]} 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 > 3: save_data.set_value({'flag': '0'}) 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(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.7.61', 'port': 4001, 'save_frequency': 0} awaken_gls10 = Process(target=sender, name='awaken_gls10', daemon=True) command_p = Client() save_image = Process(target=Receiver,name='save_image',daemon=True) p = MyProcess(name='servo',config=servo_config) # 实例化进程对象 awaken_gls10.start() command_p.daemon = True command_p.start() save_image.start() p.daemon = True p.start() while True: time.sleep(1) print('main..............................',time.time())