examples/a121/algo/smart_presence/ref_app.py

examples/a121/algo/smart_presence/ref_app.py#

  1# Copyright (c) Acconeer AB, 2023-2026
  2# All rights reserved
  3
  4from __future__ import annotations
  5
  6import numpy as np
  7import numpy.typing as npt
  8
  9from PySide6 import QtCore
 10
 11import pyqtgraph as pg
 12
 13import acconeer.exptool as et
 14from acconeer.exptool import a121
 15from acconeer.exptool.a121.algo.smart_presence._ref_app import (
 16    PresenceWakeUpConfig,
 17    PresenceZoneConfig,
 18    RefApp,
 19    RefAppConfig,
 20    RefAppResult,
 21    _Mode,
 22)
 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    ref_app_config = RefAppConfig(
 32        wake_up_mode=True,
 33        wake_up_config=PresenceWakeUpConfig(
 34            start_m=1.0,
 35            end_m=3.0,
 36            num_zones=5,
 37            num_zones_for_wake_up=2,
 38        ),
 39        nominal_config=PresenceZoneConfig(
 40            start_m=1.0,
 41            end_m=3.0,
 42            num_zones=3,
 43        ),
 44    )
 45
 46    ref_app = RefApp(client=client, sensor_id=1, ref_app_config=ref_app_config)
 47    ref_app.start()
 48
 49    nominal_sensor_config = ref_app.nominal_detector_config.to_sensor_config()
 50    distances = np.linspace(
 51        ref_app_config.nominal_config.start_m,
 52        ref_app_config.nominal_config.end_m,
 53        nominal_sensor_config.num_points,
 54    )
 55    nominal_zone_limits = ref_app.ref_app_processor.create_zones(
 56        distances, ref_app_config.nominal_config.num_zones
 57    )
 58
 59    pg_updater = PGUpdater(
 60        ref_app_config,
 61        ref_app.ref_app_context.wake_up_detector_context.estimated_frame_rate,
 62        nominal_zone_limits,
 63        ref_app.ref_app_processor.zone_limits,
 64    )
 65    pg_process = et.PGProcess(pg_updater)
 66    pg_process.start()
 67
 68    interrupt_handler = et.utils.ExampleInterruptHandler()
 69
 70    while not interrupt_handler.got_signal:
 71        ref_app_result = ref_app.get_next()
 72        if ref_app_result.presence_detected:
 73            print(f"Presence in zone {ref_app_result.max_presence_zone}")
 74        else:
 75            print("No presence")
 76        try:
 77            pg_process.put_data(ref_app_result)
 78        except et.PGProccessDiedException:
 79            break
 80
 81    ref_app.stop()
 82
 83    print("Disconnecting...")
 84    client.close()
 85
 86
 87class PGUpdater:
 88    def __init__(
 89        self,
 90        ref_app_config: RefAppConfig,
 91        estimated_frame_rate: float,
 92        nominal_zone_limits: npt.NDArray[np.float64],
 93        wake_up_zone_limits: npt.NDArray[np.float64],
 94    ) -> None:
 95        self.ref_app_config = ref_app_config
 96        self.nominal_config = ref_app_config.nominal_config
 97        self.wake_up_config = ref_app_config.wake_up_config
 98
 99        self.show_all_detected_zones = ref_app_config.show_all_detected_zones
