//====================================================================================================== // Copyright 2022, NaturalPoint Inc. //====================================================================================================== #pragma once #include "ExampleGloveDevice.h" #include "ExampleGloveAdapterSingleton.h" // OptiTrack Peripheral Device API #include "AnalogChannelDescriptor.h" #include "IDeviceManager.h" using namespace AnalogSystem; using namespace OptiTrackPluginDevices; using namespace GloveDeviceProperties; using namespace ExampleDevice; /////////////////////////////////////////////////////////////////////////////// // // Device Helper: Initialization and Shutdown // void OptiTrackPluginDevices::ExampleDevice::ExampleGlove_EnumerateDeviceFactories(IDeviceManager* pDeviceManager, std::list>& dfs) { // Start server detection if (gGloveAdapter == nullptr) { gGloveAdapter = std::make_unique(pDeviceManager); } } void OptiTrackPluginDevices::ExampleDevice::ExampleGlove_Shutdown() { if (gGloveAdapter != nullptr) { gGloveAdapter->ClientShutdown(); gGloveAdapter.reset(); } } /////////////////////////////////////////////////////////////////////////////// // // Example Glove Device Factory // const char* OptiTrackPluginDevices::ExampleDevice::ExampleGloveDeviceFactory::Name() const { return "ExampleGloveDevice"; } std::unique_ptr OptiTrackPluginDevices::ExampleDevice::ExampleGloveDeviceFactory::Create() const { ExampleGloveDevice* pDevice = new ExampleGloveDevice(mDeviceSerial, mDeviceInfo); SetCommonGloveDeviceProperties(pDevice); SetQuaternionDataChannels(pDevice); // Transfer ownership to host std::unique_ptr ptrDevice(pDevice); return ptrDevice; } /////////////////////////////////////////////////////////////////////////////// // // Example Glove Device // OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::ExampleGloveDevice(uint32_t serial, sGloveDeviceBaseInfo deviceInfo) { mDeviceInfo = deviceInfo; mDeviceSerial = deviceInfo.gloveId; bIsEnabled = true; } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::Configure() { bool success = Deconfigure(); if (!success) return false; // update device's buffer allocation based on current channel and data type configuration success = cPluginDevice::Configure(); if (!success) return false; bIsConfigured = success; DeviceManager()->MessageToHost(this, "", MessageType_RequestRestart); return success; } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::Deconfigure() { bool success = cPluginDevice::Deconfigure(); return success; } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::StartCapture() { bool success = cPluginDevice::StartCapture(); bIsCollecting = true; mCollectionThread = CreateThread(nullptr, 0, CollectionThread, this, 0, nullptr); if (mCollectionThread == nullptr) { return false; bIsCollecting = false; } return success; } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::StopCapture() { bool success = cPluginDevice::StopCapture(); // REQUIRED: Stop collecting data. Terminate hardware device polling thread bIsCollecting = false; DWORD waitResult = WaitForSingleObject(mCollectionThread, 1000); if (waitResult == WAIT_OBJECT_0) { CloseHandle(mCollectionThread); mCollectionThread = NULL; success = true; } else if (waitResult == WAIT_TIMEOUT) { BOOL result = TerminateThread(mCollectionThread, 0); success = (result == TRUE); } else { success = false; } return success; } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::OnPropertyChanged(const char* propertyName) { cPluginDevice::OnPropertyChanged(propertyName); if (strcmp(propertyName, kEnabledPropName) == 0) { // Update device enabled state GetProperty(kEnabledPropName, bIsEnabled); } else if (strcmp(propertyName, kRatePropName) == 0) { // Update device capture rate GetProperty(kRatePropName, mDeviceRateFPS); mRequestedRateMS = (1.0f / mDeviceRateFPS) * 1000.0f; } else if (strcmp(propertyName, kDataStatePropName) == 0) { int deviceState = 0; GetProperty(kDataStatePropName, deviceState); if (deviceState != DeviceDataState::DeviceState_ReceivingData) { // if not receiving data, disable battery state and signal strength mBatteryLevel = GloveDeviceProp_BatteryUninitialized; SetProperty(GloveDeviceProp_Battery, mBatteryLevel); mSignalStrength = GloveDeviceProp_SignalStrengthUnitialized; SetProperty(GloveDeviceProp_SignalStrength, mSignalStrength); } } else if (strcmp(propertyName, GloveDeviceProp_Solver) == 0) { // Route solver type to device user data for interpreting in pipeline int solver = 0; GetProperty(GloveDeviceProp_Solver, solver); SetProperty(cPluginDeviceBase::kUserDataPropName, solver); } } unsigned long __stdcall OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::CollectionThread(LPVOID Context) { ExampleGloveDevice* pThis = static_cast(Context); return pThis->DoCollectionThread(); } unsigned long OptiTrackPluginDevices::ExampleDevice::ExampleGloveDevice::DoCollectionThread() { /* Collection Thread - Motive's 15 channel glove data channel format for finger node rotations: 1 = Thumb MP 1 (w,x,y,z) 2 = Thumb PIP 2 3 = Thumb DIP 3 4 = Index MP 1 5 = Index PIP 2 6 = Index DIP 3 7 = Middle MP 1 8 = Middle PIP 2 9 = Middle DIP 3 10 = Ring MP 1 11 = Ring PIP 2 12 = Ring DIP 3 13 = Pinky MP 1 14 = Pinky PIP 2 15 = Pinky DIP 3 Hand joint orientation respects right-handed coordinate system. For left hand, +X is pointed towards the finger tip. For right hand, +X is pointer towards the wrist or the body. */ // timers used for frame and ui updates std::chrono::high_resolution_clock::time_point frameTimerStart, frameTimerEnd; std::chrono::high_resolution_clock::time_point uiTimerStart, uiTimerEnd; std::chrono::milliseconds actualFrameDurationMS; double adjustment = 0.0; double durationDeltaMS = 0.0; // Glove device parameters (rate / battery / signal) GetProperty(kEnabledPropName, bIsEnabled); GetProperty(GloveDeviceProp_Battery, mBatteryLevel); GetProperty(GloveDeviceProp_SignalStrength, mSignalStrength); GetProperty(kRatePropName, mDeviceRateFPS); mRequestedRateMS = (1.0f / mDeviceRateFPS) * 1000.0f; // Initialize glove handedness InitializeGloveProperty(); //glove channel data arrray to be copied float gloveChannelData[kGloveAnalogChannelCount]; // Collection thread uiTimerStart = std::chrono::high_resolution_clock::now(); while (bIsCollecting) { frameTimerStart = std::chrono::high_resolution_clock::now(); int deviceFrameID = this->FrameCounter(); // Skip disabled devices bool isDeviceEnabled = false; GetProperty(kEnabledPropName, isDeviceEnabled); if (!isDeviceEnabled) continue; // Poll latest glove data from the adapter sGloveDeviceData t_data; bool isGloveDataAvailable = gGloveAdapter->GetLatestData(mDeviceInfo.gloveId, t_data); if (isGloveDataAvailable) { if (!bIsInitialized) { InitializeGloveProperty(); } // Update ui every 5 secs (too frequent can cause unnecessary slowdowns) uiTimerEnd = std::chrono::high_resolution_clock::now(); auto elapsedTime = std::chrono::duration_cast(uiTimerEnd - uiTimerStart); if (elapsedTime.count() > 5) { UpdateGloveProperty(mDeviceInfo); uiTimerStart = std::chrono::high_resolution_clock::now(); } // Frame Begin: get frame from device buffer AnalogFrame* pFrame = this->BeginFrameUpdate(deviceFrameID); if (pFrame) { // fill in frame header int flags = 0; // Iterate through each bone and populate channel data. // Skip the first bone, hand base, as it's driven from rigid body transform instead. for (int i = 0; i < 15; i++) { int nodeChannelIndex = i * 4; gloveChannelData[nodeChannelIndex] = t_data.nodes.at(i).quat_w; //w gloveChannelData[nodeChannelIndex + 1] = t_data.nodes.at(i).quat_x; //x gloveChannelData[nodeChannelIndex + 2] = t_data.nodes.at(i).quat_y; //y gloveChannelData[nodeChannelIndex + 3] = t_data.nodes.at(i).quat_z; //z } pFrame->SetID(deviceFrameID); pFrame->SetFlag(flags); //pFrame->SetTimestamp((double)mLastGloveDataTimebstamp.time); ::memcpy(pFrame->ChannelData(), &gloveChannelData[0], kGloveAnalogChannelCount * sizeof(float)); EndFrameUpdate(); this->SetFrameCounter(deviceFrameID + 1); } // End frame update. Sleep the thread for the remaining frame period. std::this_thread::sleep_for(std::chrono::milliseconds((int)(mRequestedRateMS - adjustment))); frameTimerEnd = std::chrono::high_resolution_clock::now(); actualFrameDurationMS = std::chrono::duration_cast(frameTimerEnd - frameTimerStart); durationDeltaMS = actualFrameDurationMS.count() - mRequestedRateMS; // estimating adjustment to prevent oscillation on sleep duration. if (durationDeltaMS > 1.0) adjustment = -1.0; else if (durationDeltaMS > 2.0) adjustment = -2.0; else if (durationDeltaMS > 3.0) adjustment = -3.0; else if (durationDeltaMS < -3.0) adjustment = -3.0; else if (durationDeltaMS < -2.0) adjustment = 2.0; else if (durationDeltaMS < -1.0) adjustment = 1.0; else adjustment = 0.0; if (fabs(adjustment) > 1.0) { this->LogError(MessageType_StatusInfo, "[Example Device] Device timing resolution off by %3.1f ms (requested:%3.1f, actual:%3.1f)", durationDeltaMS, mRequestedRateMS, (double) actualFrameDurationMS.count()); } } } return 0; }