Streamingle_URP/Optitrack Rokoko Glove/GloveDeviceExample/ExampleGloveAdapterSingleton.cpp

//======================================================================================================
// Copyright 2023, NaturalPoint Inc.
//======================================================================================================

#include <functional>
#include <chrono>
#include <windows.h>

// Peripheral Import
#include "ExampleGloveAdapterSingleton.h"
#include "IDeviceManager.h"
#include "ExampleGloveDevice.h"
using namespace AnalogSystem;

// Simulated Device
#define SIM_DEVICE_COUNT 2
#define SIM_DEVICE_RATE 100


OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ExampleGloveAdapterSingleton(AnalogSystem::IDeviceManager* pDeviceManager)
{
    s_Instance = this;
	mDeviceManager = pDeviceManager;
	mGloveSimulator = new SimulatedPluginDevices::HardwareSimulator();
	mGloveDataMutex = new std::recursive_mutex();
	mDetectedDevices.clear();
	mLatestGloveData.clear();

	// start detection thread
	mDetectionThread = std::thread(&ExampleGloveAdapterSingleton::DoDetectionThread, this);
}


OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::~ExampleGloveAdapterSingleton()
{ 
	mDetectedDevices.clear();
	mLatestGloveData.clear();
	delete mGloveDataMutex;
	bIsConnected = false;
	bIsDetecting = false;
	s_Instance = nullptr;

	if (mDetectionThread.joinable())
	{
		mDetectionThread.join();
	}

	mGloveSimulator->Shutdown();
	delete mGloveSimulator;
}


bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ClientShutdown()
{
	bIsConnected = false;
	bIsDetecting = false;

	if (mGloveSimulator != nullptr)
	{
		mGloveSimulator->Shutdown();
	}
	return false;
}

///////////////////////////////////////////////////////////////////////////////
// 
// Glove Host Detection Thread
//
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::DoDetectionThread()
{
	while (!bIsConnected && bIsDetecting)
	{
		// Try reconnecting
		if (s_Instance->mConnectionAttemptCount < s_Instance->kMaxConnectionAttempt) {
			bIsConnected = ConnectToHost();
			s_Instance->mConnectionAttemptCount++;
		}
		else {
			bIsDetecting = false;
			NotifyConnectionFail();
		}
		std::this_thread::sleep_for(std::chrono::milliseconds(20000));
	}
}


///////////////////////////////////////////////////////////////////////////////
// 
// SDK Helper Functions
//
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConnectToHost()
{
	// Check if glove server address is defined in Motive's settings.
	if ((mServerAddress.empty())) {
		char* szServerAddress = new char[MAX_PATH];
		if (mDeviceManager->GetProperty("Glove Server Address", &szServerAddress))
		{
			std::string str(szServerAddress);
			mServerAddress = str;
		}
		delete[] szServerAddress;
	}

	/*
	* [Glove SDK Placeholder]
	*	SDK call to connect to the host at 'mServerAddress'.
	*/
	bIsConnected = true;

	
	StartSimulatedHardware(1);
	RegisterSDKCallbacks();

    return true;
}


bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::IsConnected()
{
	return bIsConnected;
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::SetLatestData(const sGloveDeviceData& gloveFingerData)
{
	s_Instance->mLatestGloveData[gloveFingerData.gloveId] = gloveFingerData;
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::SetDeviceManager(AnalogSystem::IDeviceManager* pDeviceManager)
{
	if (s_Instance == nullptr) return;
	if (pDeviceManager != nullptr)
	{
		s_Instance->mDeviceManager = pDeviceManager;
	}
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::SetLatestDeviceInfo(const sGloveDeviceBaseInfo& deviceInfo)
{
	s_Instance->mLatestDeviceInfo[deviceInfo.gloveId] = deviceInfo;
}

bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::GetLatestData(const uint64_t mDeviceSerial, sGloveDeviceData& gloveFingerData)
{
	if (s_Instance == nullptr || !s_Instance -> bIsConnected) return false;

	bool res = false;
	if (s_Instance->mGloveDataMutex->try_lock()) {
		// Iterate through the glove data table and update
		auto iter = s_Instance->mLatestGloveData.find(mDeviceSerial);
		if (iter != s_Instance->mLatestGloveData.end())
		{
			gloveFingerData = iter->second;
			res = true;
		}
		s_Instance->mGloveDataMutex->unlock();
	}
	return res;
}


void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CreateNewGloveDevice(sGloveDeviceBaseInfo& deviceInfo)
{
	uint64_t gloveId = deviceInfo.gloveId;
	std::string deviceName = "ExampleGlove_" + std::to_string(deviceInfo.gloveId);

	// Create device factory using the name/id/serial/client.
	std::unique_ptr<ExampleGloveDeviceFactory> pDF =
		std::make_unique<ExampleGloveDeviceFactory>(deviceName, deviceInfo);
	pDF->mDeviceIndex = s_Instance->mCurrentDeviceIndex;
	s_Instance->mDeviceManager->AddDevice(std::move(pDF));
	s_Instance->mCurrentDeviceIndex++;

	return;
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::NotifyConnectionFail()
{
	char* szServerAddress = new char[MAX_PATH];
	char szMessage[MAX_PATH];
	if (s_Instance->mDeviceManager->GetProperty("Glove Server Address", &szServerAddress)) {
		if (!(strlen(szServerAddress) == 0))
		{
			sprintf_s(szMessage, "[ExampleGlove] Glove host not found on %s", szServerAddress);
			s_Instance->mDeviceManager->MessageToHost(szMessage, MessageType_StatusInfo);
		}
		else
		{
			sprintf_s(szMessage, "[ExampleGlove] Glove host not found on localhost");
			s_Instance->mDeviceManager->MessageToHost(szMessage, MessageType_StatusInfo);
		}
	}
	delete[] szServerAddress;
}

///////////////////////////////////////////////////////////////////////////////
// 
// [Glove SDK Placeholder] SDK callbacks
//
/*
* Methods in this section starts the device SDK and registers the callbacks.
*/
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::StartSimulatedHardware(int deviceCount)
{
	mGloveSimulator->StartData();
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::RegisterSDKCallbacks()
{
	s_Instance->mGloveSimulator->RegisterFrameDataCallback(OnDataCallback);
	s_Instance->mGloveSimulator->RegisterDeviceInfoCallback(OnDeviceInfoCallback);

	// Add simulated glove devices
	s_Instance->mGloveSimulator->AddSimulatedGlove(1, 15, 1);	// Create left glove
	s_Instance->mGloveSimulator->AddSimulatedGlove(2, 15, 2);	// Create right glove
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::OnDataCallback(std::vector<SimulatedPluginDevices::SimulatedGloveFrameData>& gloveAllFingerData)
{
	if (s_Instance == nullptr) return;

	/*
	* [Glove SDK Placeholder]
	*	Data update callbacks to Keep the latest glove data map up-to-date.
	*/
	s_Instance->mGloveDataMutex->lock();
	for (auto gloveFingerData : gloveAllFingerData) {
		// Convert simulated data into glove data format.
		sGloveDeviceData newGloveData = ConvertDataFormat(gloveFingerData);
		s_Instance->SetLatestData(newGloveData);
	}
	s_Instance->mGloveDataMutex->unlock();
}

void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::OnDeviceInfoCallback(std::vector<SimulatedPluginDevices::SimulatedDeviceInfo>& newGloveInfo)
{
	if (s_Instance == nullptr) return;

	// Create new gloves
	for (SimulatedPluginDevices::SimulatedDeviceInfo glove : newGloveInfo)
	{
		s_Instance->mDeviceManager->MessageToHost(" [ExampleGloveDevice] New device(s) detected.", MessageType_StatusInfo);
		sGloveDeviceBaseInfo newGloveDevice = ConvertDeviceInfoFormat(glove);
		s_Instance->CreateNewGloveDevice(newGloveDevice);
	}
}

sGloveDeviceBaseInfo OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertDeviceInfoFormat(SimulatedPluginDevices::SimulatedDeviceInfo& glove)
{
	sGloveDeviceBaseInfo ret;

	ret.battery = glove.mBattery;
	ret.gloveId = glove.mDeviceSerial;
	ret.signalStrength = glove.mSignalStrength;
	ret.handSide = (glove.mHandSide == 1) ? eGloveHandSide::Left :
		(glove.mHandSide == 2) ? eGloveHandSide::Right :
		eGloveHandSide::Unknown;

	return ret;
}

sGloveDeviceData OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertDataFormat(const SimulatedPluginDevices::SimulatedGloveFrameData& gloveFrameData)
{
	sGloveDeviceData ret;
	ret.gloveId = gloveFrameData.mDeviceSerial;
	ret.timestamp = 0;

	sFingerNode defaultNodes = { 0, 0, 0, 1 };
	ret.nodes = std::vector<sFingerNode>(15, defaultNodes);

	int node_iter = 0;
	for (auto& fingerNode : ret.nodes)
	{
		fingerNode.node_id = node_iter;
		fingerNode.quat_w = gloveFrameData.gloveFingerData[node_iter].quat_w;
		fingerNode.quat_x = gloveFrameData.gloveFingerData[node_iter].quat_x;
		fingerNode.quat_y = gloveFrameData.gloveFingerData[node_iter].quat_y;
		fingerNode.quat_z = gloveFrameData.gloveFingerData[node_iter].quat_z;
		node_iter++;
	}

	return ret;
}