From e15b9eadab621ce6f8f6baa288e5f5d06d0bf38d Mon Sep 17 00:00:00 2001 From: dara Date: Thu, 30 Nov 2023 17:44:24 +1100 Subject: [PATCH 1/2] init commit --- .../IMUplot/imu_bokeh_stream/bokeh_plot.py | 289 +++++++++++++++++ .../IMUplot/imu_bokeh_stream/imu_sensor.py | 292 ++++++++++++++++++ .../examples/IMUplot/imu_bokeh_stream/main.py | 9 + .../IMUplot/imu_bokeh_stream/stack.py | 18 ++ depthai_sdk/examples/IMUplot/readme.md | 36 +++ depthai_sdk/examples/IMUplot/requirements.txt | 3 + 6 files changed, 647 insertions(+) create mode 100644 depthai_sdk/examples/IMUplot/imu_bokeh_stream/bokeh_plot.py create mode 100644 depthai_sdk/examples/IMUplot/imu_bokeh_stream/imu_sensor.py create mode 100644 depthai_sdk/examples/IMUplot/imu_bokeh_stream/main.py create mode 100644 depthai_sdk/examples/IMUplot/imu_bokeh_stream/stack.py create mode 100644 depthai_sdk/examples/IMUplot/readme.md create mode 100644 depthai_sdk/examples/IMUplot/requirements.txt diff --git a/depthai_sdk/examples/IMUplot/imu_bokeh_stream/bokeh_plot.py b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/bokeh_plot.py new file mode 100644 index 000000000..9fdd4a650 --- /dev/null +++ b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/bokeh_plot.py @@ -0,0 +1,289 @@ +from dataclasses import asdict, dataclass, field +from itertools import cycle +from pprint import pformat +from threading import Event +from typing import TYPE_CHECKING, Dict, List + +from bokeh.io import curdoc +from bokeh.layouts import column, gridplot +from bokeh.models import ColumnDataSource, HoverTool, Legend +from bokeh.models.widgets import CheckboxGroup, Div, Slider +from bokeh.palettes import Dark2_5 as palette +from bokeh.plotting import figure + +from tornado import gen +from stack import RollingStack + +if TYPE_CHECKING: + from imu_sensor import SensorDetails + +@dataclass +class GenericDataclass: + def __str__(self) -> str: + return pformat(self.dict(), indent=4) + + def dict(self): + return {k: str(v) for k, v in asdict(self).items()} + + +@dataclass +class PlotDefaults(GenericDataclass): + """Some Generic defaults for most plots + + Args: + GenericDataclass (Class): adds pretty printouts for debugging + """ + + sensor_details: "SensorDetails" = None + plot_tools: str = "box_zoom,pan,wheel_zoom,reset" + tooltips: List = field( + default_factory=lambda: [ + ("index", "$index"), + ( + "(x,y)", + "(@x, $y)", + ), + ] + ) + + # plot data + plot_title: str = "Sensor Data" + xaxis_label: str = "TS" + yaxis_label: str = "Value" + plot_width: int = 1000 + plot_height: int = 500 + ys_legend_text: Dict = field(default_factory=lambda: {"y": "Fn(x)"}) + + def __post_init__(self): + if self.sensor_details: + self.ys_legend_text = self.sensor_details.legend + self.plot_title = self.sensor_details.title + + +@dataclass +class LayoutDefaults(GenericDataclass): + """Some Generic defaults for parent canvas that contains the plots + + Args: + GenericDataclass (Class): adds pretty printouts for debugging + """ + + delay_queue: RollingStack + + page_title: str = "Real Time Sensor Data" + page_title_colour: str = "white" + page_title_width: int = 1000 + page_title_height: int = 50 + + # how much data to scroll + window_slider_start: int = 1 + window_slider_end: int = 1000 + window_slider_value: int = 250 + window_slider_step: int = 1 + + # how fast to simulate sensor new datapoints + sensor_speed_slider_start: int = 0.005 + sensor_speed_slider_end: int = 0.5 + sensor_speed_slider_value: int = 0.01 + sensor_speed_slider_step: int = 0.01 + + n_columns: int = 2 + + +class BokehPage: + def __init__(self, defaults: LayoutDefaults, sensor_is_reading: Event) -> None: + """Initialse page/canvas + + Args: + defaults (LayoutDefaults): default setup values + """ + self.doc = curdoc() + curdoc().theme = "dark_minimal" + + self.defaults = defaults + self.window_width = self.defaults.window_slider_value + self.start_stop_checkbox = None + self.window_width_slider = None + self.sensor_speed_slider = None + self.all_plots = None + self.plots = None + self.sensor_is_reading = sensor_is_reading + + self.header = Div( + text=f"

