[WIP] Delsys example

.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

devices/__init__.py

@@ -0,0 +1,2 @@
+#!/usr/bin/env python3
+# Copyright 2004-present Facebook. All Rights Reserved.

devices/hardware/__init__.py

@@ -0,0 +1,2 @@
+#!/usr/bin/env python3
+# Copyright 2004-present Facebook. All Rights Reserved.

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.

devices/hardware/delsys/delsys/__init__.py

@@ -0,0 +1,2 @@
+#!/usr/bin/env python3
+# Copyright 2004-present Facebook. All Rights Reserved.

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

devices/hardware/delsys/delsys/examples/__init__.py

@@ -0,0 +1,2 @@
+#!/usr/bin/env python3
+# Copyright 2004-present Facebook. All Rights Reserved.

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

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 ===================================

devices/hardware/delsys/delsys/tests/__init__.py

@@ -0,0 +1,2 @@
+#!/usr/bin/env python3
+# Copyright 2004-present Facebook. All Rights Reserved.