examples/a121/algo/surface_velocity/processor.py

examples/a121/algo/surface_velocity/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.surface_velocity._processor import Processor, ProcessorConfig
 16
 17
 18def main():
 19    args = a121.ExampleArgumentParser().parse_args()
 20    et.utils.config_logging(args)
 21
 22    client = a121.Client.open(**a121.get_client_args(args))
 23
 24    sensor_config = a121.SensorConfig(
 25        profile=a121.Profile.PROFILE_3,
 26        start_point=180,
 27        num_points=4,
 28        step_length=6,
 29        hwaas=16,
 30        sweeps_per_frame=128,
 31        sweep_rate=2000,
 32        continuous_sweep_mode=True,
 33        double_buffering=True,
 34        inter_frame_idle_state=a121.IdleState.READY,
 35        inter_sweep_idle_state=a121.IdleState.READY,
 36    )
 37
 38    metadata = client.setup_session(sensor_config)
 39
 40    processor_config = ProcessorConfig(
 41        surface_distance=0.40,
 42        time_series_length=1024,
 43    )
 44
 45    processor = Processor(
 46        sensor_config=sensor_config,
 47        metadata=metadata,
 48        processor_config=processor_config,
 49    )
 50
 51    pg_updater = PGUpdater(
 52        processor_config,
 53        sensor_config,
 54    )
 55
 56    pg_process = et.PGProcess(pg_updater)
 57    pg_process.start()
 58
 59    client.start_session()
 60
 61    interrupt_handler = et.utils.ExampleInterruptHandler()
 62
 63    print(f"Measured distances (m): {np.around(processor.distances, 2)}")
 64
 65    while not interrupt_handler.got_signal:
 66        result = client.get_next()
 67        processor_result = processor.process(result)
 68        print(
 69            f"Estimated velocity {np.around(processor_result.estimated_v, 2)} m/s, "
 70            f"at distance {np.around(processor_result.distance_m, 2)} m"
 71        )
 72
 73        try:
 74            pg_process.put_data(processor_result)
 75        except et.PGProccessDiedException:
 76            break
 77
 78    print("Disconnecting...")
 79    pg_process.close()
 80    client.close()
 81
 82
 83class PGUpdater:
 84    _VELOCITY_Y_SCALE_MARGIN_M = 0.25
 85
 86    def __init__(
 87        self,
 88        processor_config: ProcessorConfig,
 89        sensor_config: a121.SensorConfig,
 90    ):
 91        self.slow_zone = processor_config.slow_zone
 92        self.history_length_s = 10
 93        if sensor_config.frame_rate is None:
 94            estimated_frame_rate = sensor_config.sweep_rate / sensor_config.sweeps_per_frame
 95        else:
 96            estimated_frame_rate = sensor_config.frame_rate
 97
 98        self.history_length_n = int(np.around(self.history_length_s * estimated_frame_rate))
 99
