examples/a121/algo/surface_velocity/processor.py

examples/a121/algo/surface_velocity/processor.py#

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

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