{defaults.page_title}

", + width=defaults.page_title_width, + height=defaults.page_title_height, + background="black", + ) + + def add_plots(self, plots: List["BokehPlot"]): + """Add plots to window + + Args: + plots (List[BokehPlot]): list of bokeh plots showing sensor data + """ + self.plots = plots + grid_plot = [] + + for p in plots: + grid_plot.append(p.plt) + + n = self.defaults.n_columns + grid_plot = [grid_plot[i : i + n] for i in range(0, len(grid_plot), n)] + self.all_plots = gridplot( + grid_plot, + ) + self.all_plots.spacing = 10 + self.layout() + + def layout(self): + """Add plots and sliders to layout""" + self.doc.title = self.defaults.page_title + + self.start_stop_checkbox = CheckboxGroup(labels=["Enable Plotting"], active=[0]) + self.start_stop_checkbox.on_change("active", self.start_stop_handler) + + self.window_width_slider = Slider( + start=self.defaults.window_slider_start, + end=self.defaults.window_slider_end, + value=self.defaults.window_slider_value, + step=self.defaults.window_slider_step, + title="window_width", + ) + self.window_width_slider.on_change("value", self.window_width_handler) + + # adjust delay from sensor data updates. Can be removed for real data + self.sensor_speed = Slider( + start=self.defaults.sensor_speed_slider_start, + end=self.defaults.sensor_speed_slider_end, + value=self.defaults.sensor_speed_slider_value, + step=self.defaults.sensor_speed_slider_step, + title="Sensor Update delay", + ) + self.sensor_speed.on_change("value", self.sensor_speed_handler) + + self.hertz_div = Div( + text=f"Each plot is updating at {1/self.defaults.sensor_speed_slider_value:.1f}Hz" + ) + + a = 1 + itms = [ + self.header, + self.start_stop_checkbox, + self.window_width_slider, + self.sensor_speed, + self.hertz_div, + self.all_plots, + ] + for itm in itms: + itm.sizing_mode = "stretch_width" + + layout = column(*itms) + layout.sizing_mode = "stretch_width" + + self.doc.add_root(layout) + + def start_stop_handler(self, attr: str, old: int, new: int): + """Pause plot updates so you can + + Args: + attr (str): only used as a placeholder + old (int): only used as a placeholder + new (int): current checkbox value: 0 off, 1 on + """ + if new: + self.sensor_is_reading.set() + else: + self.sensor_is_reading.clear() + + def window_width_handler(self, attr, old, new): + """Pause plot updates so you can + + Args: + attr (str): only used as a placeholder + old (int): only used as a placeholder + new (int): sets with of rolling window + """ + self.window_width = new + + def sensor_speed_handler(self, attr, old, new): + """Pause plot updates so you can + + Args: + attr (str): only used as a placeholder + old (int): only used as a placeholder + new (int): sets delay between sensor updates + """ + self.hertz_div.text = f"Each plot is updating at {1/new:.1f}Hz" + self.defaults.delay_queue.append(new) + + +class BokehPlot: + def __init__(self, parent: BokehPage, sensor_details: "SensorDetails") -> None: + """Initialise a plot + + Args: + parent (BokehPage): parent that will contain the plot + sensor_details (SensorDetails): sensor signal details + """ + self.parent = parent + self.doc = parent.doc + + self.colours = cycle(palette) + + self.defaults = PlotDefaults(sensor_details) + + self.plot_options = dict( + width=self.defaults.plot_width, + height=self.defaults.plot_height, + tools=[ + HoverTool(tooltips=self.defaults.tooltips), + self.defaults.plot_tools, + ], + ) + + self.source, self.plt = self.definePlot() + + def definePlot(self): + """Automaticaaly define the plot based on the legend data supplied in Main + + Returns: + (source, plt): (source data for sensor, plot data based on sensor data) + """ + plt = figure(**self.plot_options, title=self.defaults.plot_title) + plt.sizing_mode = "scale_width" + plt.xaxis.axis_label = self.defaults.xaxis_label + plt.yaxis.axis_label = self.defaults.yaxis_label + + # if multiple y values (eg y, y1,y2...yn) in plot create a multiline plot + data = {_y: [0] for _y in self.defaults.ys_legend_text.keys()} + data["x"] = [0] + + source = ColumnDataSource(data=data) + + items = [] + + for y, legend_text in self.defaults.ys_legend_text.items(): + colour = next(self.colours) + r1 = plt.line(x="x", y=y, source=source, line_width=1, color=colour) + r1a = plt.circle( + x="x", y=y, source=source, fill_color="white", size=1, color=colour + ) + items.append((legend_text, [r1, r1a])) + + legend = Legend(items=items) + plt.add_layout(legend, "right") + plt.legend.click_policy = "hide" + + return source, plt + + @gen.coroutine + def update(self, new_data: dict): + """update source data from sensor data + + Args: + new_data (dict): newest data + """ + + if self.parent.sensor_is_reading.is_set(): + self.source.stream(new_data, rollover=self.parent.window_width) diff --git a/depthai_sdk/examples/IMUplot/imu_bokeh_stream/imu_sensor.py b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/imu_sensor.py new file mode 100644 index 000000000..98ace22fa --- /dev/null +++ b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/imu_sensor.py @@ -0,0 +1,292 @@ +import time +from dataclasses import dataclass +from enum import Enum, auto +from functools import partial +from threading import Event, Lock, Thread +from time import sleep +from typing import TYPE_CHECKING, Dict, List + +import depthai as dai +from bokeh_plot import BokehPage, BokehPlot, LayoutDefaults +from stack import RollingStack + +if TYPE_CHECKING: + from bokeh_plot import BokehPlot + + +@dataclass +class SensorDetails: + legend: Dict[str, str] + title: str + + delay_q: RollingStack + data_q: RollingStack + + +class SensorTag(Enum): + ACCELEROMETER = auto() + GYROSCOPE = auto() + MAGNETOMETER = auto() + + +class SensorProducer(Thread): + def __init__(self, details: SensorDetails, sensor_is_reading: Event) -> None: + """Init Sensor Producer + + Args: + details (SensorDetails): Details on how to plot sensor vals and queues + to share data between threads + sensor_is_reading (Event): Used to stop start plotting WIP + """ + Thread.__init__(self) + self.details = details + self.sensor_is_reading = sensor_is_reading + + self.start_time = self.current_milli_time() + self.x = self.start_time + + self.data = dict() + self.details.data_q.append(self.data) + + self.pipeline = self.init_oak_reader() + + def init_oak_reader(self): + # Create pipeline + pipeline = dai.Pipeline() + + # Define sources and outputs + imu = pipeline.create(dai.node.IMU) + xlinkOut = pipeline.create(dai.node.XLinkOut) + + xlinkOut.setStreamName("imu") + + # enable ACCELEROMETER_RAW at 500 hz rate + imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER, 500) + # enable GYROSCOPE_RAW at 400 hz rate + imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_CALIBRATED, 400) + # enable MAGNETMOETER_RAW at 400 hz rate + imu.enableIMUSensor(dai.IMUSensor.MAGNETOMETER_CALIBRATED, 400) + # it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections + # above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available + imu.setBatchReportThreshold(1) + # maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it + # if lower or equal to batchReportThreshold then the sending is always blocking on device + # useful to reduce device's CPU load and number of lost packets, if CPU load is high on device side due to multiple nodes + imu.setMaxBatchReports(10) + + # Link plugins IMU -> XLINK + imu.out.link(xlinkOut.input) + + return pipeline + + def run(self): + # Pipeline is defined, now we can connect to the device and get data + with dai.Device(self.pipeline) as device: + + def timeDeltaToMilliS(delta) -> float: + return delta.total_seconds() * 1000 + + # Output queue for imu bulk packets + imuQueue = device.getOutputQueue(name="imu", maxSize=50, blocking=False) + baseTs = None + + self.data = None + + while True: + imuData = ( + imuQueue.get() + ) # blocking call, will wait until a new data has arrived + + imuPackets = imuData.packets + for imuPacket in imuPackets: + acceleroValues = imuPacket.acceleroMeter + gyroValues = imuPacket.gyroscope + magnetValues = imuPacket.magneticField + + acceleroTs = acceleroValues.getTimestampDevice() + gyroTs = gyroValues.getTimestampDevice() + magnetTs = magnetValues.getTimestampDevice() + + if baseTs is None: + baseTs = acceleroTs if acceleroTs < gyroTs else gyroTs + + acceleroTs = timeDeltaToMilliS(acceleroTs - baseTs) + gyroTs = timeDeltaToMilliS(gyroTs - baseTs) + magnetTs = timeDeltaToMilliS(magnetTs - baseTs) + + # x,y,z in frame of reference of horizontal cam + data = dict() + data[SensorTag.ACCELEROMETER] = dict( + x=acceleroTs, + y=acceleroValues.y, + y1=acceleroValues.z, + y2=acceleroValues.x, + ) + + data[SensorTag.GYROSCOPE] = dict( + x=gyroTs, + y=gyroValues.y, + y1=gyroValues.z, + y2=gyroValues.x, + ) + + data[SensorTag.MAGNETOMETER] = dict( + x=magnetTs, + y=magnetValues.y, + y1=magnetValues.z, + y2=magnetValues.x, + ) + + self.details.data_q.append(data) + + def mean(self, vals: List[Dict]) -> Dict: + """Used to smooth data + + Args: + vals (List[Dict]): List of sensor history + + Returns: + Dict: mean values of recorded values + """ + history_len = len(vals) + res = { + SensorTag.ACCELEROMETER: {}, + SensorTag.GYROSCOPE: {}, + SensorTag.MAGNETOMETER: {}, + } + for key in ["x", "y", "y1", "y2"]: + for tag in SensorTag: + mymean = 0 + for i in range(history_len): + mymean += vals[i][tag][key] + res[tag][key] = [mymean / history_len] + + # calculate magnitude + for tag in SensorTag: + res[tag]["y3"] = [ + ( + res[tag]["y"][0] ** 2 + + res[tag]["y1"][0] ** 2 + + res[tag]["y2"][0] ** 2 + ) + ** 0.5 + ] + + return res + + def read(self, sensor_tag: Enum) -> Dict: + """Get latest stored values + + Args: + sensor_tag (Enum): tag for each plot + + Returns: + Dict: mean sensor values in Bokeh format + """ + vals = self.details.data_q.all() + + if vals[0]: + return self.mean(vals)[sensor_tag] + return {} + + def current_milli_time(self, start_time=0): + return round(time.time() * 1000) - start_time + + +class SensorConsumer(Thread): + def __init__( + self, + plt: "BokehPlot", + sensor: SensorProducer, + sensor_is_reading: Event, + sensor_tag: str, + ): + """_summary_ + + Args: + plt (BokehPlot): plot to display the data + sensor (SensorProducer): class that supplies sensor data + sensor_is_reading (Event): is plotting state + sensor_tag (str): identifies plot + """ + Thread.__init__(self) + + self.sensor_tag = sensor_tag + self.sensor = sensor + self.sensor_is_reading = sensor_is_reading + self.threadLock = Lock() + + self.sensor_callback = plt.update + self.bokeh_callback = plt.doc.add_next_tick_callback + + def run(self): + """Generate data""" + while True: + time.sleep(self.sensor.details.delay_q.latest()) + + if self.sensor_is_reading.is_set(): + with self.threadLock: + latest = self.sensor.read(self.sensor_tag) + + if latest: + self.bokeh_callback(partial(self.sensor_callback, latest)) + else: + sleep(1) + + +def init_oak_imu(): + """Create live plots""" + n_plots = 3 + rolling_mean = 1 + sensor_speed_slider_value = 0.005 * n_plots + sensor_is_reading = Event() + sensor_is_reading.set() + + delay_queue = RollingStack(1, sensor_speed_slider_value) + data_q = RollingStack(rolling_mean) + + accel_deets = SensorDetails( + {"y": "Accel(x)", "y1": "Accel(y)", "y2": "Accel(z)", "y3": "Magnitude"}, + "Accelerometer", + delay_queue, + data_q, + ) + + gyro_deets = SensorDetails( + {"y": "Gyro(x)", "y1": "Gyro(y)", "y2": "Gyro(z)", "y3": "Magnitude"}, + "Gyroscope", + delay_queue, + data_q, + ) + + magnet_deets = SensorDetails( + {"y": "Magnet(x)", "y1": "Magnet(y)", "y2": "Magnet(z)", "y3": "Magnitude"}, + "Magnetometer", + delay_queue, + data_q, + ) + + plots = [] + + main_page = BokehPage( + LayoutDefaults( + delay_queue, sensor_speed_slider_value=sensor_speed_slider_value + ), + sensor_is_reading, + ) + + producer = SensorProducer(accel_deets, sensor_is_reading) + producer.start() + + for deets, tag in [ + (magnet_deets, SensorTag.MAGNETOMETER), + (gyro_deets, SensorTag.GYROSCOPE), + (accel_deets, SensorTag.ACCELEROMETER), + ]: + plt = BokehPlot(main_page, deets) + consumer = SensorConsumer(plt, producer, sensor_is_reading, tag) + + plots.append(plt) + consumer.start() + + main_page.add_plots(plots) diff --git a/depthai_sdk/examples/IMUplot/imu_bokeh_stream/main.py b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/main.py new file mode 100644 index 000000000..e764eeceb --- /dev/null +++ b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/main.py @@ -0,0 +1,9 @@ +from imu_sensor import init_oak_imu + +def main(): + init_oak_imu() + + +# Run command: +# bokeh serve --show imu_bokeh_stream +main() diff --git a/depthai_sdk/examples/IMUplot/imu_bokeh_stream/stack.py b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/stack.py new file mode 100644 index 000000000..9a9ea0aab --- /dev/null +++ b/depthai_sdk/examples/IMUplot/imu_bokeh_stream/stack.py @@ -0,0 +1,18 @@ +from collections import deque +from statistics import mean +from threading import Lock + + +class RollingStack(deque): + def __init__(self, stack_size=3, init_val={}) -> None: + deque.__init__(self, maxlen=stack_size) + self.append(init_val) + self.lock = Lock() + + def latest(self): + # not all deque functions are threadsafe + with self.lock: + return self[-1] + + def all(self): + return list(self) diff --git a/depthai_sdk/examples/IMUplot/readme.md b/depthai_sdk/examples/IMUplot/readme.md new file mode 100644 index 000000000..1d93faccd --- /dev/null +++ b/depthai_sdk/examples/IMUplot/readme.md @@ -0,0 +1,36 @@ +## INSTRUCTIONS +### run the command below in a linux terminal +``` sh +bokeh serve imu_bokeh_stream +``` + +### if running in WSL + +To get an OAK running on WSL 2, you first need to attach USB device to WSL 2. + +On the windows side install the following +[usbipd-win 3.2.0 Installer](https://github.com/dorssel/usbipd-win/releases/download/v3.2.0/usbipd-win_3.2.0.msi) + +Inside WSL 2 you also need to run +``` sh +sudo apt install linux-tools-virtual hwdata +sudo update-alternatives --install /usr/local/bin/usbip usbip `ls /usr/lib/linux-tools/*/usbip | tail -n1` 20 +``` + +To attach the OAK camera to WSL 2, run the following code in Python from an admin Powershell +``` python +import time +import os + +while True: + output = os.popen('usbipd wsl list').read() + rows = output.split('\n') + for row in rows: + if ('Movidius MyriadX' in row or 'Luxonis Device' in row) and 'Not attached' in row: + busid = row.split(' ')[0] + out = os.popen(f'usbipd wsl attach --busid {busid}').read() + print(out) + print(f'Usbipd attached Myriad X on bus {busid}') + time.sleep(.5) + +``` diff --git a/depthai_sdk/examples/IMUplot/requirements.txt b/depthai_sdk/examples/IMUplot/requirements.txt new file mode 100644 index 000000000..00178decd --- /dev/null +++ b/depthai_sdk/examples/IMUplot/requirements.txt @@ -0,0 +1,3 @@ +bokeh==3.3.1 +depthai==2.23.0.0 +tornado==6.3.3 From 9de93e7aaa7d6deda4f1d9c687946025ec5532a5 Mon Sep 17 00:00:00 2001 From: hidara2000 <15170494+hidara2000@users.noreply.github.com> Date: Thu, 30 Nov 2023 17:48:35 +1100 Subject: [PATCH 2/2] Update readme.md --- depthai_sdk/examples/IMUplot/readme.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/depthai_sdk/examples/IMUplot/readme.md b/depthai_sdk/examples/IMUplot/readme.md index 1d93faccd..feec806c8 100644 --- a/depthai_sdk/examples/IMUplot/readme.md +++ b/depthai_sdk/examples/IMUplot/readme.md @@ -34,3 +34,10 @@ while True: time.sleep(.5) ``` +The window slider changes the width of the scrolling window ie no of points in view +The delay window can be used to change how many points are plotted per second. This app can comfortably plot at 120Hz + +## Example of the running program +https://github.com/hidara2000/depthai/assets/15170494/b15a974c-2955-4f1d-bc45-a0d702d04b09 + +