examples/a121/algo/speed/detector.py

examples/a121/algo/speed/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
  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._utils import estimate_frame_rate
 16from acconeer.exptool.a121.algo.speed import (
 17    Detector,
 18    DetectorConfig,
 19    DetectorMetadata,
 20    DetectorResult,
 21)
 22from acconeer.exptool.app.new.ui.stream_tab.plugin_widget import PluginPlotArea
 23
 24
 25def main():
 26    args = a121.ExampleArgumentParser().parse_args()
 27    et.utils.config_logging(args)
 28
 29    client = a121.Client.open(**a121.get_client_args(args))
 30
 31    # Detector config set to default values for clarity.
 32    detector_config = DetectorConfig(
 33        start_point=200,
 34        num_points=1,
 35        step_length=None,
 36        profile=None,
 37        frame_rate=None,
 38        sweep_rate=None,
 39        hwaas=None,
 40        num_bins=50,
 41        max_speed=10.0,
 42        threshold=100.0,
 43    )
 44
 45    SENSOR_ID = 1
 46
 47    detector = Detector(client=client, sensor_id=SENSOR_ID, detector_config=detector_config)
 48    sensor_config = detector._get_sensor_config(detector_config)
 49    session_config = a121.SessionConfig(
 50        {SENSOR_ID: sensor_config},
 51        extended=False,
 52    )
 53
 54    estimated_frame_rate = estimate_frame_rate(client, session_config)
 55
 56    detector.start()
 57
 58    pg_updater = PGUpdater(
 59        detector_config,
 60        sensor_config,
 61        estimated_frame_rate,
 62    )
 63    pg_process = et.PGProcess(pg_updater)
 64    pg_process.start()
 65
 66    interrupt_handler = et.utils.ExampleInterruptHandler()
 67
 68    while not interrupt_handler.got_signal:
 69        detector_result = detector.get_next()
 70        try:
 71            pg_process.put_data(detector_result)
 72        except et.PGProccessDiedException:
 73            break
 74
 75    detector.stop()
 76
 77    print("Disconnecting...")
 78    client.close()
 79
 80
 81class PGUpdater:
 82    def __init__(
 83        self,
 84        detector_config: DetectorConfig,
 85        detector_metadata: DetectorMetadata,
 86        estimated_frame_rate: float,
 87    ):
 88        self.detector_config = detector_config
 89
 90        self.n_depths = detector_metadata.num_points
 91
 92        max_update_rate = PluginPlotArea._FPS
 93
 94        if estimated_frame_rate > max_update_rate:
 95            plugin_frame_rate = float(max_update_rate)
 96        else:
 97            plugin_frame_rate = estimated_frame_rate
 98
 99        self.history_length_s = 10.0
