sonar/sonar_process/sonar_control/test_tcp.py

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

servo_angle = 'servo1_angle'
MIN_ANGLE = 7
MAX_ANGLE = 347
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),
)


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()
        while True:
            speed = 30
            acceleration = 2000

            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]}
            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': 'big', 'flag': '0'}
                self.redis_db.set_value(dict)
                self.rotate_zero(speed, acceleration)
            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 = 50
                    angle = 20
                    self.rotate_multiple_small(angle, speed, acceleration, self.__save_run_time, 'b')
                    self.redis_db.set_value({'range': 'small'})
                    time.sleep(3)
                    self.rotate_multiple_big(angle, speed, acceleration, self.__save_run_time, 's')

                # 关闭声纳
                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'] = int(struct.unpack('>f', res.servo1_angle)[0])
                result['servo2_angle'] = int(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 + 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])
            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]}
                    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
        send_data = True
        while True:
            status = self.get_sonar_status()
            print(status[servo_angle])
            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]}
                    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_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 status[servo_angle] == location or count > 3:
                    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]}
                    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:
            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:
            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}

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