examples/a121/algo/parking/parking.py#
1# Copyright (c) Acconeer AB, 2023-2026
2# All rights reserved
3
4from __future__ import annotations
5
6import numpy as np
7
8# Added here to force pyqtgraph to choose PySide
9import PySide6 # noqa: F401
10from PySide6 import QtCore
11
12import pyqtgraph as pg
13
14import acconeer.exptool as et
15from acconeer.exptool import a121
16from acconeer.exptool.a121.algo._utils import get_distances_m
17from acconeer.exptool.a121.algo.parking import (
18 ObstructionProcessor,
19 RefApp,
20 RefAppConfig,
21 RefAppResult,
22)
23from acconeer.exptool.a121.algo.parking._processors import MAX_AMPLITUDE
24from acconeer.exptool.a121.algo.parking._ref_app import get_sensor_configs
25
26
27SENSOR_ID = 1
28
29
30def main():
31 args = a121.ExampleArgumentParser().parse_args()
32 et.utils.config_logging(args)
33
34 client = a121.Client.open(**a121.get_client_args(args))
35 ref_app_config = RefAppConfig(
36 range_start_m=0.1,
37 range_end_m=0.4,
38 hwaas=24,
39 profile=a121.Profile.PROFILE_1,
40 update_rate=5,
41 queue_length_n=3,
42 amplitude_threshold=8.0,
43 weighted_distance_threshold_m=0.1,
44 obstruction_detection=True,
45 obstruction_start_m=0.03,
46 obstruction_end_m=0.05,
47 obstruction_distance_threshold=0.06,
48 )
49
50 ref_app = RefApp(client=client, sensor_id=SENSOR_ID, ref_app_config=ref_app_config)
51 ref_app.calibrate_ref_app()
52 ref_app.start()
53
54 pg_updater = PGUpdater(
55 ref_app_config,
56 ref_app.metadata,
57 SENSOR_ID,
58 ref_app.session_config,
59 )
60
61 pg_process = et.PGProcess(pg_updater)
62 pg_process.start()
63
64 interrupt_handler = et.utils.ExampleInterruptHandler()
65
66 while not interrupt_handler.got_signal:
67 ref_app_result = ref_app.get_next()
68 if ref_app_result.car_detected:
69 print("Car detected")
70 else:
71 print("No car detected")
72 if ref_app_config.obstruction_detection:
73 if ref_app_result.obstruction_detected:
74 print("Obstruction detected")
75 else:
76 print("No obstruction detected")
77 try:
78 pg_process.put_data(ref_app_result)
79 except et.PGProccessDiedException:
80 break
81
82 ref_app.stop()
83
84 print("Disconnecting...")
85 client.close()
86
87
88class PGUpdater:
89 def __init__(
90 self,
91 ref_app_config: RefAppConfig,
92 metadata: a121.Metadata,
93 sensor_id: int,
94 session_config: a121.SessionConfig,
95 ):
96 self.metadata = metadata
97 self.ref_app_config = ref_app_config
98 sensor_configs = get_sensor_configs(ref_app_config)
99 display_config = sensor_configs["base_config"]
100 self.sensor_id = sensor_id
101 self.session_config = session_config
102 self.sensor_config = display_config
103 self.distances = get_distances_m(display_config, metadata)
104
105 if self.ref_app_config.obstruction_detection:
106 obstruction_config = sensor_configs["obstruction_config"]
107 self.obs_distances = get_distances_m(obstruction_config, metadata)
108 self.obs_x_thres, self.obs_y_thres = ObstructionProcessor.get_thresholds(
109 ref_app_config.obstruction_distance_threshold, self.obs_distances
110 )
111
112 self.setup_is_done = False
113
114 def setup(self, win):
115 win.setWindowTitle("Acconeer parking detection example")
116 # Define pens and font.
117 blue_color = et.utils.color_cycler(0)
118 orange_color = et.utils.color_cycler(1)
119 brush = et.utils.pg_brush_cycler(0)
120
121 self.blue = dict(
122 pen=pg.mkPen(blue_color, width=2),
123 symbol="o",
124 symbolSize=1,
125 symbolBrush=brush,
126 symbolPen="k",
127 )
128
129 self.orange = dict(
130 pen=pg.mkPen(orange_color, width=2),
131 symbol="o",
132 symbolSize=1,
133 symbolBrush=brush,
134 symbolPen="k",
135 )
136 self.blue_transparent_pen = pg.mkPen(f"{blue_color}50", width=2)
137 self.orange_transparent_pen = pg.mkPen(f"{orange_color}50", width=2)
138
139 brush_dot = et.utils.pg_brush_cycler(1)
140
141 # Signature plot.
142 self.sig_plot = win.addPlot(row=0, col=0, colspan=2)
143 self.sig_plot.setTitle("Sampled Signatures")
144 self.sig_plot.setMenuEnabled(False)
145 self.sig_plot.showGrid(x=True, y=True)
146 self.sig_plot.addLegend()
147 self.sig_plot.setLabel("left", "Normalized energy")
148 self.sig_plot.setLabel("bottom", "Distance (m)")
149 self.sig_plot.addItem(pg.PlotDataItem())
150 self.sig_plot_x_range = (
151 min(self.distances),
152 max(self.distances) + self.ref_app_config.weighted_distance_threshold_m,
153 )
154
155 self.sig_plot.setXRange(self.sig_plot_x_range[0], self.sig_plot_x_range[1])
156 self.sig_plot.setYRange(0, 100)
157 symbol_kw_main = dict(
158 symbol="o", symbolSize=7, symbolBrush=brush, symbolPen=None, pen=None
159 )
160 self.sig_plot_curve = self.sig_plot.plot(**symbol_kw_main)
161 energy_threshold_line = pg.InfiniteLine(
162 angle=0, pen=pg.mkPen("k", width=1.5, style=QtCore.Qt.PenStyle.DashLine)
163 )
164 energy_threshold_line.setVisible(True)
165 energy_threshold_line.setPos(self.ref_app_config.amplitude_threshold)
166 self.sig_plot.addItem(energy_threshold_line)
167
168 self.sig_plot_cluster_start = pg.InfiniteLine(angle=90, pen=pg.mkPen("k", width=1.5))
169 self.sig_plot_cluster_start.setVisible(True)
170 self.sig_plot.addItem(self.sig_plot_cluster_start)
171
172 self.sig_plot_cluster_end = pg.InfiniteLine(angle=90, pen=pg.mkPen("k", width=1.5))
173 self.sig_plot_cluster_end.setVisible(True)
174 self.sig_plot.addItem(self.sig_plot_cluster_end)
175
176 self.sig_plot_smooth_max = et.utils.SmoothMax(self.session_config.update_rate)
177
178 parking_car_html = (
179 '<div style="text-align: center">'
180 '<span style="color: #FFFFFF;font-size:15pt;">'
181 "{}</span></div>".format("Parked car detected!")
182 )
183 self.parking_car_text_item = pg.TextItem(
184 html=parking_car_html,
185 fill=orange_color,
186 anchor=(0.5, 0),
187 )
188 parking_no_car_html = (
189 '<div style="text-align: center">'
190 '<span style="color: #FFFFFF;font-size:15pt;">'
191 "{}</span></div>".format("No car detected.")
192 )
193 self.parking_no_car_text_item = pg.TextItem(
194 html=parking_no_car_html,
195 fill=blue_color,
196 anchor=(0.5, 0),
197 )
198
199 self.sig_plot.addItem(self.parking_car_text_item)
200 self.sig_plot.addItem(self.parking_no_car_text_item)
201 self.parking_car_text_item.hide()
202 self.parking_no_car_text_item.hide()
203
204 self.cluster_width = self.ref_app_config.weighted_distance_threshold_m
205
206 # Obstruction plot.
207 if self.ref_app_config.obstruction_detection:
208 self.obstruction_plot = win.addPlot(row=1, col=1)
209 self.obstruction_plot.setTitle("Obstruction Detection Signatures")
210 self.obstruction_plot.setMenuEnabled(False)
211 self.obstruction_plot.showGrid(x=True, y=True)
212 self.obstruction_plot.addLegend()
213 self.obstruction_plot.setLabel("left", "Average energy")
214 self.obstruction_plot.setLabel("bottom", "Distance (m)")
215 self.obstruction_plot.addItem(pg.PlotDataItem())
216 self.obstruction_plot.setXRange(min(self.obs_distances), max(self.obs_distances))
217 self.obstruction_plot.setYRange(0, MAX_AMPLITUDE) # Set to standard
218 self.obstruction_plot_curve = self.obstruction_plot.plot(**self.orange)
219
220 symbol_obstruction_dot = dict(
221 symbol="o",
222 symbolSize=7,
223 symbolBrush=brush_dot,
224 symbolPen=None,
225 pen=None,
226 )
227 self.obstruction_plot_point = self.obstruction_plot.plot(**symbol_obstruction_dot)
228
229 symbol_kw_main = dict(
230 symbol="o", symbolSize=7, symbolBrush=brush, symbolPen=None, pen=None
231 )
232 self.obstruction_plot_center = self.obstruction_plot.plot(**symbol_kw_main)
233
234 self.obstruction_center_rect = pg.QtWidgets.QGraphicsRectItem(0, 0, 0.01, 0.01)
235 self.obstruction_center_rect.setPen(self.orange_transparent_pen)
236 self.obstruction_plot.addItem(self.obstruction_center_rect)
237
238 obstruction_html = (
239 '<div style="text-align: center">'
240 '<span style="color: #FFFFFF;font-size:15pt;">'
241 "{}</span></div>".format("Obstruction detected!")
242 )
243 self.obstruction_text_item = pg.TextItem(
244 html=obstruction_html,
245 fill=orange_color,
246 anchor=(0.5, 0),
247 )
248 no_obstruction_html = (
249 '<div style="text-align: center">'
250 '<span style="color: #FFFFFF;font-size:15pt;">'
251 "{}</span></div>".format("No obstruction detected.")
252 )
253 self.no_obstruction_text_item = pg.TextItem(
254 html=no_obstruction_html,
255 fill=blue_color,
256 anchor=(0.5, 0),
257 )
258
259 obs_text_x_pos = (
260 min(self.obs_distances) + (max(self.obs_distances) - min(self.obs_distances)) * 0.5
261 )
262 obs_text_y_pos = MAX_AMPLITUDE * 0.9
263
264 self.obstruction_text_item.setPos(obs_text_x_pos, obs_text_y_pos)
265 self.no_obstruction_text_item.setPos(obs_text_x_pos, obs_text_y_pos)
266
267 self.obstruction_plot.addItem(self.obstruction_text_item)
268 self.obstruction_plot.addItem(self.no_obstruction_text_item)
269 self.obstruction_text_item.hide()
270 self.no_obstruction_text_item.hide()
271
272 # Parking info plot.
273 self.parking_plot = win.addPlot(row=1, col=0)
274 self.parking_plot.setTitle("Noise adjusted amplitude")
275 self.parking_plot.setMenuEnabled(False)
276 self.parking_plot.showGrid(x=True, y=True)
277 self.parking_plot.setLabel("left", "Normalized energy")
278 self.parking_plot.setLabel("bottom", "Distance (m)")
279 self.parking_plot.addItem(pg.PlotDataItem())
280 self.parking_plot.setXRange(min(self.distances), max(self.distances))
281 self.parking_plot.setYRange(0, 100) # Set to standard
282 self.parking_plot_curve = self.parking_plot.plot(**self.blue)
283 self.parking_smooth_max = et.utils.SmoothMax(self.session_config.update_rate)
284
285 def update_obstruction_text(self) -> None:
286 self.obstruction_text_timeout -= 1
287 if self.obstruction_text_timeout < 0:
288 self.obstruction_text_timeout = 0
289 self.obstruction_text_item.hide()
290
291 def show_obstruction_text(self) -> None:
292 self.obstruction_text_timeout = 5
293 self.obstruction_text_item.show()
294
295 def update(self, ref_app_result: RefAppResult) -> None:
296 signatures = ref_app_result.extra_result.signature_history
297 parking_data = ref_app_result.extra_result.parking_data
298
299 signature_x = [elm[0] for elm in signatures]
300 signature_y = [elm[1] for elm in signatures]
301
302 cluster_start = ref_app_result.extra_result.closest_object_dist
303 cluster_end = cluster_start + self.cluster_width
304 self.sig_plot_curve.setData(x=signature_x, y=signature_y)
305 self.sig_plot_cluster_start.setPos(cluster_start)
306 self.sig_plot_cluster_end.setPos(cluster_end)
307 self.sig_plot_cluster_start.setVisible(False)
308 self.sig_plot_cluster_end.setVisible(False)
309
310 sig_max = self.sig_plot_smooth_max.update(max(signature_y))
311 self.sig_plot.setYRange(0, sig_max)
312
313 sig_text_x_pos = (
314 self.sig_plot_x_range[0] + (self.sig_plot_x_range[1] - self.sig_plot_x_range[0]) * 0.5
315 )
316 sig_text_y_pos = sig_max * 0.9
317
318 self.parking_car_text_item.setPos(sig_text_x_pos, sig_text_y_pos)
319 self.parking_no_car_text_item.setPos(sig_text_x_pos, sig_text_y_pos)
320
321 if ref_app_result.car_detected:
322 self.parking_no_car_text_item.hide()
323 self.parking_car_text_item.show()
324 self.sig_plot_cluster_start.setVisible(True)
325 self.sig_plot_cluster_end.setVisible(True)
326 else:
327 self.parking_car_text_item.hide()
328 self.parking_no_car_text_item.show()
329
330 if self.ref_app_config.obstruction_detection:
331 obstruction_data = ref_app_result.extra_result.obstruction_data
332 self.obstruction_plot_curve.setData(self.obs_distances, obstruction_data)
333 point_x, point_y = ref_app_result.extra_result.obstruction_signature
334 center_x, center_y = ref_app_result.extra_result.obstruction_center
335 rect_x = center_x - self.obs_x_thres
336 rect_y = center_y - self.obs_y_thres
337 rect_w = 2 * self.obs_x_thres
338 rect_h = 2 * self.obs_y_thres
339
340 self.obstruction_center_rect.setRect(rect_x, rect_y, rect_w, rect_h)
341
342 self.obstruction_plot_point.setData(x=[point_x], y=[point_y])
343 self.obstruction_plot_center.setData(x=[center_x], y=[center_y])
344
345 if ref_app_result.obstruction_detected:
346 self.no_obstruction_text_item.hide()
347 self.obstruction_text_item.show()
348 else:
349 self.obstruction_text_item.hide()
350 self.no_obstruction_text_item.show()
351
352 park_max = np.amax(parking_data)
353 park_max = self.parking_smooth_max.update(park_max)
354 self.parking_plot.setYRange(0, park_max)
355 self.parking_plot_curve.setData(self.distances, parking_data)
356
357
358if __name__ == "__main__":
359 main()
View this example on GitHub: acconeer/acconeer-python-exploration