examples/a121/algo/bilateration/bilaterator.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
10
11import pyqtgraph as pg
12
13import acconeer.exptool as et
14from acconeer.exptool import a121
15from acconeer.exptool.a121.algo.bilateration import Processor, ProcessorConfig
16from acconeer.exptool.a121.algo.distance import Detector, DetectorConfig, ThresholdMethod
17
18
19_SENSOR_IDS = [2, 3]
20
21
22def main():
23 args = a121.ExampleArgumentParser().parse_args()
24 et.utils.config_logging(args)
25
26 client = a121.Client.open(**a121.get_client_args(args))
27
28 detector_config = DetectorConfig(
29 start_m=0.25,
30 end_m=1.0,
31 signal_quality=25.0,
32 max_profile=a121.Profile.PROFILE_1,
33 max_step_length=2,
34 threshold_method=ThresholdMethod.CFAR,
35 )
36
37 detector = Detector(client=client, sensor_ids=_SENSOR_IDS, detector_config=detector_config)
38
39 session_config = detector.session_config
40
41 processor_config = ProcessorConfig()
42
43 processor = Processor(
44 session_config=session_config, processor_config=processor_config, sensor_ids=_SENSOR_IDS
45 )
46
47 detector.calibrate_detector()
48
49 detector.start()
50
51 pg_updater = PlotPlugin(detector_config=detector_config, bilateration_config=processor_config)
52 pg_process = et.PGProcess(pg_updater)
53 pg_process.start()
54
55 interrupt_handler = et.utils.ExampleInterruptHandler()
56
57 while not interrupt_handler.got_signal:
58 detector_result = detector.get_next()
59 processor_result = processor.process(detector_result)
60
61 try:
62 pg_process.put_data(
63 {"detector_result": detector_result, "processor_result": processor_result}
64 )
65 except et.PGProccessDiedException:
66 break
67
68 detector.stop()
69
70 print("Disconnecting...")
71 client.close()
72
73
74class PlotPlugin:
75 _NUM_POINTS_ON_CIRCLE = 100
76
77 def __init__(
78 self,
79 bilateration_config: ProcessorConfig,
80 detector_config: DetectorConfig,
81 ) -> None:
82 self.num_curves = 5
83 self.detector_config = detector_config
84 self.sensor_half_spacing_m = bilateration_config.sensor_spacing_m / 2
85
86 def setup(self, win):
87 # Define sweep plot.
88 self.sweep_plot = win.addPlot(row=0, col=0)
89 self.sweep_plot.setMenuEnabled(False)
90 self.sweep_plot.showGrid(x=True, y=True)
91 self.sweep_plot.addLegend()
92 self.sweep_plot.setLabel("left", "Amplitude")
93 self.sweep_plot.setLabel("bottom", "Distance (m)")
94 self.sweep_plot.addItem(pg.PlotDataItem())
95
96 pen_sweep_0 = et.utils.pg_pen_cycler(0)
97 pen_sweep_1 = et.utils.pg_pen_cycler(1)
98
99 # Add sweep curves for the two sensors.
100 feat_kw_1 = dict(pen=pen_sweep_0)
101 feat_kw_2 = dict(pen=pen_sweep_1)
102 self.sweep_curves_1 = [self.sweep_plot.plot(**feat_kw_1) for _ in range(self.num_curves)]
103 self.sweep_curves_2 = [self.sweep_plot.plot(**feat_kw_2) for _ in range(self.num_curves)]
104
105 pen_sweep_0 = et.utils.pg_pen_cycler(0, "--")
106 pen_sweep_1 = et.utils.pg_pen_cycler(1, "--")
107
108 # Add threshold curves for the two sensors.
109 feat_kw_1 = dict(pen=pen_sweep_0)
110 feat_kw_2 = dict(pen=pen_sweep_1)
111 self.threshold_curves_1 = [
112 self.sweep_plot.plot(**feat_kw_1) for _ in range(self.num_curves)
113 ]
114 self.threshold_curves_2 = [
115 self.sweep_plot.plot(**feat_kw_2) for _ in range(self.num_curves)
116 ]
117
118 # Add legends.
119 sweep_plot_legend = pg.LegendItem(offset=(0.0, 0.5))
120 sweep_plot_legend.setParentItem(self.sweep_plot)
121 sweep_plot_legend.addItem(self.sweep_curves_1[0], "Sweep - Left sensor")
122 sweep_plot_legend.addItem(self.threshold_curves_1[0], "Threshold - Left sensor")
123 sweep_plot_legend.addItem(self.sweep_curves_2[0], "Sweep - Right sensor")
124 sweep_plot_legend.addItem(self.threshold_curves_2[0], "Threshold - Right sensor")
125
126 self.sweep_smooth_max = et.utils.SmoothMax()
127
128 # Define obstacle plot.
129 self.obstacle_location_plot = win.addPlot(row=1, col=0)
130 self.obstacle_location_plot.setMenuEnabled(False)
131 self.obstacle_location_plot.setAspectLocked()
132 self.obstacle_location_plot.showGrid(x=True, y=True)
133 self.obstacle_location_plot.addLegend()
134 self.obstacle_location_plot.setLabel("left", "Y (m)")
135 self.obstacle_location_plot.setLabel("bottom", "X (m)")
136 self.obstacle_location_plot.addItem(pg.PlotDataItem())
137 self.obstacle_location_plot.setXRange(
138 -self.detector_config.end_m, self.detector_config.end_m
139 )
140 self.obstacle_location_plot.setYRange(0.0, self.detector_config.end_m)
141
142 pen_sweep_0 = et.utils.pg_pen_cycler(0)
143 brush_sweep_0 = et.utils.pg_brush_cycler(0)
144 symbol_kw = dict(symbol="o", symbolSize=10, symbolBrush=brush_sweep_0, symbolPen=None)
145 feat_kw = dict(pen=None, **symbol_kw)
146 self.obstacle_location_curve = self.obstacle_location_plot.plot(**feat_kw)
147 feat_kw = dict(pen=pen_sweep_0)
148 self.obstacle_location_half_curve = [
149 self.obstacle_location_plot.plot(**feat_kw) for _ in range(Processor._MAX_NUM_OBJECTS)
150 ]
151
152 def update(self, data) -> None:
153 detector_result = data["detector_result"]
154 processor_result = data["processor_result"]
155
156 # Plot sweep data from both distance detectors.
157 max_val = 0.0
158 for distance_detector_result, sweep_curves, threshold_curves in zip(
159 detector_result.values(),
160 [self.sweep_curves_1, self.sweep_curves_2],
161 [self.threshold_curves_1, self.threshold_curves_2],
162 ):
163 for idx, distance_processor_result in enumerate(
164 distance_detector_result.processor_results
165 ):
166 abs_sweep = distance_processor_result.extra_result.abs_sweep
167 threshold = distance_processor_result.extra_result.used_threshold
168 distances_m = distance_processor_result.extra_result.distances_m
169
170 assert abs_sweep is not None
171 assert threshold is not None
172 assert distances_m is not None
173
174 sweep_curves[idx].setData(distances_m, abs_sweep)
175 threshold_curves[idx].setData(distances_m, threshold)
176
177 if max_val < np.max(abs_sweep):
178 max_val = float(np.max(abs_sweep))
179
180 if max_val < np.max(threshold):
181 max_val = float(np.max(threshold))
182
183 if max_val != 0.0:
184 self.sweep_plot.setYRange(0.0, self.sweep_smooth_max.update(max_val))
185
186 # Plot result from bilateration processor.
187 # Start with the points.
188 points = processor_result.points
189 xs = [point.x_coord for point in points]
190 ys = [point.y_coord for point in points]
191 self.obstacle_location_curve.setData(xs, ys)
192
193 # Plot objects without counter part.
194 objects_without_counterpart = processor_result.objects_without_counterpart
195 for i, o in enumerate(objects_without_counterpart):
196 x = np.cos(np.linspace(0, np.pi, self._NUM_POINTS_ON_CIRCLE)) * o.distance
197 y = np.sin(np.linspace(0, np.pi, self._NUM_POINTS_ON_CIRCLE)) * o.distance
198
199 # Offset circle to center around the sensor that detected the object.
200 if o.sensor_position == Processor._SENSOR_POSITION_LEFT:
201 x -= self.sensor_half_spacing_m
202 elif o.sensor_position == Processor._SENSOR_POSITION_RIGHT:
203 x += self.sensor_half_spacing_m
204 else:
205 msg = "Invalid sensor position."
206 raise ValueError(msg)
207
208 self.obstacle_location_half_curve[i].setData(x, y)
209
210 # Remove curves that does not have an object to visualize.
211 for i in range(len(objects_without_counterpart), Processor._MAX_NUM_OBJECTS):
212 self.obstacle_location_half_curve[i].setData([], [])
213
214
215if __name__ == "__main__":
216 main()
View this example on GitHub: acconeer/acconeer-python-exploration