100        self.setup_is_done = False
101
102    def setup(self, win):
103        c0_dashed_pen = et.utils.pg_pen_cycler(0, width=2.5, style="--")
104
105        # Velocity plot
106
107        self.velocity_history_plot = self._create_plot(win, row=0, col=0)
108        self.velocity_history_plot.setTitle("Estimated velocity")
109        self.velocity_history_plot.setLabel(axis="left", text="Velocity", units="m/s")
110        self.velocity_history_plot.setLabel(axis="bottom", text="Time", units="s")
111        self.velocity_history_plot.addLegend(labelTextSize="10pt")
112        self.velocity_smooth_limits = et.utils.SmoothLimits()
113
114        self.velocity_curve = self.velocity_history_plot.plot(
115            pen=et.utils.pg_pen_cycler(0), name="Estimated velocity"
116        )
117
118        self.psd_html = (
119            '<div style="text-align: center">'
120            '<span style="color: #FFFFFF;font-size:13pt;">'
121            "{}</span></div>"
122        )
123
124        self.distance_text_item = pg.TextItem(
125            html=self.psd_html,
126            fill=pg.mkColor(0x1F, 0x77, 0xB4, 180),
127            anchor=(0.5, 0),
128        )
129
130        self.velocity_history_plot.addItem(self.distance_text_item)
131
132        self.velocity_history = np.zeros(self.history_length_n)
133
134        self.lower_std_history = np.zeros(self.history_length_n)
135        self.upper_std_history = np.zeros(self.history_length_n)
136
137        self.lower_std_curve = self.velocity_history_plot.plot()
138        self.upper_std_curve = self.velocity_history_plot.plot()
139
140        fbi = pg.FillBetweenItem(
141            self.lower_std_curve,
142            self.upper_std_curve,
143            brush=pg.mkBrush(f"{et.utils.color_cycler(0)}50"),
144        )
145
146        self.velocity_history_plot.addItem(fbi)
147
148        # PSD plot
149
150        self.psd_plot = self._create_plot(win, row=1, col=0)
151        self.psd_plot.setTitle("PSD<br>(colored area represents the slow zone)")
152        self.psd_plot.setLabel(axis="left", text="Power")
153        self.psd_plot.setLabel(axis="bottom", text="Velocity", units="m/s")
154        self.psd_plot.addLegend(labelTextSize="10pt")
155
156        self.psd_smooth_max = et.utils.SmoothMax(tau_grow=0.5, tau_decay=2.0)
157        self.psd_curve = self.psd_plot.plot(pen=et.utils.pg_pen_cycler(0), name="PSD")
158        self.psd_threshold = self.psd_plot.plot(pen=c0_dashed_pen, name="Threshold")
159
160        psd_slow_zone_color = et.utils.color_cycler(0)
161        psd_slow_zone_color = f"{psd_slow_zone_color}50"
162        psd_slow_zone_brush = pg.mkBrush(psd_slow_zone_color)
163
164        self.psd_slow_zone = pg.LinearRegionItem(brush=psd_slow_zone_brush, movable=False)
165        self.psd_plot.addItem(self.psd_slow_zone)
166
167        brush = et.utils.pg_brush_cycler(0)
168        self.psd_peak_plot_item = pg.PlotDataItem(
169            pen=None, symbol="o", symbolSize=8, symbolBrush=brush, symbolPen="k"
170        )
171        self.psd_plot.addItem(self.psd_peak_plot_item)
172
173        self.psd_plot.setLogMode(x=False, y=True)
174
175    def update(self, processor_result) -> None:
176        processor_extra_result = processor_result.extra_result
177
178        lim = self.velocity_smooth_limits.update(processor_result.estimated_v)
179
180        self.velocity_history_plot.setYRange(
181            lim[0] - self._VELOCITY_Y_SCALE_MARGIN_M, lim[1] + self._VELOCITY_Y_SCALE_MARGIN_M
182        )
183        self.velocity_history_plot.setXRange(-self.history_length_s, 0)
184
185        xs = np.linspace(-self.history_length_s, 0, self.history_length_n)
186
187        self.velocity_history = np.roll(self.velocity_history, -1)
188        self.velocity_history[-1] = processor_result.estimated_v
189        self.velocity_curve.setData(xs, self.velocity_history)
190
191        velocity_html = self.psd_html.format(
192            f"Distance {np.around(processor_result.distance_m, 2)} m"
193        )
194        self.distance_text_item.setHtml(velocity_html)
195        self.distance_text_item.setPos(
196            -self.history_length_s / 2, lim[1] + self._VELOCITY_Y_SCALE_MARGIN_M
197        )
198
199        self.lower_std_history = np.roll(self.lower_std_history, -1)
200        self.lower_std_history[-1] = (
201            processor_result.estimated_v + 0.5 * processor_extra_result.peak_width
202        )
203        self.lower_std_curve.setData(xs, self.lower_std_history)
204
205        self.upper_std_history = np.roll(self.upper_std_history, -1)
206        self.upper_std_history[-1] = (
207            processor_result.estimated_v - 0.5 * processor_extra_result.peak_width
208        )
209        self.upper_std_curve.setData(xs, self.upper_std_history)
210
211        lim = self.psd_smooth_max.update(processor_extra_result.psd)
212        self.psd_plot.setYRange(np.log(0.5), np.log(lim))
213        self.psd_plot.setXRange(
214            processor_extra_result.max_bin_vertical_vs[0],
215            processor_extra_result.max_bin_vertical_vs[-1],
216        )
217        self.psd_curve.setData(
218            processor_extra_result.vertical_velocities, processor_extra_result.psd
219        )
220        self.psd_threshold.setData(
221            processor_extra_result.vertical_velocities, processor_extra_result.psd_threshold
222        )
223        if processor_extra_result.peak_idx is not None:
224            self.psd_peak_plot_item.setData(
225                [processor_extra_result.vertical_velocities[processor_extra_result.peak_idx]],
226                [processor_extra_result.psd[processor_extra_result.peak_idx]],
227            )
228        else:
229            self.psd_peak_plot_item.clear()
230
231        middle_idx = int(np.around(processor_extra_result.vertical_velocities.shape[0] / 2))
232        self.psd_slow_zone.setRegion(
233            [
234                processor_extra_result.vertical_velocities[middle_idx - self.slow_zone],
235                processor_extra_result.vertical_velocities[middle_idx + self.slow_zone],
236            ]
237        )
238
239    @staticmethod
240    def _create_plot(parent: pg.GraphicsLayout, row: int, col: int) -> pg.PlotItem:
241        velocity_history_plot = parent.addPlot(row=row, col=col)
242        velocity_history_plot.setMenuEnabled(False)
243        velocity_history_plot.setMouseEnabled(x=False, y=False)
244        velocity_history_plot.hideButtons()
245        velocity_history_plot.showGrid(x=True, y=True, alpha=0.5)
246
247        return velocity_history_plot
248
249
250if __name__ == "__main__":
251    main()

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