312 lines
9.6 KiB
C++
312 lines
9.6 KiB
C++
//======================================================================================================
|
|
// 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<std::unique_ptr<IDeviceFactory>>& dfs)
|
|
{
|
|
// Start server detection
|
|
if (gGloveAdapter == nullptr) {
|
|
gGloveAdapter = std::make_unique<ExampleGloveAdapterSingleton>(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<AnalogSystem::IDevice> OptiTrackPluginDevices::ExampleDevice::ExampleGloveDeviceFactory::Create() const
|
|
{
|
|
ExampleGloveDevice* pDevice = new ExampleGloveDevice(mDeviceSerial, mDeviceInfo);
|
|
SetCommonGloveDeviceProperties(pDevice);
|
|
SetQuaternionDataChannels(pDevice);
|
|
|
|
// Transfer ownership to host
|
|
std::unique_ptr<AnalogSystem::IDevice> 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<ExampleGloveDevice*>(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<std::chrono::seconds>(uiTimerEnd - uiTimerStart);
|
|
if (elapsedTime.count() > 5)
|
|
{
|
|
UpdateGloveProperty(mDeviceInfo);
|
|
uiTimerStart = std::chrono::high_resolution_clock::now();
|
|
}
|
|
|
|
// Frame Begin: get frame from device buffer
|
|
AnalogFrame<float>* 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<std::chrono::milliseconds>(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;
|
|
}
|