123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197 |
- import socket
- import struct
- import select
- import time
- from io import BytesIO
- import matplotlib.pyplot as plt
- import matplotlib.image as image
- import construct as c
- import numpy as np
- from PIL import Image
- from db_connecter import UseDatabase
- from memory_storage import MemoryStorage
- from hard_disk_storage import HardDiskStorage
- from PIL import Image
- import ctypes
- import os
- SONAR_MODE = 'lvs34'
- redis_db = MemoryStorage()
- hard_disk_db = HardDiskStorage()
- data_heard = c.Struct(magic_number=c.Int32ul, length_mm=c.Int32ul, type=c.Int32ul)
- sonar_data = c.Struct(
- magic_number=c.Int32ul, # 固定不便,可能用于区分不同设备
- length_mm=c.Int32ul, # 数据包长度
- type=c.Int32ul, # 固定不变,可能用于识别声呐不同状态的数据
- sequence_count=c.Int32ul, # 每个包加一
- num_results=c.Int32ul, # 每包数据量
- pack_count=c.Int32ul, # 累加数据量,可能用于判断当前是第几个包
- length_data_mm=c.Int32ul, # 当前包数据量
- # pack_data=c.Array(c.this.length_data_mm, c.Byte) # 数据,怀疑是声呐数据
- pack_data=c.Bytes(c.this.length_data_mm)
- )
- def is_allowed_save():
- # 从redis获取是否存储标志,允许存储返回时间戳
- res = redis_db.get_value(['flag', 'timestamp', 'num', 'depth', 'temp', 'yaw', 'roll', 'pitch', 'servo_angle', 'img_type'])
- # {'flag': '0', 'timestamp': None, 'num': None, 'depth': '-2.350517511367798', 'temp': '7.96875', 'yaw': '310.625', 'roll': '-0.5', 'pitch': '178.75', 'servo_angle': '5', 'img_type': None}
- if 'flag' in res:
- if res['flag'] == '1':
- return res
- else:
- return False
- else:
- return False
- def get_uuid():
- sql = f"SELECT uuid FROM measurement_data ORDER BY id DESC LIMIT 1;"
- result = hard_disk_db.execute_sql(sql, None)
- if result:
- return result[0]['uuid']
- else:
- return None
- # 接收方
- def Receiver():
- group_ip = '239.254.2.16' # 组地址
- group_port = 50223 # 端口号
- # 创建IPv4/UDP套接字
- sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
- sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
- # 获取本地ip 先获取主机名,在通过主机名获取ip
- # local_ip = socket.gethostbyname(socket.gethostname()) #windows
- local_ip = '0.0.0.0' # ubuntu
- # local_ip = '192.168.1.188' # ubuntu
- # 绑定端口
- sock.bind((local_ip, group_port))
- mreq = struct.pack("=4sl", socket.inet_aton(group_ip), socket.INADDR_ANY)
- while True:
- try:
- # 加入组播组
- # 使用默认的IPV4组播接口
- sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
- break
- except Exception as e:
- print(e)
- time.sleep(1)
- sequence_count = None # 期望的序列号,用于检测是否丢失或乱序
- sonar_data_list = None
- count = 0
- save_img = 0
- path = '/data/bin_data/'
- sonar_temp = -10
- dll = ctypes.CDLL("/home/sencott/sonar/sonar/build/libImageDraw.so")
- while True:
- data, addr = sock.recvfrom(65536) # 接受数值大一点,防止被撑爆
- # print('images', len(data))
- if count < 100:
- count = count + 1
- if len(data) == 96:
- save_img = save_img + 1
- else:
- if save_img > 1:
- redis_db.set_value({'sonar_status': 'open'})
- else:
- redis_db.set_value({'sonar_status': 'close'})
- save_img = 0
- count = 0
- res = data_heard.parse(data)
- if res.magic_number == 2308 and res.type == 393284:
- res = sonar_data.parse(data)
- if res.pack_count == 0:
- sonar_data_list = res.pack_data
- sequence_count = res.sequence_count
- elif sequence_count == res.sequence_count:
- sonar_data_list = sonar_data_list + res.pack_data
- if sonar_data_list != None:
- debug = False
- if debug == True:
- # 显示及存储,调试使用
- if len(sonar_data_list) == res.num_results:
- if SONAR_MODE == 'lvs34':
- img = sonar_data_list[3645:] # lvs34
- elif SONAR_MODE == 'lvs62':
- img = sonar_data_list[1853:] # lvs62
- plt.clf() # 清除上一幅图像
- byte_stream = BytesIO(img)
- img = image.imread(byte_stream, format='jpeg')
- # # 图片保存
- # roiImg = Image.open(byte_stream)
- # roiImg.save('save.jpeg')
- plt.imshow(img)
- plt.pause(0.05) # 暂停0.05秒 这一句是实现动态更新的
- plt.ioff() # 关闭画图的窗口
- # redis_db.set_value({'flag': '0'})
- # if not save_info:
- # continue
- if len(sonar_data_list) == res.num_results:
- # print("数据包大小(字节):", len(sonar_data_list))
- if SONAR_MODE == 'lvs34':
- angle_data = sonar_data_list[68:2112] # lvs34
- img = sonar_data_list[3645:] # lvs34
- elif SONAR_MODE == 'lvs62':
- angle_data = sonar_data_list[68:1088] # lvs62
- img = sonar_data_list[1853:] # lvs62
- # print('save_image',img)
- image = angle_data + img
- # 预览图像
- dll.draw_image(ctypes.c_char_p('/data/tmp/'.encode()), ctypes.c_char_p('tmp'.encode()), image, len(image), 43)
- save_info = is_allowed_save()
- uuid = get_uuid()
- if save_info: # 允许存储
- # 数据库存储
- sql = f"insert into sonar_data (uuid,image_name, depth, temp, yaw, roll, pitch, servo_angle, sonar_temp) " \
- f"values (%s,%s,%s,%s,%s,%s,%s,%s,%s)"
- res_data = redis_db.get_value(["sonar_temp"])
- if res_data['sonar_temp']:
- sonar_temp = res_data['sonar_temp']
- else:
- sonar_temp = -10
- if not uuid:
- continue
- val = (uuid,
- save_info['timestamp'][0:10] + '_' + save_info['img_type'] + '_' + save_info['num'],
- save_info['depth'],
- save_info['temp'], save_info['yaw'], save_info['roll'], save_info['pitch'],
- save_info['servo_angle'], sonar_temp)
- # 二进制文件存储
- img_name = save_info['timestamp'][0:10] + '_' + save_info['img_type'] + '_' + save_info['num']
- f1 = open(path + str(img_name) + '.bin', 'wb')
- f1.write(angle_data + img)
- f1.close()
- hard_disk_db.execute_sql(sql, val)
- # 图像解析存储
- save_path_in = '/data/img_data/'
- save_path_p = ctypes.c_char_p(save_path_in.encode())
- filename_p = ctypes.c_char_p(img_name.encode())
- if save_info['img_type'] == "s":
- mat_ptr = dll.draw_image(save_path_p, filename_p, angle_data + img, len(angle_data + img), 43)
- else:
- mat_ptr = dll.draw_image(save_path_p, filename_p, angle_data + img, len(angle_data + img), 34)
- redis_db.set_value({'flag': '0'}) # 存储完一帧,置零,否则会出现
- # packet_seq_num, total_packets, packet_data = data.split(b":") # 从数据包中解析序号、总包数和数据
- # print(packet_seq_num, total_packets, packet_data)
- # img = BytesIO(data)
- if __name__ == '__main__':
- Receiver()
- # while True:
- # save_info = is_allowed_save()
- # if not save_info:
- # continue
- # print(save_info)
- # conn = UseDatabase()
- # sql = f"insert into sonar_data (image_name, angle, images, depth, temp, yaw, roll, pitch, servo_angle) " \
- # f"values (%s,%s,%s,%s,%s,%s,%s,%s,%s)"
- # val = (save_info['timestamp'] + '_' + save_info['num'], b'123', b'123', save_info['depth'],
- # save_info['temp'], save_info['yaw'], save_info['roll'], save_info['pitch'], save_info['servo_angle'])
- # conn.update(sql, val)
- # redis_db.set_value({'flag': '0'}) # 存储完一帧,置零
|