100        self.nominal_zone_limits = nominal_zone_limits
101        self.wake_up_zone_limits = wake_up_zone_limits
102        self.estimated_frame_rate = estimated_frame_rate
103
104        self.history_length_s = 5
105        self.time_fifo: list[float] = []
106        self.intra_fifo: list[float] = []
107        self.inter_fifo: list[float] = []
108
109        self.intra_limit_lines = []
110        self.inter_limit_lines = []
111
112        self.setup_is_done = False
113
114    def setup(self, win):
115        win.setWindowTitle("Acconeer smart presence example")
116
117        # Intra presence history plot
118
119        self.intra_hist_plot = win.addPlot(
120            row=0,
121            col=0,
122            title="Intra presence history (fast motions)",
123        )
124        self.intra_hist_plot.setMenuEnabled(False)
125        self.intra_hist_plot.setMouseEnabled(x=False, y=False)
126        self.intra_hist_plot.hideButtons()
127        self.intra_hist_plot.showGrid(x=True, y=True)
128        self.intra_hist_plot.setLabel("bottom", "Time (s)")
129        self.intra_hist_plot.setLabel("left", "Score")
130        self.intra_hist_plot.setXRange(-self.history_length_s, 0)
131        self.intra_history_smooth_max = et.utils.SmoothMax(self.estimated_frame_rate)
132        self.intra_hist_plot.setYRange(0, 10)
133        if not self.nominal_config.intra_enable:
134            intra_color = et.utils.color_cycler(1)
135            intra_color = f"{intra_color}50"
136            self.nominal_intra_dashed_pen = pg.mkPen(
137                intra_color, width=2.5, style=QtCore.Qt.DashLine
138            )
139            self.nominal_intra_pen = pg.mkPen(intra_color, width=2)
140        else:
141            self.nominal_intra_dashed_pen = et.utils.pg_pen_cycler(1, width=2.5, style="--")
142            self.nominal_intra_pen = et.utils.pg_pen_cycler(1)
143
144        self.intra_hist_curve = self.intra_hist_plot.plot(pen=self.nominal_intra_pen)
145        limit_line = pg.InfiniteLine(angle=0, pen=self.nominal_intra_dashed_pen)
146        self.intra_hist_plot.addItem(limit_line)
147        self.intra_limit_lines.append(limit_line)
148
149        for line in self.intra_limit_lines:
150            line.setPos(self.nominal_config.intra_detection_threshold)
151
152        # Inter presence history plot
153
154        self.inter_hist_plot = win.addPlot(
155            row=0,
156            col=1,
157            title="Inter presence history (slow motions)",
158        )
159        self.inter_hist_plot.setMenuEnabled(False)
160        self.inter_hist_plot.setMouseEnabled(x=False, y=False)
161        self.inter_hist_plot.hideButtons()
162        self.inter_hist_plot.showGrid(x=True, y=True)
163        self.inter_hist_plot.setLabel("bottom", "Time (s)")
164        self.inter_hist_plot.setLabel("left", "Score")
165        self.inter_hist_plot.setXRange(-self.history_length_s, 0)
166        self.inter_history_smooth_max = et.utils.SmoothMax(self.estimated_frame_rate)
167        self.inter_hist_plot.setYRange(0, 10)
168        if not self.nominal_config.inter_enable:
169            inter_color = et.utils.color_cycler(0)
170            inter_color = f"{inter_color}50"
171            self.nominal_inter_dashed_pen = pg.mkPen(
172                inter_color, width=2.5, style=QtCore.Qt.DashLine
173            )
174            self.nominal_inter_pen = pg.mkPen(inter_color, width=2)
175        else:
176            self.nominal_inter_pen = et.utils.pg_pen_cycler(0)
177            self.nominal_inter_dashed_pen = et.utils.pg_pen_cycler(0, width=2.5, style="--")
178
179        self.inter_hist_curve = self.inter_hist_plot.plot(pen=self.nominal_inter_pen)
180        limit_line = pg.InfiniteLine(angle=0, pen=self.nominal_inter_dashed_pen)
181        self.inter_hist_plot.addItem(limit_line)
182        self.inter_limit_lines.append(limit_line)
183
184        for line in self.inter_limit_lines:
185            line.setPos(self.nominal_config.inter_detection_threshold)
186
187        # Sector plot
188
189        if self.ref_app_config.wake_up_mode:
190            title = (
191                "Nominal config<br>"
192                "Detection type: fast (orange), slow (blue), both (green)<br>"
193                "Green background indicates active"
194            )
195        else:
196            title = "Nominal config<br>Detection type: fast (orange), slow (blue), both (green)"
197
198        self.nominal_sector_plot, self.nominal_sectors = self.create_sector_plot(
199            title,
200            self.ref_app_config.nominal_config.num_zones,
201            self.nominal_config.start_m,
202            self.nominal_zone_limits,
203        )
204
205        if not self.ref_app_config.wake_up_mode:
206            sublayout = win.addLayout(row=1, col=0, colspan=2)
207            sublayout.layout.setColumnStretchFactor(0, 2)
208            sublayout.addItem(self.nominal_sector_plot, row=0, col=0)
209        else:
210            assert self.wake_up_config is not None
211            sublayout = win.addLayout(row=1, col=0, colspan=2)
212            sublayout.addItem(self.nominal_sector_plot, row=0, col=1)
213
214            title = (
215                "Wake up config<br>"
216                "Detection type: fast (orange), slow (blue), both (green),<br>"
217                "lingering (light grey)<br>"
218                "Green background indicates active"
219            )
220            self.wake_up_sector_plot, self.wake_up_sectors = self.create_sector_plot(
221                title,
222                self.wake_up_config.num_zones,
223                self.wake_up_config.start_m,
224                self.wake_up_zone_limits,
225            )
226
227            sublayout.addItem(self.wake_up_sector_plot, row=0, col=0)
228
229            if self.wake_up_config.intra_enable:
230                self.wake_up_intra_dashed_pen = et.utils.pg_pen_cycler(1, width=2.5, style="--")
231                self.wake_up_intra_pen = et.utils.pg_pen_cycler(1)
232            else:
233                intra_color = et.utils.color_cycler(1)
234                intra_color = f"{intra_color}50"
235                self.wake_up_intra_dashed_pen = pg.mkPen(
236                    intra_color, width=2.5, style=QtCore.Qt.DashLine
237                )
238                self.wake_up_intra_pen = pg.mkPen(intra_color, width=2)
239
240            if self.wake_up_config.inter_enable:
241                self.wake_up_inter_pen = et.utils.pg_pen_cycler(0)
242                self.wake_up_inter_dashed_pen = et.utils.pg_pen_cycler(0, width=2.5, style="--")
243            else:
244                inter_color = et.utils.color_cycler(0)
245                inter_color = f"{inter_color}50"
246                self.wake_up_inter_dashed_pen = pg.mkPen(
247                    inter_color, width=2.5, style=QtCore.Qt.DashLine
248                )
249                self.wake_up_inter_pen = pg.mkPen(inter_color, width=2)
250
251    @staticmethod
252    def create_sector_plot(
253        title: str, num_sectors: int, start_m: float, zone_limits: npt.NDArray[np.float64]
254    ) -> tuple[pg.PlotItem, list[pg.QtWidgets.QGraphicsEllipseItem]]:
255        sector_plot = pg.PlotItem(title=title)
256
257        sector_plot.setAspectLocked()
258        sector_plot.hideAxis("left")
259        sector_plot.hideAxis("bottom")
260
261        sectors = []
262        limit_text = []
263
264        range_html = (
265            '<div style="text-align: center">'
266            '<span style="color: #000000;font-size:12pt;">'
267            "{}</span></div>"
268        )
269
270        if start_m == zone_limits[0]:
271            x_offset = 0.7
272        else:
273            x_offset = 0
274
275        pen = pg.mkPen("k", width=1)
276        span_deg = 25
277        for r in np.flip(np.arange(1, num_sectors + 2)):
278            sector = pg.QtWidgets.QGraphicsEllipseItem(-r, -r, r * 2, r * 2)
279            sector.setStartAngle(-16 * span_deg)
280            sector.setSpanAngle(16 * span_deg * 2)
281            sector.setPen(pen)
282            sector_plot.addItem(sector)
283            sectors.append(sector)
284
285            if r != 1:
286                limit = pg.TextItem(html=range_html, anchor=(0.5, 0.5), angle=25)
287                x = r * np.cos(np.radians(span_deg))
288                y = r * np.sin(np.radians(span_deg))
289                limit.setPos(x - x_offset, y + 0.25)
290                sector_plot.addItem(limit)
291                limit_text.append(limit)
292
293        sectors.reverse()
294
295        if not start_m == zone_limits[0]:
296            start_limit_text = pg.TextItem(html=range_html, anchor=(0.5, 0.5), angle=25)
297            start_range_html = range_html.format(f"{start_m}")
298            start_limit_text.setHtml(start_range_html)
299            x = 1 * np.cos(np.radians(span_deg))
300            y = 1 * np.sin(np.radians(span_deg))
301
302            start_limit_text.setPos(x, y + 0.25)
303            sector_plot.addItem(start_limit_text)
304
305        unit_text = pg.TextItem(html=range_html, anchor=(0.5, 0.5))
306        unit_html = range_html.format("[m]")
307        unit_text.setHtml(unit_html)
308        x = (num_sectors + 2) * np.cos(np.radians(span_deg))
309        y = (num_sectors + 2) * np.sin(np.radians(span_deg))
310        unit_text.setPos(x - x_offset, y + 0.25)
311        sector_plot.addItem(unit_text)
312
313        for text_item, limit in zip(limit_text, np.flip(zone_limits)):
314            zone_range_html = range_html.format(np.around(limit, 1))
315            text_item.setHtml(zone_range_html)
316
317        return sector_plot, sectors
318
319    def update(self, data: RefAppResult) -> None:
320        if data.used_config == _Mode.NOMINAL_CONFIG:
321            inter_threshold = self.nominal_config.inter_detection_threshold
322            intra_threshold = self.nominal_config.intra_detection_threshold
323            intra_pen = self.nominal_intra_pen
324            intra_dashed_pen = self.nominal_intra_dashed_pen
325            inter_pen = self.nominal_inter_pen
326            inter_dashed_pen = self.nominal_inter_dashed_pen
327        else:
328            assert self.wake_up_config is not None
329            inter_threshold = self.wake_up_config.inter_detection_threshold
330            intra_threshold = self.wake_up_config.intra_detection_threshold
331            intra_pen = self.wake_up_intra_pen
332            intra_dashed_pen = self.wake_up_intra_dashed_pen
333            inter_pen = self.wake_up_inter_pen
334            inter_dashed_pen = self.wake_up_inter_dashed_pen
335
336        self.time_fifo.append(data.service_result.tick_time)
337
338        if data.switch_delay:
339            self.intra_fifo.append(float("nan"))
340            self.inter_fifo.append(float("nan"))
341        else:
342            self.intra_fifo.append(data.intra_presence_score)
343            self.inter_fifo.append(data.inter_presence_score)
344
345        while self.time_fifo[-1] - self.time_fifo[0] > self.history_length_s:
346            self.time_fifo.pop(0)
347            self.intra_fifo.pop(0)
348            self.inter_fifo.pop(0)
349
350        times = [t - self.time_fifo[-1] for t in self.time_fifo]
351
352        # Intra presence
353
354        if np.isnan(self.intra_fifo).all():
355            m_hist = intra_threshold
356        else:
357            m_hist = np.maximum(float(np.nanmax(self.intra_fifo)), intra_threshold * 1.05)
358
359        m_hist = self.intra_history_smooth_max.update(m_hist)
360
361        self.intra_hist_plot.setYRange(0, m_hist)
362        self.intra_hist_curve.setData(times, self.intra_fifo, connect="finite")
363        self.intra_hist_curve.setPen(intra_pen)
364
365        for line in self.intra_limit_lines:
366            line.setPos(intra_threshold)
367            line.setPen(intra_dashed_pen)
368
369        # Inter presence
370
371        if np.isnan(self.inter_fifo).all():
372            m_hist = inter_threshold
373        else:
374            m_hist = np.maximum(float(np.nanmax(self.inter_fifo)), inter_threshold * 1.05)
375
376        m_hist = self.inter_history_smooth_max.update(m_hist)
377
378        self.inter_hist_plot.setYRange(0, m_hist)
379        self.inter_hist_curve.setData(times, self.inter_fifo, connect="finite")
380        self.inter_hist_curve.setPen(inter_pen)
381
382        for line in self.inter_limit_lines:
383            line.setPos(inter_threshold)
384            line.setPen(inter_dashed_pen)
385
386        # Sector
387
388        brush = et.utils.pg_brush_cycler(7)
389        for sector in self.nominal_sectors:
390            sector.setBrush(brush)
391
392        if not self.ref_app_config.wake_up_mode:
393            sectors = self.nominal_sectors[1:]
394            show_all_zones = self.show_all_detected_zones
395            color_nominal = "white"
396        else:
397            if data.used_config == _Mode.WAKE_UP_CONFIG:
398                sectors = self.wake_up_sectors[1:]
399                show_all_zones = True
400                color_wake_up = "#DFF1D6"
401                color_nominal = "white"
402            else:
403                sectors = self.nominal_sectors[1:]
404                show_all_zones = self.show_all_detected_zones
405                color_wake_up = "white"
406                color_nominal = "#DFF1D6"
407
408            vb = self.nominal_sector_plot.getViewBox()
409            vb.setBackgroundColor(color_nominal)
410            vb = self.wake_up_sector_plot.getViewBox()
411            vb.setBackgroundColor(color_wake_up)
412
413            for sector in self.wake_up_sectors:
414                sector.setBrush(brush)
415
416        if data.presence_detected:
417            self.color_zones(data, show_all_zones, sectors)
418            self.switch_data = data
419        elif data.switch_delay:
420            self.color_zones(self.switch_data, True, self.wake_up_sectors[1:])
421
422        self.nominal_sectors[0].setPen(pg.mkPen(color_nominal, width=1))
423        self.nominal_sectors[0].setBrush(pg.mkBrush(color_nominal))
424
425        if self.ref_app_config.wake_up_mode:
426            self.wake_up_sectors[0].setPen(pg.mkPen(color_wake_up, width=1))
427            self.wake_up_sectors[0].setBrush(pg.mkBrush(color_wake_up))
428
429    @staticmethod
430    def color_zones(
431        data: RefAppResult,
432        show_all_detected_zones: bool,
433        sectors: list[pg.QtWidgets.QGraphicsEllipseItem],
434    ) -> None:
435        if show_all_detected_zones:
436            for zone, (inter_value, intra_value) in enumerate(
437                zip(data.inter_zone_detections, data.intra_zone_detections)
438            ):
439                if inter_value + intra_value == 2:
440                    sectors[zone].setBrush(et.utils.pg_brush_cycler(2))
441                elif inter_value == 1:
442                    sectors[zone].setBrush(et.utils.pg_brush_cycler(0))
443                elif intra_value == 1:
444                    sectors[zone].setBrush(et.utils.pg_brush_cycler(1))
445                elif data.used_config == _Mode.WAKE_UP_CONFIG:
446                    assert data.wake_up_detections is not None
447                    if data.wake_up_detections[zone] > 0:
448                        sectors[zone].setBrush(pg.mkBrush("#b5afa0"))
449        else:
450            assert data.max_presence_zone is not None
451            if data.max_presence_zone == data.max_intra_zone:
452                sectors[data.max_presence_zone].setBrush(et.utils.pg_brush_cycler(1))
453            else:
454                sectors[data.max_presence_zone].setBrush(et.utils.pg_brush_cycler(0))
455
456
457if __name__ == "__main__":
458    main()

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