INSERR/EthernetClient.py at main · IsaacSinn/INSERR · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
import time
import socket
import struct
from pubsub import pub
from ModuleBase import Module

class EthernetClient(Module):
    def __init__(self):
        super().__init__()

        # self.HOST = "169.254.196.165"  # Isaac's Laptop
        # self.HOST = '169.254.243.121' # Silver Laptop
        self.HOST = '192.168.50.110' # Gavin laptop
        self.PORT = 226  # The port used by the server

        self.connect_to_server()

        pub.subscribe(self.message_listener, "ethernet.send")

    def connect_to_server(self):
        while True:
            try:
                self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.socket.connect((self.HOST, self.PORT))

                print(f"Connected to server {self.HOST}")
                break
            except ConnectionRefusedError:
                print('No connection established. Retrying in 3 seconds...')
                time.sleep(3)


    def message_listener(self, message):

        if message["type"] == "CAN":

            START = "X".encode()
            message_type = "CAN".encode()

            address1 = message["address"] >> 8 & 0xff
            address2 = message["address"] & 0xff

            if message["data"] == []:

                data = [address1] + [address2] + [0]
            else:
                data = [address1] + [address2] + message["data"]

            format_string = f"1s1B3s{len(data)}B"

            data_bytes = struct.pack(format_string, START, len(data), type, *data)

        elif message["type"] == "TST":

            START = "X".encode()
            type = "TST".encode()

            time_byte = struct.pack("d", message["data"])
            data_bytes = struct.pack("1s1B3s", START, len(time_byte), type)
            data_bytes = data_bytes + time_byte

        else: # LID, SON, IMU
            pass

        try:
            self.socket.sendall(data_bytes)
        except (BrokenPipeError, ConnectionResetError, ConnectionAbortedError):
            self.socket.close()
            print(f"Disconnected from {self.HOST}")
            self.connect_to_server()

    # receive
    def run(self):
        try:
            data_receive = self.socket.recv(5) # get 2 bytes first, X start of frame, 1 integer length of frame

            if data_receive:
                data = struct.unpack(f"1s1B3s", data_receive)
                if data[0].decode() == "X":
                    frame_length = data[1]
                    type = data[2].decode()
                else:
                    frame_length = 0
            else:
                frame_length = 0

            data_frame = self.socket.recv(frame_length)

            if data_frame:

                if type == "CAN":
                    data = struct.unpack(f"{frame_length}B", data_frame)
                    address, data = data[0], data[1:]
                    #print("client", address, data)
                    pub.sendMessage("can.send", message = {"address": address, "data": data})

                elif type == "TST":
                    data = struct.unpack("d", data_frame)
                    time_rec = data[0]
                    pub.sendMessage("ethernet.send", message = {"type": "TST", "data": time_rec})

                else: # LID, SON, IMU
                    pass

        except (BrokenPipeError, ConnectionResetError, ConnectionAbortedError):
            self.socket.close()
            print(f"Disconnected from {self.HOST}")
            self.connect_to_server()

if __name__ == "__main__":
    from CANHandler import CANHandler
    #from LidarReaderPi import LidarReaderPi
    #from IMUReaderPi import IMUReaderPi
    #from USBCameraClient import USBCamera

    EthernetClient = EthernetClient()
    #USBCamera = USBCamera(0, 'MJPG', 1280, 720, 30)
    CANHandler = CANHandler(250000)

    EthernetClient.start(120) #120
    #USBCamera.start(60)
    CANHandler.start(30)

    #LidarReaderPi = LidarReaderPi()
    #LidarReaderPi.start(1)

    #IMUReaderPi = IMUReaderPi()
    #IMUReaderPi.start(100)