100        self.history_length = int(self.history_length_s * plugin_frame_rate)
101
102        self.time_window_length_s = 3.0
103        self.time_window_length_n = int(self.time_window_length_s * plugin_frame_rate)
104
105        self.speed_history = np.zeros(self.history_length)
106        self.speed_history_xs = np.array([i for i in range(-self.history_length, 0)])
107
108        n_ticks_to_display = 10
109        x_labels = np.linspace(-self.history_length_s, 0, self.history_length)
110        all_ticks = [(t, f"{label:.0f}") for t, label in zip(self.speed_history_xs, x_labels)]
111        subsample_step = self.history_length // n_ticks_to_display
112        self.display_ticks = [all_ticks[::subsample_step]]
113
114        self.setup_is_done = False
115
116    def setup(self, win):
117        win.setWindowTitle("Acconeer speed detection example")
118
119        self.raw_fft_plot = win.addPlot(row=1, col=0)
120        self.raw_fft_plot.setTitle("Frequency data")
121        self.raw_fft_plot.setLabel(axis="left", text="Amplitude")
122        self.raw_fft_plot.setLabel(axis="bottom", text="Frequency", units="Hz")
123        self.raw_fft_plot.addLegend(labelTextSize="10pt")
124        self.raw_fft_limits = et.utils.SmoothLimits()
125        self.raw_fft_plot.setMenuEnabled(False)
126        self.raw_fft_plot.setMouseEnabled(x=False, y=False)
127        self.raw_fft_plot.hideButtons()
128        self.raw_fft_plot.showGrid(x=True, y=True)
129        self.raw_fft_plot.setLogMode(x=False, y=True)
130        self.raw_fft_curves = []
131        self.raw_fft_smooth_max = et.utils.SmoothMax(self.detector_config.frame_rate)
132        self.raw_thresholds_curves = []
133
134        for i in range(self.n_depths):
135            raw_fft_curve = self.raw_fft_plot.plot(pen=et.utils.pg_pen_cycler(i), name="Fft")
136            threshold_curve = self.raw_fft_plot.plot(
137                pen=et.utils.pg_pen_cycler(i), name="Threshold"
138            )
139            self.raw_fft_curves.append(raw_fft_curve)
140            self.raw_thresholds_curves.append(threshold_curve)
141
142        self.speed_history_plot = win.addPlot(row=0, col=0)
143        self.speed_history_plot.setTitle("Speed history")
144        self.speed_history_plot.setLabel(axis="left", text="Speed", units="m/s")
145        self.speed_history_plot.setLabel(axis="bottom", text="Time", units="Seconds")
146        self.speed_history_plot.addLegend(labelTextSize="10pt")
147        self.speed_history_curve = self.speed_history_plot.plot(
148            pen=None,
149            name="speed",
150            symbol="o",
151            symbolsize=3,
152        )
153
154        if self.detector_config.sweep_rate is not None:
155            actual_max_speed = DetectorConfig._get_max_speed(self.detector_config.sweep_rate)
156            self.speed_history_plot.setYRange(-actual_max_speed, actual_max_speed)
157        else:
158            self.speed_history_plot.setYRange(
159                -self.detector_config.max_speed, self.detector_config.max_speed
160            )
161        self.speed_history_plot.setXRange(-self.history_length, 0)
162        ay = self.speed_history_plot.getAxis("bottom")
163        ay.setTicks(self.display_ticks)
164
165        self.speed_html_format = (
166            '<div style="text-align: center">'
167            '<span style="color: #FFFFFF;font-size:15pt;">'
168            "{}</span></div>"
169        )
170
171        self.speed_text_item = pg.TextItem(
172            fill=pg.mkColor(0xFF, 0x7F, 0x0E, 200),
173            anchor=(0.5, 0.5),
174        )
175
176        self.speed_text_item.setPos(-self.history_length / 2, -self.detector_config.max_speed / 2)
177        brush = et.utils.pg_brush_cycler(1)
178        self.speed_history_peak_plot_item = pg.PlotDataItem(
179            pen=None, symbol="o", symbolSize=8, symbolBrush=brush, symbolPen="k"
180        )
181        self.speed_history_plot.addItem(self.speed_history_peak_plot_item)
182        self.speed_history_plot.addItem(self.speed_text_item)
183
184        self.speed_text_item.hide()
185
186        self.setup_is_done = True
187
188    def update(self, data: DetectorResult) -> None:
189        psd = data.extra_result.psd
190        speed_guess = data.max_speed
191        x_speeds = data.extra_result.velocities
192        thresholds = data.extra_result.actual_thresholds
193
194        self.speed_history = np.roll(self.speed_history, -1)
195
196        self.speed_history[-1] = speed_guess
197
198        if self.time_window_length_n > 0:
199            pos_speed = np.max(self.speed_history[-self.time_window_length_n :])
200            pos_ind = int(np.argmax(self.speed_history[-self.time_window_length_n :]))
201            neg_speed = np.min(self.speed_history[-self.time_window_length_n :])
202            neg_ind = int(np.argmin(self.speed_history[-self.time_window_length_n :]))
203
204            if abs(neg_speed) > abs(pos_speed):
205                max_display_speed = neg_speed
206                max_display_ind = neg_ind
207            else:
208                max_display_speed = pos_speed
209                max_display_ind = pos_ind
210        else:
211            max_display_speed = self.speed_history[-1]
212            max_display_ind = -1
213
214        if max_display_speed != 0.0:
215            speed_text = f"Max speed estimate {max_display_speed:.4f} m/s"
216            speed_html = self.speed_html_format.format(speed_text)
217
218            self.speed_text_item.setHtml(speed_html)
219            self.speed_text_item.show()
220
221            sub_xs = self.speed_history_xs[-self.time_window_length_n :]
222            self.speed_history_peak_plot_item.setData(
223                [sub_xs[max_display_ind]], [max_display_speed]
224            )
225        else:
226            self.speed_history_peak_plot_item.clear()
227            self.speed_text_item.hide()
228
229        display_inds = np.array([i for i, x in enumerate(self.speed_history) if x != 0.0])
230        if len(display_inds) > 0:
231            display_xs = self.speed_history_xs[display_inds]
232            display_data = self.speed_history[display_inds]
233        else:
234            display_xs = []
235            display_data = []
236        self.speed_history_curve.setData(display_xs, display_data)
237
238        assert psd is not None
239        assert thresholds is not None
240
241        top_max = max(np.max(psd), np.max(thresholds))
242
243        smooth_max_val = np.log10(self.raw_fft_smooth_max.update(top_max))
244        self.raw_fft_plot.setYRange(-2, smooth_max_val)
245        for i in range(psd.shape[1]):
246            self.raw_fft_curves[i].setData(x_speeds, psd[:, i])
247
248            threshold_line = np.full(x_speeds.shape[0], thresholds[i])
249            self.raw_thresholds_curves[i].setData(x_speeds, threshold_line)
250
251
252if __name__ == "__main__":
253    main()

View this example on GitHub: acconeer/acconeer-python-exploration