Streamingle_URP/Optitrack Rokoko Glove/RokokoGloveDevice/HardwareSimulator.cpp

//======================================================================================================
// Copyright 2023, NaturalPoint Inc.
//======================================================================================================
#include "HardwareSimulator.h"
#include <fstream>
#include <iostream>
#include <sstream>
#include <windows.h>

using namespace std;

SimulatedPluginDevices::HardwareSimulator::HardwareSimulator()
{}

SimulatedPluginDevices::HardwareSimulator::~HardwareSimulator()
{
	bIsRunning = false;
	if (mUpdateThread.joinable()) {
		mUpdateThread.join();
	}
}

void SimulatedPluginDevices::HardwareSimulator::StartData()
{
	bIsRunning = true;
	mUpdateThread = std::thread(&HardwareSimulator::ReadDataFromCSV, this);
}

void SimulatedPluginDevices::HardwareSimulator::Shutdown()
{
	bIsRunning = false;
	if (mUpdateThread.joinable())
	{
		mUpdateThread.join();
	}
}

void SimulatedPluginDevices::HardwareSimulator::RegisterDeviceInfoCallback(std::function<void(std::vector<SimulatedDeviceInfo>&)> device_info_callback)
{
	mOnDeviceInfoUpdate = device_info_callback;
}

void SimulatedPluginDevices::HardwareSimulator::NotifyDataCallback()
{
	if (mOnFrameDataUpdate)
	{
		mOnFrameDataUpdate(mSimulatedFrameDataSet);
	}
}

void SimulatedPluginDevices::HardwareSimulator::NotifyInfoCallback()
{
	if (mOnDeviceInfoUpdate)
	{
		mOnDeviceInfoUpdate(mNewDeviceInfo);
		mNewDeviceInfo.clear();
	}
}

void SimulatedPluginDevices::HardwareSimulator::AddSimulatedGlove(int deviceId, int nodeCount, int handedness)
{
	SimulatedDeviceInfo device(deviceId, nodeCount, handedness);
	mSimulatedDeviceInfoSet.push_back(device);
	mNewDeviceInfo.push_back(device);
	mSimulatedFrameDataSet.push_back(SimulatedGloveFrameData(device.mDeviceSerial, device.mNodeCount));
	NotifyInfoCallback();
}

void SimulatedPluginDevices::HardwareSimulator::RegisterFrameDataCallback(std::function<void(std::vector<SimulatedGloveFrameData>&)> data_callback)
{
	mOnFrameDataUpdate = data_callback;
}


/**
 * [Example] Simply read each frame data from the provided csv file and update the data callback.
 */
bool SimulatedPluginDevices::HardwareSimulator::ReadDataFromCSV()
{
	double mRequestedRateMS = (1.0f / mDataSampleRate) * 1000.0f;
	std::string filename = GetExePath() + "\\devices\\ExampleGloveData.csv";

	// Read the provided example CSV file (60 channels)
	std::ifstream fin(filename);
	if (!fin.is_open())
	{
		// Failed to open the device
		return false;
	}

	bool isFirstLine = true;
	std::vector<string> field_names;

	// loop through lines of data
	std::string lineRead;
	while (bIsRunning)
	{
		std::vector<SimulatedFingerData> frame_data;
		std::vector<float> jointData;

		if (fin.eof())
		{
			//loop back to begining
			fin.clear();
			fin.seekg(0, std::ios::beg);
			isFirstLine = true;
		}

		std::getline(fin, lineRead);
		std::stringstream lineStream(lineRead);

		// Read comma-separated values
		std::string val;
		std::getline(lineStream, val, ','); // skip first column
		while (getline(lineStream, val, ','))
		{
			if (isFirstLine)
			{
				field_names.push_back(val);
			}
			else
			{
				try
				{
					float val_f = std::stof(val);
					jointData.push_back(val_f);

				}
				catch (const std::exception* e)
				{
					// error converting the value. Terminate
					return false;
				}

				if (jointData.size() == 4)
				{
					SimulatedFingerData finger;
					// next finger joint
					finger.quat_w = jointData.at(0);
					finger.quat_x = jointData.at(1);
					finger.quat_y = jointData.at(2);
					finger.quat_z = jointData.at(3);
					frame_data.push_back(finger);
					jointData.clear();
				}
			}
		}

		if (isFirstLine)
			isFirstLine = false;
		else {
			if (frame_data.size() != 0)
			{
				UpdateAllDevicesWithData(frame_data);
			}
		}

		// End of a line sleep
		std::this_thread::sleep_for(std::chrono::milliseconds((int)mRequestedRateMS));
	}
	return true;
}

void SimulatedPluginDevices::HardwareSimulator::UpdateAllDevicesWithData(std::vector<SimulatedFingerData>& data)
{
	mDataLock.lock();
	// Loop through list of data instances in the frame data vector and update all instances
	for (auto& gloveDevice : mSimulatedFrameDataSet)
	{
		// Set all finger data 
		gloveDevice.gloveFingerData = data;
	}
	NotifyDataCallback();
	mDataLock.unlock();
}

std::string SimulatedPluginDevices::HardwareSimulator::GetExePath()
{
	std::string path;
	char buffer[MAX_PATH];
	GetModuleFileNameA(NULL, buffer, MAX_PATH);
	string::size_type pos = string(buffer).find_last_of("\\/");
	path = string(buffer).substr(0, pos)/*+"\\system.exe"*/;
	return path;
}