123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384 |
- 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())
|