examples/a121/algo/obstacle/detector.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
9# import PySide6 # noqa: F401
10# from PySide6.QtWidgets import QLabel, QPushButton, QVBoxLayout, QWidget
11from PySide6.QtGui import QTransform
12
13import pyqtgraph as pg
14
15import acconeer.exptool as et
16from acconeer.exptool import a121
17from acconeer.exptool.a121.algo.obstacle import Detector, DetectorConfig, DetectorResult
18
19
20SENSOR_IDS = [2, 3]
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 # Creating a list of subsweep configurations.
30 subsweep_configurations = [
31 a121.SubsweepConfig(
32 start_point=24, # ~6 cm
33 num_points=96,
34 step_length=1,
35 profile=a121.Profile.PROFILE_1,
36 hwaas=4,
37 ),
38 a121.SubsweepConfig(
39 start_point=96, # ~24 cm
40 num_points=48,
41 step_length=4,
42 profile=a121.Profile.PROFILE_3,
43 hwaas=16,
44 ),
45 a121.SubsweepConfig(
46 start_point=300, # ~75 cm
47 num_points=48,
48 step_length=4,
49 profile=a121.Profile.PROFILE_3,
50 hwaas=2,
51 ),
52 ]
53
54 # Configure the detector using multiple subsweeps
55 detector_config = DetectorConfig(
56 num_mean_threshold=1.5,
57 num_std_threshold=4,
58 subsweep_configurations=subsweep_configurations,
59 )
60
61 detector = Detector(client=client, sensor_ids=SENSOR_IDS, detector_config=detector_config)
62
63 detector.calibrate_detector()
64 detector.start()
65
66 pg_updater = PGUpdater(
67 num_sensors=len(SENSOR_IDS),
68 num_subsweeps=len(subsweep_configurations),
69 )
70
71 pg_process = et.PGProcess(pg_updater)
72 pg_process.start()
73
74 interrupt_handler = et.utils.ExampleInterruptHandler()
75
76 while not interrupt_handler.got_signal:
77 # v_current = get_speed_from_some_Robot_API(), e.g ROS?
78 # detector.update_robot_speed(v_current)
79
80 detector_result = detector.get_next()
81
82 try:
83 pg_process.put_data(detector_result)
84 except et.PGProccessDiedException:
85 break
86
87 detector.stop()
88
89 print("Disconnecting...")
90 client.close()
91
92
93PLOT_HISTORY_FRAMES = 50
94PLOT_THRESHOLDS = True
95
96
97class PGUpdater:
98 def __init__(self, num_sensors, num_subsweeps):
99 self.num_sensors = num_sensors
100 self.num_subsweeps = num_subsweeps
101 self.obst_vel_ys = num_sensors * [np.nan * np.ones(PLOT_HISTORY_FRAMES)]
102 self.obst_dist_ys = num_sensors * [np.nan * np.ones(PLOT_HISTORY_FRAMES)]
103 self.obst_bil_ys = np.nan * np.ones(PLOT_HISTORY_FRAMES)
104 self.hist_x = np.linspace(-100, 0, PLOT_HISTORY_FRAMES)
105
106 def setup(self, win: pg.GraphicsLayout):
107 self.fftmap_plots: list[pg.PlotItem] = []
108 self.fftmap_images: list[pg.ImageItem] = []
109 self.range_hist_curves: list[pg.PlotDataItem] = []
110 self.angle_hist_curves: list[pg.PlotDataItem] = []
111
112 if PLOT_THRESHOLDS:
113 self.bin0_curves: list[pg.PlotDataItem] = []
114 self.bin0_threshold_curves: list[pg.PlotDataItem] = []
115 self.other_bins_curves: list[pg.PlotDataItem] = []
116 self.other_bins_threshold_curves: list[pg.PlotDataItem] = []
117
118 row_offset = 2 if PLOT_THRESHOLDS else 0
119
120 for i_s in range(self.num_sensors):
121 for i_ss in range(self.num_subsweeps):
122 col = i_s * self.num_subsweeps + i_ss
123 p = win.addPlot(
124 row=0, col=col, title=f"FFT Map, sensor {SENSOR_IDS[i_s]}, subsweep {i_ss}"
125 )
126 im = pg.ImageItem(autoDownsample=True)
127 im.setLookupTable(et.utils.pg_mpl_cmap("viridis"))
128 self.fftmap_images.append(im)
129
130 p.setLabel("bottom", "Distance (cm)")
131 p.setLabel("left", "Velocity (cm/s)")
132 p.addItem(im)
133
134 self.fftmap_plots.append(p)
135
136 if PLOT_THRESHOLDS:
137 self.bin0 = pg.PlotItem(title="Zeroth velocity/angle bin")
138 self.bin0.showGrid(x=True, y=True)
139 self.bin0.setLabel("bottom", "Range (cm)")
140 self.bin0.setLabel("left", "Amplitude")
141 self.bin0.addLegend()
142
143 pen = et.utils.pg_pen_cycler(0)
144 brush = et.utils.pg_brush_cycler(0)
145 symbol_kw = dict(symbol="o", symbolSize=1, symbolBrush=brush, symbolPen="k")
146 feat_kw = dict(pen=pen, **symbol_kw)
147 self.bin0_curves += [self.bin0.plot(**feat_kw) for _ in range(self.num_subsweeps)]
148
149 pen = et.utils.pg_pen_cycler(1)
150 brush = et.utils.pg_brush_cycler(1)
151 symbol_kw = dict(symbol="o", symbolSize=1, symbolBrush=brush, symbolPen="k")
152 feat_kw = dict(pen=pen, **symbol_kw)
153 self.bin0_threshold_curves += [
154 self.bin0.plot(**feat_kw) for _ in range(self.num_subsweeps)
155 ]
156
157 bin0_plot_legend = pg.LegendItem(offset=(0.0, 0.5))
158 bin0_plot_legend.setParentItem(self.bin0)
159 bin0_plot_legend.addItem(self.bin0_curves[0], "Sweep")
160 bin0_plot_legend.addItem(self.bin0_threshold_curves[0], "Threshold")
161
162 sublayout = win.addLayout(
163 row=1,
164 col=i_s * self.num_subsweeps,
165 colspan=self.num_subsweeps,
166 )
167 sublayout.layout.setColumnStretchFactor(0, self.num_subsweeps)
168 sublayout.addItem(self.bin0, row=0, col=0)
169
170 self.other_bins = pg.PlotItem(title="Other velocity/angle bins")
171 self.other_bins.showGrid(x=True, y=True)
172 self.other_bins.setLabel("bottom", "Range (cm)")
173 self.other_bins.setLabel("left", "Amplitude")
174 self.other_bins.addLegend()
175
176 pen = et.utils.pg_pen_cycler(0)
177 brush = et.utils.pg_brush_cycler(0)
178 symbol_kw = dict(symbol="o", symbolSize=1, symbolBrush=brush, symbolPen="k")
179 feat_kw = dict(pen=pen, **symbol_kw)
180 self.other_bins_curves += [
181 self.other_bins.plot(**feat_kw) for _ in range(self.num_subsweeps)
182 ]
183
184 pen = et.utils.pg_pen_cycler(1)
185 brush = et.utils.pg_brush_cycler(1)
186 symbol_kw = dict(symbol="o", symbolSize=1, symbolBrush=brush, symbolPen="k")
187 feat_kw = dict(pen=pen, **symbol_kw)
188 self.other_bins_threshold_curves += [
189 self.other_bins.plot(**feat_kw) for _ in range(self.num_subsweeps)
190 ]
191
192 other_bins_plot_legend = pg.LegendItem(offset=(0.0, 0.5))
193 other_bins_plot_legend.setParentItem(self.other_bins)
194 other_bins_plot_legend.addItem(self.other_bins_curves[0], "Max Sweep")
195 other_bins_plot_legend.addItem(self.other_bins_threshold_curves[0], "Threshold")
196
197 sublayout = win.addLayout(
198 row=2,
199 col=i_s * self.num_subsweeps,
200 colspan=self.num_subsweeps,
201 )
202 sublayout.layout.setColumnStretchFactor(0, self.num_subsweeps)
203 sublayout.addItem(self.other_bins, row=0, col=0)
204
205 self.angle_hist = pg.PlotItem(title="Angle/velocity history")
206 self.angle_hist.showGrid(x=True, y=True)
207 self.angle_hist.setLabel("bottom", "Time (frames)")
208 self.angle_hist.setLabel("left", "velocity (cm/s)")
209 self.angle_hist.setXRange(-100, 0)
210 self.angle_hist.addLegend()
211 self.angle_hist_curves.append(self.angle_hist.plot(symbolSize=5, symbol="o"))
212
213 sublayout = win.addLayout(
214 row=1 + row_offset,
215 col=i_s * self.num_subsweeps,
216 colspan=self.num_subsweeps,
217 )
218
219 sublayout.layout.setColumnStretchFactor(0, self.num_subsweeps)
220 sublayout.addItem(self.angle_hist, row=0, col=0)
221
222 self.range_hist = pg.PlotItem(title="Range history")
223 self.range_hist.showGrid(x=True, y=True)
224 self.range_hist.setLabel("bottom", "Time (frames)")
225 self.range_hist.setLabel("left", "Range (cm)")
226 self.range_hist.setXRange(-100, 0)
227 self.range_hist.addLegend()
228 self.range_hist_curves.append(self.range_hist.plot(symbolSize=5, symbol="o"))
229
230 sublayout = win.addLayout(
231 row=2 + row_offset,
232 col=i_s * self.num_subsweeps,
233 colspan=self.num_subsweeps,
234 )
235
236 sublayout.layout.setColumnStretchFactor(0, self.num_subsweeps)
237 sublayout.addItem(self.range_hist, row=0, col=0)
238
239 def update(self, detector_result: DetectorResult):
240 for i_s in range(self.num_sensors):
241 pr = detector_result.processor_results[SENSOR_IDS[i_s]]
242
243 for i_ss in range(self.num_subsweeps):
244 curve_idx = self.num_subsweeps * i_s + i_ss
245
246 fftmap = pr.subsweeps_extra_results[i_ss].fft_map
247 fftmap_threshold = pr.subsweeps_extra_results[i_ss].fft_map_threshold
248
249 # fftmap = 10*np.log10(fftmap) # to dB
250
251 spf = fftmap.shape[0]
252 r = 100 * pr.subsweeps_extra_results[i_ss].r
253
254 transform = QTransform()
255 transform.translate(
256 r[0], -100 * pr.extra_result.dv * spf / 2 - 0.5 * 100 * pr.extra_result.dv
257 )
258 transform.scale(r[1] - r[0], 100 * pr.extra_result.dv)
259
260 self.fftmap_images[curve_idx].setTransform(transform)
261
262 self.fftmap_images[curve_idx].updateImage(
263 np.fft.fftshift(fftmap, 0).T,
264 levels=(0, 1.05 * np.max(fftmap)),
265 )
266
267 if PLOT_THRESHOLDS:
268 bin0 = fftmap[0, :]
269 threshold_bin0 = fftmap_threshold[0, :]
270
271 max_other_bins = np.max(fftmap[1:, :], axis=0)
272 threshold_other_bins = fftmap_threshold[1, :]
273
274 self.bin0_curves[curve_idx].setData(r, bin0)
275 self.bin0_threshold_curves[curve_idx].setData(r, threshold_bin0)
276 self.other_bins_curves[curve_idx].setData(r, max_other_bins)
277 self.other_bins_threshold_curves[curve_idx].setData(r, threshold_other_bins)
278
279 v = pr.targets[0].velocity if pr.targets else np.nan
280
281 self.obst_vel_ys[i_s] = np.roll(self.obst_vel_ys[i_s], -1)
282 self.obst_vel_ys[i_s][-1] = 100 * v # m/s -> cm/s
283
284 if np.isnan(self.obst_vel_ys[i_s]).all():
285 self.angle_hist_curves[i_s].setVisible(False)
286 else:
287 self.angle_hist_curves[i_s].setVisible(True)
288 self.angle_hist_curves[i_s].setData(
289 self.hist_x, self.obst_vel_ys[i_s], connect="finite"
290 )
291
292 # TODO: should be known earlier
293 # r_min = min([er.r[0] for er in pr.subsweeps_extra_results])
294 # r_max = min([er.r[-1] for er in pr.subsweeps_extra_results])
295 # self.range_hist.setYRange(100*r_min, 100*r_max)
296
297 r_targets = pr.targets[0].distance if pr.targets else np.nan
298
299 self.obst_dist_ys[i_s] = np.roll(self.obst_dist_ys[i_s], -1)
300 self.obst_dist_ys[i_s][-1] = 100 * r_targets # m -> cm
301
302 # print(f'{i_s}: r = {r_targets}, v = {v}')
303
304 if np.isnan(self.obst_dist_ys[i_s]).all():
305 self.range_hist_curves[i_s].setVisible(False)
306 else:
307 self.range_hist_curves[i_s].setVisible(True)
308 self.range_hist_curves[i_s].setData(
309 self.hist_x, self.obst_dist_ys[i_s], connect="finite"
310 )
311
312
313if __name__ == "__main__":
314 main()
View this example on GitHub: acconeer/acconeer-python-exploration