diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..80c5cd199 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "devices/hardware/delsys/delsys/delsysAPI"] + path = devices/hardware/delsys/delsys/delsysAPI + url = https://github.com/delsys-inc/Delsys-Python-Demo.git diff --git a/devices/__init__.py b/devices/__init__.py new file mode 100644 index 000000000..860ac27c6 --- /dev/null +++ b/devices/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. diff --git a/devices/hardware/__init__.py b/devices/hardware/__init__.py new file mode 100644 index 000000000..860ac27c6 --- /dev/null +++ b/devices/hardware/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. diff --git a/devices/hardware/delsys/README.md b/devices/hardware/delsys/README.md new file mode 100644 index 000000000..b73990d70 --- /dev/null +++ b/devices/hardware/delsys/README.md @@ -0,0 +1,7 @@ +# LabGraph node for Delsys + +Install python3 - tested with Python 3.6.15 + +Go to /devices/hardware/delsys/delsys/delsys.py and copy paste key/license (be sure to wrap in quatations) + +Make sure base station is plugged in and has power. \ No newline at end of file diff --git a/devices/hardware/delsys/delsys/__init__.py b/devices/hardware/delsys/delsys/__init__.py new file mode 100644 index 000000000..860ac27c6 --- /dev/null +++ b/devices/hardware/delsys/delsys/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. diff --git a/devices/hardware/delsys/delsys/delsys.py b/devices/hardware/delsys/delsys/delsys.py new file mode 100644 index 000000000..2290b04c9 --- /dev/null +++ b/devices/hardware/delsys/delsys/delsys.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +# # -*- coding: utf-8 -*- +""" +This class creates an instance of the Trigno base. Put in your key and license here +""" +import clr +import numpy as np +from collections import deque + +clr.AddReference("devices/hardware/delsys/delsys/delsysAPI/resources/DelsysAPI") +clr.AddReference("System.Collections") + +from Aero import AeroPy # type: ignore # noqa: E402 +from System.Collections.Generic import List # type: ignore # noqa: E402 +from System import Int32 # type: ignore # noqa: E402 + + +class Delsys(): + + def __init__(self): + + self.key = "" + + self.license = "" + + self.TrigBase = AeroPy() + + self.packetCount = 0 + self.sampleCount = 0 + self.EMGBuffer = 0 + self.emg_plot = deque() + self.isSetupDone = False + self.isInDebugMode = False + self.basisForDebugging = np.ones((8, 3)) + self.realDataStreamID = 0 + + def connect(self) -> None: + """ + Connect to the base + """ + self.TrigBase.ValidateBase(self.key, self.license, "RF") + + def pair(self) -> None: + """ + Enter pair mode for new sensors + """ + self.TrigBase.PairSensors() + + def scan(self) -> None: + """ + Scan for any available sensors + """ + self.TrigBase.ScanSensors().Result + self.nameList = self.TrigBase.ListSensorNames() + self.SensorsFound = len(self.nameList) + + self.TrigBase.ConnectSensors() + self.isSetupDone = True + + def start(self) -> None: + """ + Start the data stream from Sensors + """ + newTransform = self.TrigBase.CreateTransform("raw") + index = List[Int32]() + + self.TrigBase.ClearSensorList() + + for i in range(self.SensorsFound): + selectedSensor = self.TrigBase.GetSensorObject(i) + self.TrigBase.AddSensortoList(selectedSensor) + index.Add(i) + + self.sampleRates = [[] for i in range(self.SensorsFound)] + self.TrigBase.StreamData(index, newTransform, 1) + + self.dataStreamIdx = [] + # self.plotCount = 0 + idxVal = 0 + for i in range(self.SensorsFound): + selectedSensor = self.TrigBase.GetSensorObject(i) + for channel in range(len(selectedSensor.TrignoChannels)): + self.sampleRates[i].append((selectedSensor.TrignoChannels[channel].SampleRate, selectedSensor.TrignoChannels[channel].Name)) + if "EMG" in selectedSensor.TrignoChannels[channel].Name: + self.dataStreamIdx.append(idxVal) + # self.plotCount+=1 + idxVal += 1 + + def stop(self) -> None: + """ + Stop the data stream + """ + self.TrigBase.StopData() + + # Helper Functions + def getSampleModes(self, sensorIdx): + """ + Gets the list of sample modes available for selected sensor + """ + sampleModes = self.TrigBase.ListSensorModes(sensorIdx) + return sampleModes + + def getCurMode(self): + """ + Gets the current mode of the sensors + """ + curMode = self.TrigBase.GetSampleMode() + return curMode + + def setSampleMode(self, curSensor, setMode): + """ + Sets the sample mode for the selected sensor + """ + self.TrigBase.SetSampleMode(curSensor, setMode) + + def getEMGData(self): + + outArr = self.GetData() + if outArr is not None: + outData = list(np.asarray(outArr)[tuple([self.dataStreamIdx])]) + new = np.asarray(outData) + self.EMGBuffer = new + return self.EMGBuffer + else: + return None + + def GetData(self): + """ + Callback to get the data from the streaming sensors + """ + dataReady = self.TrigBase.CheckDataQueue() + if dataReady: + DataOut = self.TrigBase.PollData() + if len(DataOut) > 0: # Check for lost Packets, len(DataOut) = #Channels(8) * #Sensor(EMG+ACC_X_Y_Z = 4) = 32 + outArr = [[] for i in range(len(DataOut))] + for j in range(len(DataOut)): + if len(DataOut[j]) > 1: + print('Packet accumulation!!!') + for k in range(len(DataOut[j])): + outBuf = DataOut[j][k] + outArr[j].extend(outBuf) + return outArr + else: + return None + else: + return None + +# region Helpers + def getPacketCount(self): + return self.packetCount + + def resetPacketCount(self): + self.packetCount = 0 + self.sampleCount = 0 + + def getSampleCount(self): + return self.sampleCount diff --git a/devices/hardware/delsys/delsys/delsysAPI b/devices/hardware/delsys/delsys/delsysAPI new file mode 160000 index 000000000..35f45062f --- /dev/null +++ b/devices/hardware/delsys/delsys/delsysAPI @@ -0,0 +1 @@ +Subproject commit 35f45062f00fa62cc19f838c713e1bf39bb4c8fd diff --git a/devices/hardware/delsys/delsys/examples/__init__.py b/devices/hardware/delsys/delsys/examples/__init__.py new file mode 100644 index 000000000..860ac27c6 --- /dev/null +++ b/devices/hardware/delsys/delsys/examples/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. diff --git a/devices/hardware/delsys/delsys/helpers/Rate.py b/devices/hardware/delsys/delsys/helpers/Rate.py new file mode 100644 index 000000000..55151ed5a --- /dev/null +++ b/devices/hardware/delsys/delsys/helpers/Rate.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. +# -*- coding: utf-8 -*- + +# Inspired by ROS Rate feature and this is a modified version of script below. +# https://github.com/ros/ros_comm/blob/noetic-devel/clients/rospy/src/rospy/timer.py + + +# Built-in imports +import time +import asyncio + + +class Rate(object): + """ + Convenience class for sleeping in a loop at a specified rate + """ + def __init__(self, hz: float): + """ + Constructor. + @param hz: hz rate to determine sleeping + @type hz: float + """ + self.last_time = time.time() + self.sleep_dur = 1.0 / hz + + def _remaining(self, curr_time: float): + """ + Calculate the time remaining for rate to sleep. + @param curr_time: current time + @type curr_time: float + @return: time remaining + @rtype: float + """ + # detect time jumping backwards + if self.last_time > curr_time: + self.last_time = curr_time + + # calculate remaining time + elapsed = curr_time - self.last_time + return self.sleep_dur - elapsed + + def remaining(self): + """ + Return the time remaining for rate to sleep. + @return: time remaining + @rtype: float + """ + curr_time = time.time() + return self._remaining(curr_time) + + async def sleep(self): + + curr_time = time.time() + timeRemaining = self._remaining(curr_time) + + if timeRemaining > 0.0: + await asyncio.sleep(timeRemaining) + else: + await asyncio.sleep(0) + + self.last_time = self.last_time + self.sleep_dur + + if curr_time - self.last_time > self.sleep_dur * 2: + print("Time jumping forward detected") + self.last_time = curr_time diff --git a/devices/hardware/delsys/delsys/nodeDelsys.py b/devices/hardware/delsys/delsys/nodeDelsys.py new file mode 100644 index 000000000..aec68ad94 --- /dev/null +++ b/devices/hardware/delsys/delsys/nodeDelsys.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +# # -*- coding: utf-8 -*- +import time +import asyncio +import numpy as np +import labgraph as lg + +from .helpers.Rate import Rate +from .delsys import Delsys + + +# The messages for the Delsys data +class messageEMG(lg.Message): + timestamps: np.ndarray + data: np.ndarray + + +# The messages to chage the Delsys state +class messageDelsysState(lg.Message): + timestamp: float + setConnectState: bool + setPairState: bool + setScanState: bool + setStartState: bool + setStopState: bool + + +# The configuration for the Delsys +class configDelsys(lg.Config): + sample_rate: float # Rate at which to get data + + +# The state for the Delsys +class stateDelsys(lg.State): + Connect: bool = False + Pair: bool = False + Scan: bool = False + Start: bool = False + Stop: bool = False + Ready: bool = False + + +# ================================= DELSYS DATA PUBLISHER ==================================== +class nodeDelsys(lg.Node): + INPUT = lg.Topic(messageDelsysState) + OUTPUT = lg.Topic(messageEMG) + config: configDelsys + state: stateDelsys + + def setup(self) -> None: + self.Delsys = Delsys() + self.rate = Rate(self.config.sample_rate) + self.shutdown = False + + def cleanup(self) -> None: + self.shutdown = True + + def setDelsysState(self) -> None: + if self.state.Connect: + self.Delsys.connect() + self.state.Connect = False + elif self.state.Pair: + self.Delsys.pair() + self.state.Pair = False + elif self.state.Scan: + self.Delsys.scan() + self.state.Scan = False + elif self.state.Start: + self.Delsys.start() + self.state.Start = False + self.state.Ready = True + elif self.state.Stop: + self.Delsys.stop() + self.state.Stop = False + self.state.Ready = False + + # A subscriber method that simply receives data and updates the node's state + @lg.subscriber(INPUT) + async def getDelsysState(self, message: messageDelsysState) -> None: + self.state.Connect = message.setConnectState + self.state.Pair = message.setConnectState + self.state.Scan = message.setScanState + self.state.Start = message.setStartState + self.state.Stop = message.setStopState + self.setDelsysState() + + # A publisher method that produces data on a single topic + @lg.publisher(OUTPUT) + async def publishEMG(self) -> lg.AsyncPublisher: + + isFirstData = True + while not self.shutdown: + if self.state.Ready: + EMGData = self.Delsys.getEMGData() + if isFirstData and (EMGData is not None): + isFirstData = False + self.rate.last_time = time.time() + if EMGData is not None: + timestampArray = np.linspace(self.rate.last_time, self.rate.last_time + self.rate.sleep_dur, num=EMGData.shape[1], endpoint=False) + yield self.OUTPUT, messageEMG(timestamps=timestampArray, data=EMGData) + await self.rate.sleep() + await asyncio.sleep(0) +# ================================= DELSYS DATA PUBLISHER =================================== diff --git a/devices/hardware/delsys/delsys/tests/__init__.py b/devices/hardware/delsys/delsys/tests/__init__.py new file mode 100644 index 000000000..860ac27c6 --- /dev/null +++ b/devices/hardware/delsys/delsys/tests/__init__.py @@ -0,0 +1,2 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. diff --git a/devices/hardware/delsys/delsys/tests/test_nodeDelsys.py b/devices/hardware/delsys/delsys/tests/test_nodeDelsys.py new file mode 100644 index 000000000..3900f6568 --- /dev/null +++ b/devices/hardware/delsys/delsys/tests/test_nodeDelsys.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +# # -*- coding: utf-8 -*- +import dataclasses +import pytest +import labgraph as lg +from ..nodeDelsys import configDelsys, nodeDelsys, stateDelsys, messageDelsysState + +DATA_SHAPE = (8, 26) +SAMPLE_RATE = 1926.0 +MAX_NUM_RESULTS = 10 + + +def test_get_node() -> None: + """ + Test NodeTestHarness.get_node() + """ + harness = lg.NodeTestHarness(nodeDelsys) + with harness.get_node(config=configDelsys(sample_rate=SAMPLE_RATE), + state=stateDelsys(Connect=False, + Pair=False, + Scan=False, + Start=False, + Stop=False, + Ready=False)) as node: + # Ensure node is of correct type + assert isinstance(node, nodeDelsys) + # Check the node has its config set + assert node.config.asdict() == {"sample_rate": SAMPLE_RATE} + # Check the node has its state set + assert dataclasses.asdict(node.state) == {"Connect": False, + "Pair": False, + "Scan": False, + "Start": False, + "Stop": False, + "Ready": False} + + +def test_run_async_max_num_results() -> None: + """ + Test passing max_num_results to run_async + """ + harness = lg.NodeTestHarness(nodeDelsys) + with harness.get_node(config=configDelsys(sample_rate=SAMPLE_RATE), + state=stateDelsys(Connect=False, + Pair=False, + Scan=False, + Start=False, + Stop=False, + Ready=False)) as node: + + lg.run_async(node.getDelsysState, args=[messageDelsysState(timestamp=1.0, + setConnectState=False, + setPairState=False, + setScanState=False, + setStartState=False, + setStopState=False)]) + assert node.state.Connect is False + assert node.state.Pair is False + assert node.state.Scan is False + assert node.state.Start is False + assert node.state.Stop is False + assert node.state.Ready is False diff --git a/devices/hardware/delsys/setup.py b/devices/hardware/delsys/setup.py new file mode 100644 index 000000000..ab2f98536 --- /dev/null +++ b/devices/hardware/delsys/setup.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python3 +# Copyright 2004-present Facebook. All Rights Reserved. + +from setuptools import find_packages, setup + + +setup( + name="labgraph_delsys", + version="1.0.0", + description="Node for Delsys EMG sensor system in labgraph", + packages=find_packages(), + python_requires=">=3.6", + install_requires=[ + "labgraph>=1.0.2", + "pythonnet", + ], +)