examples/a121/algo/speed/processor.py

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

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