autocar3g 0.1.4__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- autocar3g-0.1.4/PKG-INFO +28 -0
- autocar3g-0.1.4/README.md +2 -0
- autocar3g-0.1.4/autocar3g/AI.py +201 -0
- autocar3g-0.1.4/autocar3g/__init__.py +0 -0
- autocar3g-0.1.4/autocar3g/absclient.py +72 -0
- autocar3g-0.1.4/autocar3g/camera.py +170 -0
- autocar3g-0.1.4/autocar3g/driving.py +47 -0
- autocar3g-0.1.4/autocar3g/encoder.py +19 -0
- autocar3g-0.1.4/autocar3g/imu.py +55 -0
- autocar3g-0.1.4/autocar3g/led.py +17 -0
- autocar3g-0.1.4/autocar3g/lidar.py +36 -0
- autocar3g-0.1.4/autocar3g/ultrasonic.py +19 -0
- autocar3g-0.1.4/autocar3g.egg-info/PKG-INFO +28 -0
- autocar3g-0.1.4/autocar3g.egg-info/SOURCES.txt +17 -0
- autocar3g-0.1.4/autocar3g.egg-info/dependency_links.txt +1 -0
- autocar3g-0.1.4/autocar3g.egg-info/requires.txt +6 -0
- autocar3g-0.1.4/autocar3g.egg-info/top_level.txt +1 -0
- autocar3g-0.1.4/setup.cfg +4 -0
- autocar3g-0.1.4/setup.py +28 -0
autocar3g-0.1.4/PKG-INFO
ADDED
|
@@ -0,0 +1,28 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: autocar3g
|
|
3
|
+
Version: 0.1.4
|
|
4
|
+
Summary: Pop plus library for Autocar3G
|
|
5
|
+
Home-page: https://github.com/hanback-lab/Autocar3G
|
|
6
|
+
Author: Hanback Electronics CO. Ltd. Research Insitutue
|
|
7
|
+
Author-email: lab@hanback.co.kr
|
|
8
|
+
Classifier: Programming Language :: Python :: 3
|
|
9
|
+
Classifier: License :: OSI Approved :: MIT License
|
|
10
|
+
Classifier: Operating System :: OS Independent
|
|
11
|
+
Requires-Python: >=3.8
|
|
12
|
+
Description-Content-Type: text/markdown
|
|
13
|
+
Requires-Dist: paho-mqtt
|
|
14
|
+
Requires-Dist: tensorflow>=2.5.0; platform_system != "Linux" or platform_machine != "aarch64"
|
|
15
|
+
Requires-Dist: torch>=1.13.0; platform_system != "Linux" or platform_machine != "aarch64"
|
|
16
|
+
Requires-Dist: ultralytics; platform_system != "Linux" or platform_machine != "aarch64"
|
|
17
|
+
Dynamic: author
|
|
18
|
+
Dynamic: author-email
|
|
19
|
+
Dynamic: classifier
|
|
20
|
+
Dynamic: description
|
|
21
|
+
Dynamic: description-content-type
|
|
22
|
+
Dynamic: home-page
|
|
23
|
+
Dynamic: requires-dist
|
|
24
|
+
Dynamic: requires-python
|
|
25
|
+
Dynamic: summary
|
|
26
|
+
|
|
27
|
+
# Pop plus :: Autocar3 G
|
|
28
|
+
|
|
@@ -0,0 +1,201 @@
|
|
|
1
|
+
import cv2, time, os, sys
|
|
2
|
+
import numpy as np
|
|
3
|
+
# import tensorrt as trt
|
|
4
|
+
import atexit
|
|
5
|
+
import __main__
|
|
6
|
+
|
|
7
|
+
import warnings
|
|
8
|
+
warnings.filterwarnings(action='ignore')
|
|
9
|
+
|
|
10
|
+
IDENTIFIER="_models"
|
|
11
|
+
|
|
12
|
+
mnist_train_x=None
|
|
13
|
+
mnist_train_y=None
|
|
14
|
+
mnist_test_x=None
|
|
15
|
+
mnist_test_y=None
|
|
16
|
+
|
|
17
|
+
#--------------------------------------------- Ondevice AI ---------------------------------------------
|
|
18
|
+
|
|
19
|
+
def import_tensorflow():
|
|
20
|
+
global tf
|
|
21
|
+
import tensorflow as tf
|
|
22
|
+
import tensorflow.keras as __module
|
|
23
|
+
|
|
24
|
+
for k in [m for m in dir(__module) if not "__" in m]:
|
|
25
|
+
globals()[k]=__module.__dict__[k]
|
|
26
|
+
|
|
27
|
+
import tensorflow.keras.models as __module
|
|
28
|
+
|
|
29
|
+
for k in [m for m in dir(__module) if not "__" in m]:
|
|
30
|
+
globals()[k]=__module.__dict__[k]
|
|
31
|
+
|
|
32
|
+
import tensorflow.keras.losses as __module
|
|
33
|
+
|
|
34
|
+
for k in [m for m in dir(__module) if not "__" in m]:
|
|
35
|
+
globals()[k]=__module.__dict__[k]
|
|
36
|
+
|
|
37
|
+
import tensorflow.keras.layers as __module
|
|
38
|
+
|
|
39
|
+
for k in [m for m in dir(__module) if not "__" in m]:
|
|
40
|
+
globals()[k]=__module.__dict__[k]
|
|
41
|
+
|
|
42
|
+
import tensorflow.keras.optimizers as __module
|
|
43
|
+
|
|
44
|
+
for k in [m for m in dir(__module) if not "__" in m]:
|
|
45
|
+
globals()[k]=__module.__dict__[k]
|
|
46
|
+
|
|
47
|
+
import tensorflow.keras.utils as __module
|
|
48
|
+
|
|
49
|
+
for k in [m for m in dir(__module) if not "__" in m]:
|
|
50
|
+
globals()[k]=__module.__dict__[k]
|
|
51
|
+
|
|
52
|
+
gpus=tf.config.experimental.list_physical_devices('GPU')
|
|
53
|
+
|
|
54
|
+
for gpu in gpus:
|
|
55
|
+
tf.config.experimental.set_memory_growth(gpu, True)
|
|
56
|
+
|
|
57
|
+
def bgr8_to_jpeg(value):
|
|
58
|
+
return bytes(cv2.imencode('.jpg', value)[1])
|
|
59
|
+
|
|
60
|
+
from ultralytics import YOLO
|
|
61
|
+
|
|
62
|
+
class Object_Follow():
|
|
63
|
+
def __init__(self, camera):
|
|
64
|
+
|
|
65
|
+
self.camera = camera
|
|
66
|
+
self.boxes_image = None
|
|
67
|
+
|
|
68
|
+
def load_model(self, path):
|
|
69
|
+
self.model = YOLO(path)
|
|
70
|
+
|
|
71
|
+
def detect(self, image=None, index=None, threshold=0.5, callback=None):
|
|
72
|
+
if image is None:
|
|
73
|
+
image=self.camera.read()
|
|
74
|
+
if image is None:
|
|
75
|
+
return
|
|
76
|
+
|
|
77
|
+
if type(index)==str:
|
|
78
|
+
try:
|
|
79
|
+
index=list(self.model.names.values()).index(index)
|
|
80
|
+
except ValueError:
|
|
81
|
+
print("index is not available.")
|
|
82
|
+
return
|
|
83
|
+
|
|
84
|
+
result = self.model.predict(source=image, imgsz=640, verbose=False)[0]
|
|
85
|
+
self.boxes_image = result.plot()
|
|
86
|
+
|
|
87
|
+
width = image.shape[1]
|
|
88
|
+
height = image.shape[0]
|
|
89
|
+
|
|
90
|
+
detections = []
|
|
91
|
+
for b in result.boxes:
|
|
92
|
+
detected_index = int(b.cls[0].cpu().numpy().astype(int))
|
|
93
|
+
xywh = b.xywh[0].cpu().numpy()
|
|
94
|
+
if index is not None:
|
|
95
|
+
if detected_index != index:
|
|
96
|
+
continue
|
|
97
|
+
detections.append({
|
|
98
|
+
'index' : detected_index,
|
|
99
|
+
'label' : self.model.names[detected_index],
|
|
100
|
+
'x' : xywh[0],
|
|
101
|
+
'y' : xywh[1],
|
|
102
|
+
'size_rate' : (xywh[2] * xywh[3]) / (width*height)
|
|
103
|
+
})
|
|
104
|
+
|
|
105
|
+
return detections
|
|
106
|
+
|
|
107
|
+
import tensorflow as tf
|
|
108
|
+
from tensorflow import keras
|
|
109
|
+
from tensorflow.keras import *
|
|
110
|
+
from tensorflow.keras.models import *
|
|
111
|
+
from tensorflow.keras.losses import *
|
|
112
|
+
from tensorflow.keras.layers import *
|
|
113
|
+
from tensorflow.keras.optimizers import *
|
|
114
|
+
from tensorflow.keras.utils import *
|
|
115
|
+
from autocar3g.camera import Camera
|
|
116
|
+
|
|
117
|
+
class Track_Follow_TF():
|
|
118
|
+
MODEL_PATH = 'Track_Follow.h5'
|
|
119
|
+
dataset_path = 'track_dataset'
|
|
120
|
+
BATCH_SIZE = 8
|
|
121
|
+
model=None
|
|
122
|
+
device=None
|
|
123
|
+
datasets=None
|
|
124
|
+
optimizer=None
|
|
125
|
+
prob=None
|
|
126
|
+
probWidget=None
|
|
127
|
+
STAT_DEFINED=0
|
|
128
|
+
STAT_READY=1
|
|
129
|
+
_stat=STAT_DEFINED
|
|
130
|
+
|
|
131
|
+
def __init__(self,camera:Camera):
|
|
132
|
+
self.camera = camera
|
|
133
|
+
import_tensorflow()
|
|
134
|
+
self.default_path=os.path.abspath(__file__+"/../model/Track_Follow/")
|
|
135
|
+
|
|
136
|
+
def _load_layers(self):
|
|
137
|
+
input1 = keras.layers.Input(shape=(150, 400, 3,))
|
|
138
|
+
conv1 = keras.layers.Conv2D(filters=16, kernel_size=(3, 3), strides=(2, 2), padding="same", activation="swish")(input1)
|
|
139
|
+
norm1 = keras.layers.BatchNormalization()(conv1)
|
|
140
|
+
pool1 = keras.layers.MaxPooling2D(pool_size=(3, 3), strides=(2, 2))(norm1)
|
|
141
|
+
conv2 = keras.layers.Conv2D(filters=32, kernel_size=(3, 3), strides=(2, 2), padding="same", activation="swish")(pool1)
|
|
142
|
+
norm2 = keras.layers.BatchNormalization()(conv2)
|
|
143
|
+
conv3 = keras.layers.Conv2D(filters=32, kernel_size=(3, 3), strides=(1, 1), padding="same", activation="swish")(norm2)
|
|
144
|
+
norm3 = keras.layers.BatchNormalization()(conv3)
|
|
145
|
+
add1 = keras.layers.Add()([norm2, norm3])
|
|
146
|
+
conv4 = keras.layers.Conv2D(filters=64, kernel_size=(3, 3), strides=(2, 2), padding="same", activation="swish")(add1)
|
|
147
|
+
norm4 = keras.layers.BatchNormalization()(conv4)
|
|
148
|
+
conv5 = keras.layers.Conv2D(filters=64, kernel_size=(3, 3), strides=(1, 1), padding="same", activation="swish")(norm4)
|
|
149
|
+
norm5 = keras.layers.BatchNormalization()(conv5)
|
|
150
|
+
add2 = keras.layers.Add()([norm4, norm5])
|
|
151
|
+
conv6 = keras.layers.Conv2D(filters=128, kernel_size=(3, 3), strides=(2, 2), padding="same", activation="swish")(add2)
|
|
152
|
+
norm6 = keras.layers.BatchNormalization()(conv6)
|
|
153
|
+
conv7 = keras.layers.Conv2D(filters=128, kernel_size=(3, 3), strides=(1, 1), padding="same", activation="swish")(norm6)
|
|
154
|
+
norm7 = keras.layers.BatchNormalization()(conv7)
|
|
155
|
+
add3 = keras.layers.Add()([norm6, norm7])
|
|
156
|
+
conv8 = keras.layers.Conv2D(filters=256, kernel_size=(3, 3), strides=(2, 2), padding="same", activation="swish")(add3)
|
|
157
|
+
norm7 = keras.layers.BatchNormalization()(conv8)
|
|
158
|
+
conv9 = keras.layers.Conv2D(filters=512, kernel_size=(3, 3), strides=(2, 2), padding="same", activation="swish")(norm7)
|
|
159
|
+
norm8 = keras.layers.BatchNormalization()(conv9)
|
|
160
|
+
flat1 = keras.layers.Flatten()(norm8)
|
|
161
|
+
dense1 = keras.layers.Dense(128, activation="swish")(flat1)
|
|
162
|
+
norm9 = keras.layers.BatchNormalization()(dense1)
|
|
163
|
+
dense2 = keras.layers.Dense(64, activation="swish")(norm9)
|
|
164
|
+
norm10 = keras.layers.BatchNormalization()(dense2)
|
|
165
|
+
dense3 = keras.layers.Dense(64, activation="swish")(norm10)
|
|
166
|
+
norm11 = keras.layers.BatchNormalization()(dense3)
|
|
167
|
+
dense4 = keras.layers.Dense(2, activation="sigmoid")(norm11)
|
|
168
|
+
self.model = keras.models.Model(inputs=input1, outputs=dense4)
|
|
169
|
+
|
|
170
|
+
def load_model(self,path=MODEL_PATH):
|
|
171
|
+
if not os.path.exists(path):
|
|
172
|
+
print(path," doesn't exist.")
|
|
173
|
+
return
|
|
174
|
+
|
|
175
|
+
if self.model is None:
|
|
176
|
+
self._load_layers()
|
|
177
|
+
decay=schedules.ExponentialDecay(1.0e-04, decay_steps=800, decay_rate=0.96, staircase=True)
|
|
178
|
+
adam=Adam(learning_rate=decay, beta_1=0.9, beta_2=0.999, epsilon=1e-07, amsgrad=False)
|
|
179
|
+
self.model.compile(optimizer=adam, loss='MAE')
|
|
180
|
+
|
|
181
|
+
self.model.load_weights(path)
|
|
182
|
+
|
|
183
|
+
def run(self, value=None, callback=None):
|
|
184
|
+
if self.model is None :
|
|
185
|
+
print("Please load datasets as load_datasets() method or load a trained model as load_model() method.")
|
|
186
|
+
return
|
|
187
|
+
|
|
188
|
+
img = self.camera.read()[120:270,:] if value is None else value
|
|
189
|
+
x, y = self.model(np.array([img]).astype(np.float32)).numpy()[0]
|
|
190
|
+
height, width, _ = img.shape
|
|
191
|
+
cX = int(width * (x))
|
|
192
|
+
cY = int(height * (y))
|
|
193
|
+
|
|
194
|
+
self.value = cv2.circle(img, (cX, cY), 6, (255, 255, 255), 2)
|
|
195
|
+
|
|
196
|
+
result={"x":x,"y":y}
|
|
197
|
+
|
|
198
|
+
if callback is not None:
|
|
199
|
+
callback(result)
|
|
200
|
+
|
|
201
|
+
return result
|
|
File without changes
|
|
@@ -0,0 +1,72 @@
|
|
|
1
|
+
import paho.mqtt.client as mqtt
|
|
2
|
+
from threading import Thread, Lock, Event
|
|
3
|
+
import time, os, signal, traceback
|
|
4
|
+
|
|
5
|
+
product_file_path = "product"
|
|
6
|
+
|
|
7
|
+
TIMEOUT_SEC = 5
|
|
8
|
+
|
|
9
|
+
class AbstractPopClient:
|
|
10
|
+
with open(product_file_path, 'r') as file:
|
|
11
|
+
BROKER_DOMAIN = None
|
|
12
|
+
DEV_NUM = None
|
|
13
|
+
DEV_NAME = None
|
|
14
|
+
INSITUTION_NAME = None
|
|
15
|
+
for line in file:
|
|
16
|
+
line = line.strip()
|
|
17
|
+
if line.startswith('BROKER_DOMAIN='):
|
|
18
|
+
BROKER_DOMAIN = line.split('=')[1].strip()
|
|
19
|
+
if line.startswith('DEV_NUM='):
|
|
20
|
+
DEV_NUM = line.split('=')[1].strip()
|
|
21
|
+
if line.startswith('DEVICE_NAME='):
|
|
22
|
+
DEV_NAME = line.split('=')[1].strip()
|
|
23
|
+
if line.startswith('INSITUTION_NAME='):
|
|
24
|
+
INSITUTION_NAME = line.split('=')[1].strip()
|
|
25
|
+
if BROKER_DOMAIN is None:
|
|
26
|
+
raise ValueError("[Error] There is no product file. Please make sure the device has product info")
|
|
27
|
+
|
|
28
|
+
def __init__(self):
|
|
29
|
+
self.__update_lock = Lock()
|
|
30
|
+
self.__update_time_tag = time.time()
|
|
31
|
+
self.__close_event = Event()
|
|
32
|
+
|
|
33
|
+
self._TOPIC_HEADER = __class__.DEV_NAME + '/' + __class__.INSITUTION_NAME + __class__.DEV_NUM
|
|
34
|
+
self._client = mqtt.Client()
|
|
35
|
+
self._client.on_connect = self._on_connect
|
|
36
|
+
self._client.on_message = self._on_message
|
|
37
|
+
self._client.connect(__class__.BROKER_DOMAIN)
|
|
38
|
+
self._client_enabled = False
|
|
39
|
+
self._client.loop_start()
|
|
40
|
+
|
|
41
|
+
wait_time_tag = time.time()
|
|
42
|
+
while not self._client_enabled:
|
|
43
|
+
if time.time() - wait_time_tag > 4:
|
|
44
|
+
raise TimeoutError("Please check the broker connection state.")
|
|
45
|
+
|
|
46
|
+
def __connection_check(self):
|
|
47
|
+
try:
|
|
48
|
+
while True:
|
|
49
|
+
if self.__close_event.set():
|
|
50
|
+
break
|
|
51
|
+
self.__update_lock.acquire()
|
|
52
|
+
if time.time() - self.__update_time_tag > TIMEOUT_SEC:
|
|
53
|
+
raise ConnectionError()
|
|
54
|
+
self.__update_lock.release()
|
|
55
|
+
time.sleep(1)
|
|
56
|
+
except ConnectionError:
|
|
57
|
+
traceback.print_exc()
|
|
58
|
+
os.kill(os.getpid(), signal.SIGINT)
|
|
59
|
+
|
|
60
|
+
def _on_connect(self, client, userdata, flags, rc):
|
|
61
|
+
self.__connection_check_thread = Thread(target=self.__connection_check, daemon=True)
|
|
62
|
+
self.__connection_check_thread.start()
|
|
63
|
+
self._client_enabled = True
|
|
64
|
+
|
|
65
|
+
def _on_message(self, client, userdata, message):
|
|
66
|
+
self.__update_lock.acquire()
|
|
67
|
+
self.__update_time_tag = time.time()
|
|
68
|
+
self.__update_lock.release()
|
|
69
|
+
self._decode(message)
|
|
70
|
+
|
|
71
|
+
def _decode(self, message):
|
|
72
|
+
raise NotImplementedError()
|
|
@@ -0,0 +1,170 @@
|
|
|
1
|
+
import socket
|
|
2
|
+
import struct
|
|
3
|
+
import logging
|
|
4
|
+
import time
|
|
5
|
+
import threading
|
|
6
|
+
|
|
7
|
+
import cv2
|
|
8
|
+
import numpy as np
|
|
9
|
+
|
|
10
|
+
logger = logging.getLogger(__name__)
|
|
11
|
+
|
|
12
|
+
MAX = 1400
|
|
13
|
+
TO = 2.0
|
|
14
|
+
HDR_FMT = ">IHHQI"
|
|
15
|
+
HDR_SIZE = struct.calcsize(HDR_FMT)
|
|
16
|
+
|
|
17
|
+
SERVER_PORT = 5000
|
|
18
|
+
|
|
19
|
+
class Camera:
|
|
20
|
+
with open("product", 'r') as file:
|
|
21
|
+
SERVER_IP = None
|
|
22
|
+
for line in file:
|
|
23
|
+
line = line.strip()
|
|
24
|
+
if line.startswith('CAMERA_DOMAIN='):
|
|
25
|
+
SERVER_IP = line.split('=')[1].strip()
|
|
26
|
+
if SERVER_IP is None:
|
|
27
|
+
raise "[Error] There is no product file. Please make sure the device has product info"
|
|
28
|
+
|
|
29
|
+
def __init__(self, server_ip=SERVER_IP, server_port=SERVER_PORT, client_port=None, start_timeout=5.0, no_data_timeout=5.0):
|
|
30
|
+
self.server_ip = server_ip
|
|
31
|
+
self.server_port = server_port
|
|
32
|
+
self.client_port = client_port
|
|
33
|
+
|
|
34
|
+
self.start_timeout = start_timeout
|
|
35
|
+
self.no_data_timeout = no_data_timeout
|
|
36
|
+
|
|
37
|
+
self._sock = None
|
|
38
|
+
self._thread = None
|
|
39
|
+
self._running = threading.Event()
|
|
40
|
+
|
|
41
|
+
self._last_frame = None
|
|
42
|
+
self._last_frame_id = None
|
|
43
|
+
self._lock = threading.Lock()
|
|
44
|
+
|
|
45
|
+
self._first_frame_event = threading.Event()
|
|
46
|
+
|
|
47
|
+
def start(self):
|
|
48
|
+
if self._running.is_set():
|
|
49
|
+
return
|
|
50
|
+
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
51
|
+
self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 1 << 20)
|
|
52
|
+
if self.client_port is None:
|
|
53
|
+
self._sock.bind(("", 0))
|
|
54
|
+
self.client_port = self._sock.getsockname()[1]
|
|
55
|
+
else:
|
|
56
|
+
self._sock.bind(("", self.client_port))
|
|
57
|
+
self._sock.settimeout(1.0)
|
|
58
|
+
self._running.set()
|
|
59
|
+
self._first_frame_event.clear()
|
|
60
|
+
try:
|
|
61
|
+
self._sock.sendto(b"HELLO", (self.server_ip, self.server_port))
|
|
62
|
+
except Exception as e:
|
|
63
|
+
logger.error("failed to send HELLO: %s", e)
|
|
64
|
+
self._thread = threading.Thread(target=self._recv_loop, daemon=True)
|
|
65
|
+
self._thread.start()
|
|
66
|
+
logger.info("Camera started on port %d", self.client_port)
|
|
67
|
+
if not self._first_frame_event.wait(timeout=self.start_timeout):
|
|
68
|
+
logger.error("no frame received within %.1f seconds after start", self.start_timeout)
|
|
69
|
+
self.stop()
|
|
70
|
+
raise RuntimeError("no frame received from server")
|
|
71
|
+
|
|
72
|
+
def stop(self):
|
|
73
|
+
if not self._running.is_set():
|
|
74
|
+
return
|
|
75
|
+
self._running.clear()
|
|
76
|
+
if self._sock is not None:
|
|
77
|
+
try:
|
|
78
|
+
self._sock.sendto(b"BYE", (self.server_ip, self.server_port))
|
|
79
|
+
except Exception:
|
|
80
|
+
pass
|
|
81
|
+
try:
|
|
82
|
+
self._sock.close()
|
|
83
|
+
except Exception:
|
|
84
|
+
pass
|
|
85
|
+
self._sock = None
|
|
86
|
+
if self._thread is not None:
|
|
87
|
+
self._thread.join(timeout=2.0)
|
|
88
|
+
self._thread = None
|
|
89
|
+
logger.info("Camera stopped")
|
|
90
|
+
|
|
91
|
+
def _recv_loop(self):
|
|
92
|
+
fbuf = {}
|
|
93
|
+
frame_count = 0
|
|
94
|
+
last_data_time = time.time()
|
|
95
|
+
while self._running.is_set():
|
|
96
|
+
now = time.time()
|
|
97
|
+
if now - last_data_time > self.no_data_timeout:
|
|
98
|
+
logger.error("no camera data for %.1f seconds, stopping client", self.no_data_timeout)
|
|
99
|
+
self._running.clear()
|
|
100
|
+
break
|
|
101
|
+
try:
|
|
102
|
+
try:
|
|
103
|
+
d, _ = self._sock.recvfrom(MAX)
|
|
104
|
+
except socket.timeout:
|
|
105
|
+
now = time.time()
|
|
106
|
+
drop = [k for k, v in fbuf.items() if now - v["t"] > TO]
|
|
107
|
+
for k in drop:
|
|
108
|
+
del fbuf[k]
|
|
109
|
+
continue
|
|
110
|
+
except OSError:
|
|
111
|
+
break
|
|
112
|
+
last_data_time = time.time()
|
|
113
|
+
if len(d) < HDR_SIZE:
|
|
114
|
+
continue
|
|
115
|
+
try:
|
|
116
|
+
fid, idx, tot, ts_us, size = struct.unpack(HDR_FMT, d[:HDR_SIZE])
|
|
117
|
+
except struct.error:
|
|
118
|
+
continue
|
|
119
|
+
pay = d[HDR_SIZE:]
|
|
120
|
+
f = fbuf.get(fid)
|
|
121
|
+
if f is None:
|
|
122
|
+
f = {"t": time.time(), "tot": tot, "p": {}, "ts": ts_us, "sz": size}
|
|
123
|
+
fbuf[fid] = f
|
|
124
|
+
f["p"][idx] = pay
|
|
125
|
+
if len(f["p"]) == f["tot"]:
|
|
126
|
+
try:
|
|
127
|
+
j = b"".join(f["p"][i] for i in range(f["tot"]))
|
|
128
|
+
except KeyError:
|
|
129
|
+
del fbuf[fid]
|
|
130
|
+
continue
|
|
131
|
+
if len(j) != f["sz"]:
|
|
132
|
+
logger.warning("size mismatch frame=%d expected=%d got=%d", fid, f["sz"], len(j))
|
|
133
|
+
a = np.frombuffer(j, np.uint8)
|
|
134
|
+
img = cv2.imdecode(a, cv2.IMREAD_COLOR)
|
|
135
|
+
if img is not None:
|
|
136
|
+
with self._lock:
|
|
137
|
+
self._last_frame = img
|
|
138
|
+
self._last_frame_id = fid
|
|
139
|
+
if not self._first_frame_event.is_set():
|
|
140
|
+
self._first_frame_event.set()
|
|
141
|
+
frame_count += 1
|
|
142
|
+
if frame_count % 100 == 0:
|
|
143
|
+
logger.info("received frame=%d", fid)
|
|
144
|
+
del fbuf[fid]
|
|
145
|
+
|
|
146
|
+
def read(self, copy=True):
|
|
147
|
+
with self._lock:
|
|
148
|
+
if self._last_frame is None:
|
|
149
|
+
return None
|
|
150
|
+
if copy:
|
|
151
|
+
return self._last_frame.copy()
|
|
152
|
+
return self._last_frame
|
|
153
|
+
|
|
154
|
+
|
|
155
|
+
if __name__ == "__main__":
|
|
156
|
+
logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s")
|
|
157
|
+
client = Camera()
|
|
158
|
+
try:
|
|
159
|
+
client.start()
|
|
160
|
+
except RuntimeError as e:
|
|
161
|
+
logger.error("failed to start client: %s", e)
|
|
162
|
+
else:
|
|
163
|
+
try:
|
|
164
|
+
while True:
|
|
165
|
+
frame = client.read()
|
|
166
|
+
time.sleep(0.01)
|
|
167
|
+
except KeyboardInterrupt:
|
|
168
|
+
pass
|
|
169
|
+
finally:
|
|
170
|
+
client.stop()
|
|
@@ -0,0 +1,47 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
import time
|
|
3
|
+
from autocar3g.absclient import AbstractPopClient
|
|
4
|
+
|
|
5
|
+
class Driving(AbstractPopClient):
|
|
6
|
+
def __init__(self, wait=True):
|
|
7
|
+
super().__init__()
|
|
8
|
+
self._client.subscribe(self._TOPIC_HEADER + '/drive/steering')
|
|
9
|
+
self._client.subscribe(self._TOPIC_HEADER + '/drive/throttle')
|
|
10
|
+
self._client.subscribe(self._TOPIC_HEADER+'/int')
|
|
11
|
+
self.__steering_value = None
|
|
12
|
+
self.__throttle_value = None
|
|
13
|
+
self.__wait = wait
|
|
14
|
+
|
|
15
|
+
def _decode(self, message):
|
|
16
|
+
if 'steering' in message.topic:
|
|
17
|
+
self.__steering_value = struct.unpack("<f", message.payload)[0]
|
|
18
|
+
elif 'throttle' in message.topic:
|
|
19
|
+
self.__throttle_value = struct.unpack("<i", message.payload)[0]
|
|
20
|
+
else:
|
|
21
|
+
print(message.payload)
|
|
22
|
+
|
|
23
|
+
@property
|
|
24
|
+
def steering(self):
|
|
25
|
+
while self.__steering_value is None and self.__wait: time.sleep(0.01)
|
|
26
|
+
value = self.__steering_value
|
|
27
|
+
self.__steering_value = None
|
|
28
|
+
return value
|
|
29
|
+
|
|
30
|
+
@steering.setter
|
|
31
|
+
def steering(self, value:float):
|
|
32
|
+
if value < -1.0 or value > 1.0:
|
|
33
|
+
raise ValueError("Wrong steering value.")
|
|
34
|
+
self._client.publish(self._TOPIC_HEADER+"/drive/set", struct.pack('<fi', value, self.throttle), 0)
|
|
35
|
+
|
|
36
|
+
@property
|
|
37
|
+
def throttle(self):
|
|
38
|
+
while self.__throttle_value is None and self.__wait: time.sleep(0.01)
|
|
39
|
+
value = self.__throttle_value
|
|
40
|
+
self.__throttle_value = None
|
|
41
|
+
return value
|
|
42
|
+
|
|
43
|
+
@throttle.setter
|
|
44
|
+
def throttle(self, value:int):
|
|
45
|
+
if value < -99 or value > 99:
|
|
46
|
+
raise ValueError("Wrong throttle value.")
|
|
47
|
+
self._client.publish(self._TOPIC_HEADER+"/drive/set", struct.pack('<fi', self.steering, value), 0)
|
|
@@ -0,0 +1,19 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
import time
|
|
3
|
+
from autocar3g.absclient import AbstractPopClient
|
|
4
|
+
|
|
5
|
+
class Encoder(AbstractPopClient):
|
|
6
|
+
def __init__(self, wait=True):
|
|
7
|
+
super().__init__()
|
|
8
|
+
self._client.subscribe(self._TOPIC_HEADER + '/encoder')
|
|
9
|
+
self.__value = None
|
|
10
|
+
self.__wait = wait
|
|
11
|
+
|
|
12
|
+
def _decode(self, message):
|
|
13
|
+
self.__value = tuple(struct.unpack("<ii", message.payload))
|
|
14
|
+
|
|
15
|
+
def read(self):
|
|
16
|
+
while self.__value is None and self.__wait: time.sleep(0.01)
|
|
17
|
+
value = self.__value
|
|
18
|
+
self.__value = None
|
|
19
|
+
return value
|
|
@@ -0,0 +1,55 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
import math
|
|
3
|
+
import time
|
|
4
|
+
from .absclient import AbstractPopClient
|
|
5
|
+
|
|
6
|
+
class Imu(AbstractPopClient):
|
|
7
|
+
def __init__(self, wait=True):
|
|
8
|
+
super().__init__()
|
|
9
|
+
self._client.subscribe(self._TOPIC_HEADER + '/imu')
|
|
10
|
+
self.__accel_value = None
|
|
11
|
+
self.__gyro_value = None
|
|
12
|
+
self.__quat_value = None
|
|
13
|
+
self.__wait = wait
|
|
14
|
+
|
|
15
|
+
def _decode(self, message):
|
|
16
|
+
unpacked = struct.unpack('<10d', message.payload)
|
|
17
|
+
self.__accel_value = tuple(unpacked[0:3])
|
|
18
|
+
self.__gyro_value = tuple(unpacked[3:6])
|
|
19
|
+
self.__quat_value = tuple(unpacked[6:10])
|
|
20
|
+
|
|
21
|
+
def accel(self):
|
|
22
|
+
while self.__accel_value is None and self.__wait : time.sleep(0.01)
|
|
23
|
+
value = self.__accel_value
|
|
24
|
+
# self.__accel_value = None
|
|
25
|
+
return value
|
|
26
|
+
|
|
27
|
+
def gyro(self):
|
|
28
|
+
while self.__gyro_value is None and self.__wait : time.sleep(0.01)
|
|
29
|
+
value = self.__gyro_value
|
|
30
|
+
# self.__gyro_value = None
|
|
31
|
+
return value
|
|
32
|
+
|
|
33
|
+
def euler(self):
|
|
34
|
+
w,x,y,z = self.quat()
|
|
35
|
+
|
|
36
|
+
t0 = +2.0 * (w*x + y*z)
|
|
37
|
+
t1 = +1.0 - 2.0 * (x*x + y*y)
|
|
38
|
+
roll = math.atan2(t0, t1)
|
|
39
|
+
|
|
40
|
+
t2 = +2.0 * (w*y - z*x)
|
|
41
|
+
t2 = +1.0 if t2 > +1.0 else t2
|
|
42
|
+
t2 = -1.0 if t2 < -1.0 else t2
|
|
43
|
+
pitch = math.asin(t2)
|
|
44
|
+
|
|
45
|
+
t3 = +2.0 * (w*z + x*y)
|
|
46
|
+
t4 = +1.0 - 2.0 * (y*y + z*z)
|
|
47
|
+
yaw = math.atan2(t3, t4)
|
|
48
|
+
|
|
49
|
+
return (roll, pitch, yaw)
|
|
50
|
+
|
|
51
|
+
def quat(self):
|
|
52
|
+
while self.__quat_value is None and self.__wait : time.sleep(0.01)
|
|
53
|
+
value = self.__quat_value
|
|
54
|
+
self.__quat_value = None
|
|
55
|
+
return value
|
|
@@ -0,0 +1,17 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
import time
|
|
3
|
+
from autocar3g.absclient import AbstractPopClient
|
|
4
|
+
|
|
5
|
+
class Led(AbstractPopClient):
|
|
6
|
+
def __init__(self, wait=True):
|
|
7
|
+
super().__init__()
|
|
8
|
+
self._client.subscribe(self._TOPIC_HEADER + '/drive/steering')
|
|
9
|
+
|
|
10
|
+
def _decode(self, message):
|
|
11
|
+
pass
|
|
12
|
+
|
|
13
|
+
def onoff(self, front, rear):
|
|
14
|
+
value = 0
|
|
15
|
+
value += 1 if front else 0
|
|
16
|
+
value += 2 if rear else 0
|
|
17
|
+
self._client.publish(self._TOPIC_HEADER+"/lamp/set", struct.pack('<i', value), 0)
|
|
@@ -0,0 +1,36 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
import math
|
|
3
|
+
import time
|
|
4
|
+
from autocar3g.absclient import AbstractPopClient
|
|
5
|
+
|
|
6
|
+
class LiDAR(AbstractPopClient):
|
|
7
|
+
def __init__(self, wait=True):
|
|
8
|
+
super().__init__()
|
|
9
|
+
self._client.subscribe(self._TOPIC_HEADER + '/scan')
|
|
10
|
+
self.__value = None
|
|
11
|
+
self.__wait = wait
|
|
12
|
+
|
|
13
|
+
def _decode(self, message):
|
|
14
|
+
header_ffi = '<ffI'
|
|
15
|
+
header_size = struct.calcsize(header_ffi)
|
|
16
|
+
|
|
17
|
+
angle_min, angle_increment, n = struct.unpack(
|
|
18
|
+
header_ffi, message.payload[:header_size]
|
|
19
|
+
)
|
|
20
|
+
|
|
21
|
+
ranges_fmt = '<{}f'.format(n)
|
|
22
|
+
ranges = struct.unpack(
|
|
23
|
+
ranges_fmt, message.payload[header_size:]
|
|
24
|
+
)
|
|
25
|
+
|
|
26
|
+
self.__value = {
|
|
27
|
+
'angle_min' : angle_min,
|
|
28
|
+
'angle_increment' : angle_increment,
|
|
29
|
+
'ranges' : ranges
|
|
30
|
+
}
|
|
31
|
+
|
|
32
|
+
def read(self):
|
|
33
|
+
while self.__value is None and self.__wait : time.sleep(0.01)
|
|
34
|
+
value = self.__value
|
|
35
|
+
self.__value = None
|
|
36
|
+
return value
|
|
@@ -0,0 +1,19 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
import time
|
|
3
|
+
from autocar3g.absclient import AbstractPopClient
|
|
4
|
+
|
|
5
|
+
class Ultrasonic(AbstractPopClient):
|
|
6
|
+
def __init__(self, wait=True):
|
|
7
|
+
super().__init__()
|
|
8
|
+
self._client.subscribe(self._TOPIC_HEADER + '/ultra')
|
|
9
|
+
self.__value = None
|
|
10
|
+
self.__wait = wait
|
|
11
|
+
|
|
12
|
+
def _decode(self, message):
|
|
13
|
+
self.__value = tuple(struct.unpack("<ii", message.payload))
|
|
14
|
+
|
|
15
|
+
def read(self):
|
|
16
|
+
while self.__value is None and self.__wait: time.sleep(0.01)
|
|
17
|
+
value = self.__value
|
|
18
|
+
self.__value = None
|
|
19
|
+
return value
|
|
@@ -0,0 +1,28 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: autocar3g
|
|
3
|
+
Version: 0.1.4
|
|
4
|
+
Summary: Pop plus library for Autocar3G
|
|
5
|
+
Home-page: https://github.com/hanback-lab/Autocar3G
|
|
6
|
+
Author: Hanback Electronics CO. Ltd. Research Insitutue
|
|
7
|
+
Author-email: lab@hanback.co.kr
|
|
8
|
+
Classifier: Programming Language :: Python :: 3
|
|
9
|
+
Classifier: License :: OSI Approved :: MIT License
|
|
10
|
+
Classifier: Operating System :: OS Independent
|
|
11
|
+
Requires-Python: >=3.8
|
|
12
|
+
Description-Content-Type: text/markdown
|
|
13
|
+
Requires-Dist: paho-mqtt
|
|
14
|
+
Requires-Dist: tensorflow>=2.5.0; platform_system != "Linux" or platform_machine != "aarch64"
|
|
15
|
+
Requires-Dist: torch>=1.13.0; platform_system != "Linux" or platform_machine != "aarch64"
|
|
16
|
+
Requires-Dist: ultralytics; platform_system != "Linux" or platform_machine != "aarch64"
|
|
17
|
+
Dynamic: author
|
|
18
|
+
Dynamic: author-email
|
|
19
|
+
Dynamic: classifier
|
|
20
|
+
Dynamic: description
|
|
21
|
+
Dynamic: description-content-type
|
|
22
|
+
Dynamic: home-page
|
|
23
|
+
Dynamic: requires-dist
|
|
24
|
+
Dynamic: requires-python
|
|
25
|
+
Dynamic: summary
|
|
26
|
+
|
|
27
|
+
# Pop plus :: Autocar3 G
|
|
28
|
+
|
|
@@ -0,0 +1,17 @@
|
|
|
1
|
+
README.md
|
|
2
|
+
setup.py
|
|
3
|
+
autocar3g/AI.py
|
|
4
|
+
autocar3g/__init__.py
|
|
5
|
+
autocar3g/absclient.py
|
|
6
|
+
autocar3g/camera.py
|
|
7
|
+
autocar3g/driving.py
|
|
8
|
+
autocar3g/encoder.py
|
|
9
|
+
autocar3g/imu.py
|
|
10
|
+
autocar3g/led.py
|
|
11
|
+
autocar3g/lidar.py
|
|
12
|
+
autocar3g/ultrasonic.py
|
|
13
|
+
autocar3g.egg-info/PKG-INFO
|
|
14
|
+
autocar3g.egg-info/SOURCES.txt
|
|
15
|
+
autocar3g.egg-info/dependency_links.txt
|
|
16
|
+
autocar3g.egg-info/requires.txt
|
|
17
|
+
autocar3g.egg-info/top_level.txt
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
autocar3g
|
autocar3g-0.1.4/setup.py
ADDED
|
@@ -0,0 +1,28 @@
|
|
|
1
|
+
from setuptools import setup, find_packages
|
|
2
|
+
|
|
3
|
+
with open("README.md", "r") as fh:
|
|
4
|
+
long_description = fh.read()
|
|
5
|
+
|
|
6
|
+
setup(
|
|
7
|
+
name="autocar3g", # Replace with your own username
|
|
8
|
+
version="0.1.4",
|
|
9
|
+
author="Hanback Electronics CO. Ltd. Research Insitutue",
|
|
10
|
+
author_email="lab@hanback.co.kr",
|
|
11
|
+
description="Pop plus library for Autocar3G",
|
|
12
|
+
long_description=long_description,
|
|
13
|
+
long_description_content_type="text/markdown",
|
|
14
|
+
url="https://github.com/hanback-lab/Autocar3G",
|
|
15
|
+
packages=find_packages(),
|
|
16
|
+
classifiers=[
|
|
17
|
+
"Programming Language :: Python :: 3",
|
|
18
|
+
"License :: OSI Approved :: MIT License",
|
|
19
|
+
"Operating System :: OS Independent",
|
|
20
|
+
],
|
|
21
|
+
install_requires=[
|
|
22
|
+
'paho-mqtt',
|
|
23
|
+
'tensorflow>=2.5.0; platform_system != "Linux" or platform_machine != "aarch64"',
|
|
24
|
+
'torch>=1.13.0; platform_system != "Linux" or platform_machine != "aarch64"',
|
|
25
|
+
'ultralytics; platform_system != "Linux" or platform_machine != "aarch64"'
|
|
26
|
+
],
|
|
27
|
+
python_requires='>=3.8',
|
|
28
|
+
)
|