sonar/sonar_control/test_tcp_0.9.py

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