examples/a121/algo/waste_level/processor.py#
1# Copyright (c) Acconeer AB, 2024-2026
2# All rights reserved
3
4from __future__ import annotations
5
6import numpy as np
7
8from PySide6 import QtCore
9
10import pyqtgraph as pg
11
12import acconeer.exptool as et
13from acconeer.exptool import a121
14from acconeer.exptool.a121.algo._utils import APPROX_BASE_STEP_LENGTH_M, get_distances_m
15from acconeer.exptool.a121.algo.waste_level import (
16 Processor,
17 ProcessorConfig,
18 ProcessorResult,
19 get_sensor_config,
20)
21
22
23def main():
24 args = a121.ExampleArgumentParser().parse_args()
25 et.utils.config_logging(args)
26
27 client = a121.Client.open(**a121.get_client_args(args))
28
29 processor_config = ProcessorConfig()
30
31 sensor_config = get_sensor_config()
32
33 metadata = client.setup_session(sensor_config)
34 client.start_session()
35
36 processor = Processor(
37 sensor_config=sensor_config,
38 metadata=metadata,
39 processor_config=processor_config,
40 )
41
42 pg_updater = PGUpdater(
43 metadata=metadata, sensor_config=sensor_config, processor_config=processor_config
44 )
45 pg_process = et.PGProcess(pg_updater)
46 pg_process.start()
47
48 interrupt_handler = et.utils.ExampleInterruptHandler()
49
50 while not interrupt_handler.got_signal:
51 result = client.get_next()
52 processor_result = processor.process(result)
53 try:
54 pg_process.put_data(processor_result)
55 except et.PGProccessDiedException:
56 break
57
58 print("Disconnecting...")
59 pg_process.close()
60 client.stop_session()
61 client.close()
62
63
64class PGUpdater:
65 def __init__(self, metadata, sensor_config, processor_config):
66 self.metadata = metadata
67 self.sensor_config = sensor_config
68 self.processor_config = processor_config
69
70 def setup(self, win) -> None:
71 # Phase standard deviation plot
72 self.phase_std_plot = self._create_phase_std_plot(win)
73 pen = et.utils.pg_pen_cycler(0)
74 brush = et.utils.pg_brush_cycler(0)
75 self.phase_std_curve = self.phase_std_plot.plot(pen=pen)
76 self.phase_std_dots_above = pg.ScatterPlotItem(symbol="o", size=10, brush=brush, pen="k")
77 self.phase_std_plot.addItem(self.phase_std_dots_above)
78 brush = et.utils.pg_brush_cycler(1)
79 self.phase_std_dots_below = pg.ScatterPlotItem(symbol="o", size=10, brush=brush, pen="k")
80 self.phase_std_plot.addItem(self.phase_std_dots_below)
81 brush = et.utils.pg_brush_cycler(2)
82 self.phase_std_dots_detection = pg.ScatterPlotItem(
83 symbol="o", size=10, brush=brush, pen="k"
84 )
85 self.phase_std_plot.addItem(self.phase_std_dots_detection)
86
87 dashed_pen = pg.mkPen("k", width=2.5, style=QtCore.Qt.PenStyle.DashLine)
88 threshold_line = pg.InfiniteLine(
89 pos=self.processor_config.threshold, angle=0, pen=dashed_pen
90 )
91 self.phase_std_plot.addItem(threshold_line)
92
93 vertical_line_start = pg.InfiniteLine(
94 pos=self.processor_config.bin_start_m,
95 pen=et.utils.pg_pen_cycler(7),
96 label="Bin top",
97 labelOpts={
98 "position": 0.6,
99 "color": (0, 100, 0),
100 "fill": (200, 200, 200, 50),
101 "movable": True,
102 },
103 )
104 self.phase_std_plot.addItem(vertical_line_start)
105
106 vertical_line_end = pg.InfiniteLine(
107 pos=self.processor_config.bin_end_m,
108 pen=et.utils.pg_pen_cycler(7),
109 label="Bin bottom",
110 labelOpts={
111 "position": 0.6,
112 "color": (0, 100, 0),
113 "fill": (200, 200, 200, 50),
114 "movable": True,
115 },
116 )
117 self.phase_std_plot.addItem(vertical_line_end)
118
119 self.level_line = pg.InfiniteLine(
120 pen=et.utils.pg_pen_cycler(2),
121 label="Fill level",
122 labelOpts={
123 "position": 0.8,
124 "color": (0, 100, 0),
125 "fill": (200, 200, 200, 50),
126 "movable": True,
127 },
128 )
129 self.phase_std_plot.addItem(self.level_line)
130 self.level_line.hide()
131
132 self.distances_m = get_distances_m(self.sensor_config, self.metadata)
133 self.threshold = self.processor_config.threshold
134 self.sequence_ones = np.ones(self.processor_config.distance_sequence_n)
135
136 # Level history plot
137 self.level_history_plot = self._create_history_plot(win)
138
139 if self.sensor_config.frame_rate is not None:
140 history_length_s = 5
141 history_length_n = int(round(history_length_s * self.sensor_config.frame_rate))
142 self.hist_xs = np.linspace(-history_length_s, 0, history_length_n)
143 else:
144 history_length_n = 100
145 self.hist_xs = np.linspace(-history_length_n, 0, history_length_n)
146 self.level_history_plot.setLabel("bottom", "Frame")
147
148 self.level_history = np.full(history_length_n, np.nan)
149 self.level_history_plot.setYRange(
150 0,
151 self.processor_config.bin_end_m
152 - np.minimum(
153 self.processor_config.bin_start_m,
154 self.sensor_config.subsweeps[0].start_point * APPROX_BASE_STEP_LENGTH_M,
155 ),
156 )
157
158 pen = et.utils.pg_pen_cycler(0)
159 brush = et.utils.pg_brush_cycler(0)
160 symbol_kw = dict(symbol="o", symbolSize=5, symbolBrush=brush, symbolPen="k")
161 feat_kw = dict(pen=pen, **symbol_kw)
162 self.level_history_curve = self.level_history_plot.plot(**feat_kw, connect="finite")
163
164 top_bin_horizontal_line = pg.InfiniteLine(
165 pos=self.processor_config.bin_end_m - self.processor_config.bin_start_m,
166 pen=et.utils.pg_pen_cycler(7),
167 angle=0,
168 label="Bin top",
169 labelOpts={
170 "position": 0.5,
171 "color": (0, 100, 0),
172 "fill": (200, 200, 200, 50),
173 "movable": True,
174 },
175 )
176 self.level_history_plot.addItem(top_bin_horizontal_line)
177
178 bottom_bin_horizontal_line = pg.InfiniteLine(
179 pos=0,
180 pen=et.utils.pg_pen_cycler(7),
181 angle=0,
182 label="Bin bottom",
183 labelOpts={
184 "position": 0.5,
185 "color": (0, 100, 0),
186 "fill": (200, 200, 200, 50),
187 "movable": True,
188 },
189 )
190 self.level_history_plot.addItem(bottom_bin_horizontal_line)
191
192 # Level plot
193 self.num_rects = 16
194 self.rect_plot = pg.PlotItem()
195 self.rect_plot.setAspectLocked()
196 self.rect_plot.hideAxis("left")
197 self.rect_plot.hideAxis("bottom")
198 self.rects = []
199
200 pen = pg.mkPen(None)
201 rect_width = self.num_rects / 2.0
202 for r in np.arange(self.num_rects) + 1:
203 rect = pg.QtWidgets.QGraphicsRectItem(0, r, rect_width, 1)
204 rect.setPen(pen)
205 self.rect_plot.addItem(rect)
206 self.rects.append(rect)
207
208 self.level_html_format = (
209 '<div style="text-align: center">'
210 '<span style="color: #FFFFFF;font-size:12pt;">'
211 "{}</span></div>"
212 )
213
214 self.level_text_item = pg.TextItem(
215 fill=pg.mkColor(0x1F, 0x77, 0xB4, 180),
216 anchor=(0.5, 0),
217 )
218
219 no_detection_html = (
220 '<div style="text-align: center">'
221 '<span style="color: #FFFFFF;font-size:12pt;">'
222 "{}</span></div>".format("No detection")
223 )
224
225 self.no_detection_text_item = pg.TextItem(
226 html=no_detection_html,
227 fill=pg.mkColor(0xFF, 0x7F, 0x0E, 200),
228 anchor=(0.5, 0),
229 )
230
231 self.rect_plot.addItem(self.level_text_item)
232 self.rect_plot.addItem(self.no_detection_text_item)
233 self.level_text_item.setPos(self.num_rects / 4.0, self.num_rects + 4.0)
234 self.level_text_item.hide()
235 self.no_detection_text_item.setPos(self.num_rects / 4.0, self.num_rects + 4.0)
236 self.no_detection_text_item.show()
237
238 win.addItem(self.rect_plot, row=0, col=0)
239 self.win = win
240
241 def update(self, processor_result: ProcessorResult) -> None:
242 # Phase standard deviation plot
243 self.phase_std_curve.setData(self.distances_m, processor_result.extra_result.phase_std)
244 if processor_result.extra_result.distance_m is not None:
245 self.level_line.setPos(processor_result.extra_result.distance_m)
246 self.level_line.show()
247 else:
248 self.level_line.hide()
249
250 detection_array = processor_result.extra_result.phase_std < self.threshold
251 if np.all(detection_array):
252 self.phase_std_dots_below.setData(
253 self.distances_m, processor_result.extra_result.phase_std
254 )
255 self.phase_std_dots_above.hide()
256 self.phase_std_dots_detection.hide()
257 elif np.any(detection_array):
258 above_idxs = np.argwhere(~detection_array)
259 above_idxs = above_idxs.reshape(above_idxs.shape[0])
260 below_idxs = np.argwhere(detection_array)
261 below_idxs = below_idxs.reshape(below_idxs.shape[0])
262 consecutive_true_indices = np.where(
263 np.convolve(detection_array, self.sequence_ones, mode="valid")
264 == self.sequence_ones.shape[0]
265 )[0]
266 detection_idxs = []
267 for i in consecutive_true_indices:
268 for j in np.arange(i, i + self.sequence_ones.shape[0]):
269 if j not in detection_idxs and j < detection_array.shape[0]:
270 detection_idxs.append(j)
271
272 remove = np.isin(below_idxs, detection_idxs)
273 below_idxs = below_idxs[~remove]
274
275 self.phase_std_dots_above.setData(
276 self.distances_m[above_idxs], processor_result.extra_result.phase_std[above_idxs]
277 )
278 self.phase_std_dots_below.setData(
279 self.distances_m[below_idxs], processor_result.extra_result.phase_std[below_idxs]
280 )
281 if len(detection_idxs) > 0:
282 self.phase_std_dots_detection.setData(
283 self.distances_m[detection_idxs],
284 processor_result.extra_result.phase_std[detection_idxs],
285 )
286 self.phase_std_dots_detection.show()
287 else:
288 self.phase_std_dots_detection.hide()
289
290 self.phase_std_dots_above.show()
291 self.phase_std_dots_below.show()
292 else:
293 self.phase_std_dots_above.setData(
294 self.distances_m, processor_result.extra_result.phase_std
295 )
296 self.phase_std_dots_below.hide()
297 self.phase_std_dots_detection.hide()
298
299 # History plot
300
301 self.level_history = np.roll(self.level_history, -1)
302 if processor_result.level_m is not None:
303 self.level_history[-1] = processor_result.level_m
304 else:
305 self.level_history[-1] = np.nan
306
307 if np.all(np.isnan(self.level_history)):
308 self.level_history_curve.hide()
309 else:
310 self.level_history_curve.setData(self.hist_xs, self.level_history)
311 self.level_history_curve.show()
312
313 # Level plot
314
315 # Show the percentage level plot if the plot width is greater than 600 pixels,
316 # otherwise display the level as text.
317 if self.win.width() < 600:
318 if processor_result.level_percent is None:
319 self.level_text_item.hide()
320 self.no_detection_text_item.show()
321 elif processor_result.level_percent > 100: # Overflow
322 level_text = "Overflow"
323 level_html = self.level_html_format.format(level_text)
324 self.level_text_item.setHtml(level_html)
325 self.level_text_item.show()
326 self.no_detection_text_item.hide()
327 elif processor_result.level_percent > 0: # In bin detection
328 assert processor_result.level_m is not None
329 assert processor_result.level_percent is not None
330 level_text = (
331 f"Level: {processor_result.level_m:.2f} m, "
332 f"{processor_result.level_percent:.0f} %"
333 )
334 level_html = self.level_html_format.format(level_text)
335 self.level_text_item.setHtml(level_html)
336 self.level_text_item.show()
337 self.no_detection_text_item.hide()
338 else: # No detection
339 self.level_text_item.hide()
340 self.no_detection_text_item.show()
341
342 for rect in self.rects:
343 rect.setVisible(False)
344 else:
345 if processor_result.level_percent is None: # No detection
346 level_text = "No detection"
347 for rect in self.rects:
348 rect.setBrush(et.utils.pg_brush_cycler(7))
349 self.level_text_item.hide()
350 self.no_detection_text_item.show()
351 elif processor_result.level_percent > 100: # Overflow
352 for rect in self.rects:
353 rect.setBrush(et.utils.pg_brush_cycler(0))
354
355 level_text = "Overflow"
356 level_html = self.level_html_format.format(level_text)
357 self.level_text_item.setHtml(level_html)
358 self.level_text_item.show()
359 self.no_detection_text_item.hide()
360 else: # In bin detection
361 self.bar_loc = int(
362 np.around(processor_result.level_percent / 100 * self.num_rects)
363 )
364 for rect in self.rects[: self.bar_loc]:
365 rect.setBrush(et.utils.pg_brush_cycler(0))
366
367 for rect in self.rects[self.bar_loc :]:
368 rect.setBrush(et.utils.pg_brush_cycler(7))
369
370 assert processor_result.level_m is not None
371 assert processor_result.level_percent is not None
372 level_text = (
373 f"Level: {processor_result.level_m:.2f} m, "
374 f"{processor_result.level_percent:.0f} %"
375 )
376 level_html = self.level_html_format.format(level_text)
377 self.level_text_item.setHtml(level_html)
378 self.level_text_item.show()
379 self.no_detection_text_item.hide()
380
381 for rect in self.rects:
382 rect.setVisible(True)
383
384 @staticmethod
385 def _create_phase_std_plot(parent: pg.GraphicsLayout) -> pg.PlotItem:
386 phase_std_plot = parent.addPlot(row=1, col=0, colspan=3)
387 phase_std_plot.setTitle("Phase standard deviation")
388 phase_std_plot.setLabel(axis="bottom", text="Distance [m]")
389 phase_std_plot.setLabel("left", "Phase std")
390 phase_std_plot.setMenuEnabled(False)
391 phase_std_plot.setMouseEnabled(x=False, y=False)
392 phase_std_plot.hideButtons()
393 phase_std_plot.showGrid(x=True, y=True, alpha=0.5)
394 phase_std_plot.setYRange(0, 4)
395
396 return phase_std_plot
397
398 @staticmethod
399 def _create_history_plot(parent: pg.GraphicsLayout) -> pg.PlotItem:
400 history_plot = parent.addPlot(row=0, col=1, colspan=2)
401 history_plot.setTitle("Level history")
402 history_plot.setMenuEnabled(False)
403 history_plot.showGrid(x=True, y=True)
404 history_plot.setLabel("left", "Estimated level (m)")
405 history_plot.setLabel("bottom", "Time (s)")
406
407 return history_plot
408
409
410if __name__ == "__main__":
411 main()
View this example on GitHub: acconeer/acconeer-python-exploration