sonar/sonar_control/sonar_socket3_test.py

import math
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 = 'lvs62'
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)
)

attitude_data = c.Struct(
    unknown_1_1=c.Float16l,
    unknown_1_21=c.Int8ul,
    unknown_1_22=c.Int8ul,
    unknown_2=c.Float32l,  # 俯仰角：-90°（俯）~ 90°（仰）
    unknown_3=c.Float32l,  # 横滚角: -180（左倾）~ 180°（右倾）
    unknown_4=c.Int32ul,
    unknown_5=c.Float32l,  # 浮点数
    unknown_6=c.Int32ul,
    unknown_7=c.Int32ul,
    unknown_8=c.Int32ul,
    unknown_9=c.Float32l,  # 浮点数
    unknown_10=c.Float32l,  # 浮点数
    unknown_11=c.Int32ul,
    unknown_12=c.Float32l,  # 浮点数
    unknown_13=c.Int32ul,
    unknown_14=c.Int32ul,
    unknown_15=c.Int32ul,
    unknown_16=c.Int32ul,
    unknown_17=c.Int32ul,
)


def angle_parse(raw_data):
    angle_list = []
    for i in range(0, len(raw_data), 4):
        res = c.Struct(angle=c.Float32l, ).parse(raw_data[i:i + 4])
        angle_list.append(res.angle * 180 / math.pi)
    return angle_list


def is_allowed_save():
    # 从redis获取是否存储标志,允许存储返回时间戳
    # res = redis_db.get_value(['flag', 'timestamp', 'num', 'depth', 'temp', 'yaw', 'roll', 'pitch', 'servo_angle', 'img_type'])
    res = {'flag': '1', 'timestamp': str(int(time.time())), 'num': '0', 'depth': '0', 'temp': '0', 'yaw': '0', 'roll': '0', 'pitch': '0', 'servo_angle': '0',
           'img_type': 's'}
    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_test/'
    save_path_in = '/data/img_data_test/'
    sonar_temp = -10
    dll = ctypes.CDLL("/home/sencott/sonar/sonar/build/libImageDraw62.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)
                        from matplotlib import image
                        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':
                        attitude = sonar_data_list[:68]
                        angle_data = sonar_data_list[68:2112]  # lvs34
                        img = sonar_data_list[3645:]  # lvs34
                    elif SONAR_MODE == 'lvs62':
                        attitude = sonar_data_list[:68]
                        angle_data = sonar_data_list[68:1088]  # lvs62
                        img = sonar_data_list[1853:]  # lvs62
                        print(angle_parse(angle_data)[:5], angle_parse(angle_data)[-5:])
                    # 解析俯仰和横滚数据
                    res1 = attitude_data.parse(attitude)
                    pitch = res1.unknown_2 * 180 / math.pi
                    roll = res1.unknown_3 * 180 / math.pi
                    redis_db.set_value({"sonar_pitch": pitch, "sonar_roll": roll})
                    print({"sonar_pitch": pitch, "sonar_roll": roll})
                    # 预览图像
                    dll.draw_image(ctypes.c_char_p('./data/tmp/'.encode()), ctypes.c_char_p('tmp'.encode()), sonar_data_list, len(sonar_data_list), 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, sonar_pitch, sonar_roll) " \
                              f"values (%s,%s,%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, pitch, roll)
                        hard_disk_db.execute_sql(sql, val)
                        # 二进制文件存储
                        img_name = save_info['timestamp'][0:10] + '_' + save_info['img_type'] + '_' + save_info['num']
                        f1 = open(path + str(img_name) + '.bin', 'wb')
                        f1.write(sonar_data_list)
                        f1.close()
                        # 图像解析存储
                        save_path_p = ctypes.c_char_p(save_path_in.encode())
                        filename_p = ctypes.c_char_p(img_name.encode())
                        print(len(sonar_data_list))
                        if save_info['img_type'] == "s":
                            mat_ptr = dll.draw_image(save_path_p, filename_p, sonar_data_list, len(sonar_data_list), 43)
                        else:
                            mat_ptr = dll.draw_image(save_path_p, filename_p, sonar_data_list, len(sonar_data_list), 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'})  # 存储完一帧，置零