好记性不如烂笔头，之前进行了源码的简单分析并尝试添加了joystick这类包含analog以及button类型数据的设备。这次我们更近一步，尝试添加最为复杂的追踪设备。本次添加的设备为optitrack，由于需要用到人体追踪的数据，所以使用了提供的Natnet SDK进行数据接入，接入的数据包含一般刚体与人体骨骼数据两种类别（SDK也支持手柄等数据的接入，但是目前还没有需求，就没有加，以后用到再加）。

1.获取设备数据

整体流程跟joystick的一样，首先是用自己的程序获取数据，下面贴出了Natnet SDK给的Sample，代码我简单改了一下去掉了些没必要的内容，对于拿数据没什么影响，分析见注释，英文注释比较全了，中文我就是点出了其中比较重要的几块内容。

// ScreenShot.cpp : This file contains the 'main' function. Program execution begins and ends there.
//#include <iostream>
#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>#ifdef _WIN32
#   include <conio.h>
#else
#   include <unistd.h>
#   include <termios.h>
#endif#include <vector>#include <NatNetTypes.h>
#include <NatNetCAPI.h>
#include <NatNetClient.h>using namespace std;int IdOffset = 65535;//用于修正骨骼的ID值,natnet使用一个32位的int存储骨骼的id，其中高16位代表骨架编号，低16位代表骨架中骨骼的编号，高位没什么用，所以通过与操作把低位取出来
NatNetClient* g_pClient = NULL;
int g_analogSamplesPerMocapFrame = 0;
sServerDescription g_serverDescription;
std::vector< sNatNetDiscoveredServer > g_discoveredServers;
sNatNetClientConnectParams g_connectParams;
static const ConnectionType kDefaultConnectionType = ConnectionType_Multicast;
char g_discoveredMulticastGroupAddr[kNatNetIpv4AddrStrLenMax] = NATNET_DEFAULT_MULTICAST_ADDRESS;
void NATNET_CALLCONV ServerDiscoveredCallback(const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext); //try to discover server
void NATNET_CALLCONV MessageHandler(Verbosity msgType, const char* msg);      // receives NatNet error messages
void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData);    // receives data from the server
int ConnectClient();
void resetClient();int main(int argc, char* argv[])
{/*Try to find the Server*/// print version infounsigned char ver[4];NatNet_GetVersion(ver);printf("NatNet Sample Client (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1], ver[2], ver[3]);// Install logging callbackNatNet_SetLogCallback(MessageHandler);// create NatNet clientg_pClient = new NatNetClient();// set the frame callback handler 绑定一个Callback函数，在运行过程中，这个函数会自己启一个线程去处理，独立于主线程之外。g_pClient->SetFrameReceivedCallback(DataHandler, g_pClient); // this function will receive data from the serverprintf("Looking for servers on the local network.\n");// try to find the serverNatNetDiscoveryHandle discovery;NatNet_CreateAsyncServerDiscovery(&discovery, ServerDiscoveredCallback);// build the connection parametersunsigned int serverIndex = 0;//Make sure at least one server is foundwhile (g_discoveredServers.size()<1){std::cout << "No server found"<<std::endl;}/*Try to connect to the server*/const sNatNetDiscoveredServer& discoveredServer = g_discoveredServers[serverIndex];if (discoveredServer.serverDescription.bConnectionInfoValid){// Build the connection parameters.
#ifdef _WIN32_snprintf_s(
#elsesnprintf(
#endifg_discoveredMulticastGroupAddr, sizeof g_discoveredMulticastGroupAddr,"%" PRIu8 ".%" PRIu8".%" PRIu8".%" PRIu8"",discoveredServer.serverDescription.ConnectionMulticastAddress[0],discoveredServer.serverDescription.ConnectionMulticastAddress[1],discoveredServer.serverDescription.ConnectionMulticastAddress[2],discoveredServer.serverDescription.ConnectionMulticastAddress[3]);g_connectParams.connectionType = discoveredServer.serverDescription.ConnectionMulticast ? ConnectionType_Multicast : ConnectionType_Unicast;g_connectParams.serverCommandPort = discoveredServer.serverCommandPort;g_connectParams.serverDataPort = discoveredServer.serverDescription.ConnectionDataPort;g_connectParams.serverAddress = discoveredServer.serverAddress;g_connectParams.localAddress = discoveredServer.localAddress;g_connectParams.multicastAddress = g_discoveredMulticastGroupAddr;}else{// We're missing some info because it's a legacy server.// Guess the defaults and make a best effort attempt to connect.g_connectParams.connectionType = kDefaultConnectionType;g_connectParams.serverCommandPort = discoveredServer.serverCommandPort;g_connectParams.serverDataPort = 0;g_connectParams.serverAddress = discoveredServer.serverAddress;g_connectParams.localAddress = discoveredServer.localAddress;g_connectParams.multicastAddress = NULL;}NatNet_FreeAsyncServerDiscovery(discovery);  int iResult;//Connect to MotiveiResult = ConnectClient();if (iResult != ErrorCode_OK){printf("Error initializing client.  See log for details.  Exiting");return 1;}else{printf("Client initialized and ready.\n");}// Send/receive Context 这部分内容就是尝试接受一些场景内的环境信息，比如有几个刚体，几个人体，人体有几个骨骼，骨骼间的位置关系是啥这种，可用可不用void* response;int nBytes;printf("[SampleClient] Sending Test Request\n");iResult = g_pClient->SendMessageAndWait("TestRequest", &response, &nBytes);if (iResult == ErrorCode_OK){printf("[SampleClient] Received: %s", (char*)response);}// Retrieve Data Descriptions from Motiveprintf("\n\n[SampleClient] Requesting Data Descriptions...");sDataDescriptions* pDataDefs = NULL;iResult = g_pClient->GetDataDescriptionList(&pDataDefs);if (iResult != ErrorCode_OK || pDataDefs == NULL){printf("[SampleClient] Unable to retrieve Data Descriptions.");return -2;}else{printf("[SampleClient] Received %d Data Descriptions:\n", pDataDefs->nDataDescriptions);for (int i = 0; i < pDataDefs->nDataDescriptions; i++){printf("Data Description # %d (type=%d)\n", i, pDataDefs->arrDataDescriptions[i].type);if (pDataDefs->arrDataDescriptions[i].type == Descriptor_MarkerSet){// MarkerSetsMarkerSetDescription* pMS = pDataDefs->arrDataDescriptions[i].Data.MarkerSetDescription;printf("MarkerSet Name : %s\n", pMS->szName);for (int i = 0; i < pMS->nMarkers; i++)printf("%s\n", pMS->szMarkerNames[i]);}else if (pDataDefs->arrDataDescriptions[i].type == Descriptor_RigidBody){// RigidBodysRigidBodyDescription* pRB = pDataDefs->arrDataDescriptions[i].Data.RigidBodyDescription;printf("RigidBody Name : %s\n", pRB->szName);printf("RigidBody ID : %d\n", pRB->ID);printf("RigidBody Parent ID : %d\n", pRB->parentID);printf("Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx, pRB->offsety, pRB->offsetz);if (pRB->MarkerPositions != NULL && pRB->MarkerRequiredLabels != NULL){for (int markerIdx = 0; markerIdx < pRB->nMarkers; ++markerIdx){const MarkerData& markerPosition = pRB->MarkerPositions[markerIdx];const int markerRequiredLabel = pRB->MarkerRequiredLabels[markerIdx];printf("\tMarker #%d:\n", markerIdx);printf("\t\tPosition: %.2f, %.2f, %.2f\n", markerPosition[0], markerPosition[1], markerPosition[2]);if (markerRequiredLabel != 0){printf("\t\tRequired active label: %d\n", markerRequiredLabel);}}}}else if (pDataDefs->arrDataDescriptions[i].type == Descriptor_Skeleton){// SkeletonsSkeletonDescription* pSK = pDataDefs->arrDataDescriptions[i].Data.SkeletonDescription;printf("Skeleton Name : %s\n", pSK->szName);printf("Skeleton ID : %d\n", pSK->skeletonID);printf("RigidBody (Bone) Count : %d\n", pSK->nRigidBodies);for (int j = 0; j < pSK->nRigidBodies; j++){sRigidBodyDescription* pRB = &pSK->RigidBodies[j];printf("  RigidBody Name : %s\n", pRB->szName);printf("  RigidBody ID : %d\n", pRB->ID);printf("  RigidBody Parent ID : %d\n", pRB->parentID);printf("  Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx, pRB->offsety, pRB->offsetz);}}else if (pDataDefs->arrDataDescriptions[i].type == Descriptor_ForcePlate){// Force PlatesForcePlateDescription* pFP = pDataDefs->arrDataDescriptions[i].Data.ForcePlateDescription;printf("Force Plate ID : %d\n", pFP->ID);printf("Force Plate Serial : %s\n", pFP->strSerialNo);printf("Force Plate Width : %3.2f\n", pFP->fWidth);printf("Force Plate Length : %3.2f\n", pFP->fLength);printf("Force Plate Electrical Center Offset (%3.3f, %3.3f, %3.3f)\n", pFP->fOriginX, pFP->fOriginY, pFP->fOriginZ);for (int iCorner = 0; iCorner < 4; iCorner++)printf("Force Plate Corner %d : (%3.4f, %3.4f, %3.4f)\n", iCorner, pFP->fCorners[iCorner][0], pFP->fCorners[iCorner][1], pFP->fCorners[iCorner][2]);printf("Force Plate Type : %d\n", pFP->iPlateType);printf("Force Plate Data Type : %d\n", pFP->iChannelDataType);printf("Force Plate Channel Count : %d\n", pFP->nChannels);for (int iChannel = 0; iChannel < pFP->nChannels; iChannel++)printf("\tChannel %d : %s\n", iChannel, pFP->szChannelNames[iChannel]);}else if (pDataDefs->arrDataDescriptions[i].type == Descriptor_Device){// Peripheral DevicesDeviceDescription* pDevice = pDataDefs->arrDataDescriptions[i].Data.DeviceDescription;printf("Device Name : %s\n", pDevice->strName);printf("Device Serial : %s\n", pDevice->strSerialNo);printf("Device ID : %d\n", pDevice->ID);printf("Device Channel Count : %d\n", pDevice->nChannels);for (int iChannel = 0; iChannel < pDevice->nChannels; iChannel++)printf("\tChannel %d : %s\n", iChannel, pDevice->szChannelNames[iChannel]);}else{printf("Unknown data type.");// Unknown}}}// Ready to receive marker stream!printf("\nClient is connected to server and listening for data...\n");int c;bool bExit = false;while (c = _getch()){switch (c){case 'q':bExit = true;break;case 'r':resetClient();break;case 'p':sServerDescription ServerDescription;memset(&ServerDescription, 0, sizeof(ServerDescription));g_pClient->GetServerDescription(&ServerDescription);if (!ServerDescription.HostPresent){printf("Unable to connect to server. Host not present. Exiting.");return 1;}break;case 's':{printf("\n\n[SampleClient] Requesting Data Descriptions...");sDataDescriptions* pDataDefs = NULL;iResult = g_pClient->GetDataDescriptionList(&pDataDefs);if (iResult != ErrorCode_OK || pDataDefs == NULL){printf("[SampleClient] Unable to retrieve Data Descriptions.");}else{printf("[SampleClient] Received %d Data Descriptions:\n", pDataDefs->nDataDescriptions);}}break;case 'm':                           // change to multicastg_connectParams.connectionType = ConnectionType_Multicast;iResult = ConnectClient();if (iResult == ErrorCode_OK)printf("Client connection type changed to Multicast.\n\n");elseprintf("Error changing client connection type to Multicast.\n\n");break;case 'u':                          // change to unicastg_connectParams.connectionType = ConnectionType_Unicast;iResult = ConnectClient();if (iResult == ErrorCode_OK)printf("Client connection type changed to Unicast.\n\n");elseprintf("Error changing client connection type to Unicast.\n\n");break;case 'c':                          // connectiResult = ConnectClient();break;case 'd':                          // disconnect// note: applies to unicast connections only - indicates to Motive to stop sending packets to that client endpointiResult = g_pClient->SendMessageAndWait("Disconnect", &response, &nBytes);if (iResult == ErrorCode_OK)printf("[SampleClient] Disconnected");break;default:break;}if (bExit)break;}// Done - clean up.if (g_pClient){g_pClient->Disconnect();delete g_pClient;g_pClient = NULL;}return ErrorCode_OK;return 0;
}// MessageHandler receives NatNet error/debug messages
void NATNET_CALLCONV MessageHandler(Verbosity msgType, const char* msg)
{// Optional: Filter out debug messagesif (msgType < Verbosity_Info){return;}printf("\n[NatNetLib]");switch (msgType){case Verbosity_Debug:printf(" [DEBUG]");break;case Verbosity_Info:printf("  [INFO]");break;case Verbosity_Warning:printf("  [WARN]");break;case Verbosity_Error:printf(" [ERROR]");break;default:printf(" [?????]");break;}printf(": %s\n", msg);
}// DataHandler receives data from the server
// This function is called by NatNet when a frame of mocap data is availablevoid NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData)
{if(1){//主要用来测试的时候开启关闭接收数据的功能用的NatNetClient* pClient = (NatNetClient*)pUserData;// Software latency here is defined as the span of time between://   a) The reception of a complete group of 2D frames from the camera system (CameraDataReceivedTimestamp)// and//   b) The time immediately prior to the NatNet frame being transmitted over the network (TransmitTimestamp)//// This figure may appear slightly higher than the "software latency" reported in the Motive user interface,// because it additionally includes the time spent preparing to stream the data via NatNet.cout << "-------------------------------------------"<<endl;  cout << "Timestamp is: "<<data->CameraDataReceivedTimestamp << endl;cout << "-------------------------------------------"<<endl;const uint64_t softwareLatencyHostTicks = data->TransmitTimestamp - data->CameraDataReceivedTimestamp;const double softwareLatencyMillisec = (softwareLatencyHostTicks * 1000) / static_cast<double>(g_serverDescription.HighResClockFrequency);// Transit latency is defined as the span of time between Motive transmitting the frame of data, and its reception by the client (now).// The SecondsSinceHostTimestamp method relies on NatNetClient's internal clock synchronization with the server using Cristian's algorithm.const double transitLatencyMillisec = pClient->SecondsSinceHostTimestamp(data->TransmitTimestamp) * 1000.0;int i = 0;printf("FrameID : %d\n", data->iFrame);printf("Timestamp : %3.2lf\n", data->fTimestamp);printf("Software latency : %.2lf milliseconds\n", softwareLatencyMillisec);// Only recent versions of the Motive software in combination with ethernet camera systems support system latency measurement.// If it's unavailable (for example, with USB camera systems, or during playback), this field will be zero.const bool bSystemLatencyAvailable = data->CameraMidExposureTimestamp != 0;if (bSystemLatencyAvailable){// System latency here is defined as the span of time between://   a) The midpoint of the camera exposure window, and therefore the average age of the photons (CameraMidExposureTimestamp)// and//   b) The time immediately prior to the NatNet frame being transmitted over the network (TransmitTimestamp)const uint64_t systemLatencyHostTicks = data->TransmitTimestamp - data->CameraMidExposureTimestamp;const double systemLatencyMillisec = (systemLatencyHostTicks * 1000) / static_cast<double>(g_serverDescription.HighResClockFrequency);// Client latency is defined as the sum of system latency and the transit time taken to relay the data to the NatNet client.// This is the all-inclusive measurement (photons to client processing).const double clientLatencyMillisec = pClient->SecondsSinceHostTimestamp(data->CameraMidExposureTimestamp) * 1000.0;// You could equivalently do the following (not accounting for time elapsed since we calculated transit latency above)://const double clientLatencyMillisec = systemLatencyMillisec + transitLatencyMillisec;printf("System latency : %.2lf milliseconds\n", systemLatencyMillisec);printf("Total client latency : %.2lf milliseconds (transit time +%.2lf ms)\n", clientLatencyMillisec, transitLatencyMillisec);}else{printf("Transit latency : %.2lf milliseconds\n", transitLatencyMillisec);}// FrameOfMocapData paramsbool bIsRecording = ((data->params & 0x01) != 0);bool bTrackedModelsChanged = ((data->params & 0x02) != 0);if (bIsRecording)printf("RECORDING\n");if (bTrackedModelsChanged)printf("Models Changed.\n");// timecode - for systems with an eSync and SMPTE timecode generator - decode to valuesint hour, minute, second, frame, subframe;NatNet_DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);// decode to friendly stringchar szTimecode[128] = "";NatNet_TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);printf("Timecode : %s\n", szTimecode);// Rigid Bodies获取刚体数据printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);for (i = 0; i < data->nRigidBodies; i++){// params// 0x01 : bool, rigid body was successfully tracked in this framebool bTrackingValid = data->RigidBodies[i].params & 0x01;printf("Rigid Body [ID=%d  Error=%3.2f  Valid=%d]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError, bTrackingValid);printf("\tx\ty\tz\tqx\tqy\tqz\tqw\n");printf("\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",data->RigidBodies[i].x,data->RigidBodies[i].y,data->RigidBodies[i].z,data->RigidBodies[i].qx,data->RigidBodies[i].qy,data->RigidBodies[i].qz,data->RigidBodies[i].qw);}// Skeletons获取骨骼数据printf("Skeletons [Count=%d]\n", data->nSkeletons);for (i = 0; i < data->nSkeletons; i++){sSkeletonData skData = data->Skeletons[i];printf("Skeleton [ID=%d  Bone count=%d]\n", skData.skeletonID, skData.nRigidBodies);for (int j = 0; j < skData.nRigidBodies; j++){sRigidBodyData rbData = skData.RigidBodyData[j];printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",rbData.ID&IdOffset, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw);}}// labeled markers - this includes all markers (Active, Passive, and 'unlabeled' (markers with no asset but a PointCloud ID)bool bOccluded;     // marker was not visible (occluded) in this framebool bPCSolved;     // reported position provided by point cloud solvebool bModelSolved;  // reported position provided by model solvebool bHasModel;     // marker has an associated asset in the data streambool bUnlabeled;    // marker is 'unlabeled', but has a point cloud ID that matches Motive PointCloud ID (In Motive 3D View)bool bActiveMarker; // marker is an actively labeled LED markerprintf("Markers [Count=%d]\n", data->nLabeledMarkers);for (i = 0; i < data->nLabeledMarkers; i++){bOccluded = ((data->LabeledMarkers[i].params & 0x01) != 0);bPCSolved = ((data->LabeledMarkers[i].params & 0x02) != 0);bModelSolved = ((data->LabeledMarkers[i].params & 0x04) != 0);bHasModel = ((data->LabeledMarkers[i].params & 0x08) != 0);bUnlabeled = ((data->LabeledMarkers[i].params & 0x10) != 0);bActiveMarker = ((data->LabeledMarkers[i].params & 0x20) != 0);sMarker marker = data->LabeledMarkers[i];// Marker ID Scheme:// Active Markers://   ID = ActiveID, correlates to RB ActiveLabels list// Passive Markers: //   If Asset with Legacy Labels//      AssetID    (Hi Word)//      MemberID   (Lo Word)//   Else//      PointCloud IDint modelID, markerID;NatNet_DecodeID(marker.ID, &modelID, &markerID);char szMarkerType[512];if (bActiveMarker)strcpy(szMarkerType, "Active");else if (bUnlabeled)strcpy(szMarkerType, "Unlabeled");elsestrcpy(szMarkerType, "Labeled");printf("%s Marker [ModelID=%d, MarkerID=%d, Occluded=%d, PCSolved=%d, ModelSolved=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n",szMarkerType, modelID, markerID, bOccluded, bPCSolved, bModelSolved, marker.size, marker.x, marker.y, marker.z);}// force platesprintf("Force Plate [Count=%d]\n", data->nForcePlates);for (int iPlate = 0; iPlate < data->nForcePlates; iPlate++){printf("Force Plate %d\n", data->ForcePlates[iPlate].ID);for (int iChannel = 0; iChannel < data->ForcePlates[iPlate].nChannels; iChannel++){printf("\tChannel %d:\t", iChannel);if (data->ForcePlates[iPlate].ChannelData[iChannel].nFrames == 0){printf("\tEmpty Frame\n");}else if (data->ForcePlates[iPlate].ChannelData[iChannel].nFrames != g_analogSamplesPerMocapFrame){printf("\tPartial Frame [Expected:%d   Actual:%d]\n", g_analogSamplesPerMocapFrame, data->ForcePlates[iPlate].ChannelData[iChannel].nFrames);}for (int iSample = 0; iSample < data->ForcePlates[iPlate].ChannelData[iChannel].nFrames; iSample++)printf("%3.2f\t", data->ForcePlates[iPlate].ChannelData[iChannel].Values[iSample]);printf("\n");}}// devicesprintf("Device [Count=%d]\n", data->nDevices);for (int iDevice = 0; iDevice < data->nDevices; iDevice++){printf("Device %d\n", data->Devices[iDevice].ID);for (int iChannel = 0; iChannel < data->Devices[iDevice].nChannels; iChannel++){printf("\tChannel %d:\t", iChannel);if (data->Devices[iDevice].ChannelData[iChannel].nFrames == 0){printf("\tEmpty Frame\n");}else if (data->Devices[iDevice].ChannelData[iChannel].nFrames != g_analogSamplesPerMocapFrame){printf("\tPartial Frame [Expected:%d   Actual:%d]\n", g_analogSamplesPerMocapFrame, data->Devices[iDevice].ChannelData[iChannel].nFrames);}for (int iSample = 0; iSample < data->Devices[iDevice].ChannelData[iChannel].nFrames; iSample++)printf("%3.2f\t", data->Devices[iDevice].ChannelData[iChannel].Values[iSample]);printf("\n");}}}
}void NATNET_CALLCONV ServerDiscoveredCallback(const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext)
{char serverHotkey = '.';if (g_discoveredServers.size() < 9){serverHotkey = static_cast<char>('1' + g_discoveredServers.size());}const char* warning = "";if (pDiscoveredServer->serverDescription.bConnectionInfoValid == false){warning = " (WARNING: Legacy server, could not autodetect settings. Auto-connect may not work reliably.)";}printf("[%c] %s %d.%d at %s%s\n",serverHotkey,pDiscoveredServer->serverDescription.szHostApp,pDiscoveredServer->serverDescription.HostAppVersion[0],pDiscoveredServer->serverDescription.HostAppVersion[1],pDiscoveredServer->serverAddress,warning);g_discoveredServers.push_back(*pDiscoveredServer);
}// Establish a NatNet Client connection
int ConnectClient()
{// Release previous serverg_pClient->Disconnect();// Init Client and connect to NatNet serverint retCode = g_pClient->Connect(g_connectParams);if (retCode != ErrorCode_OK){printf("Unable to connect to server.  Error code: %d. Exiting", retCode);return ErrorCode_Internal;}else{// connection succeededvoid* pResult;int nBytes = 0;ErrorCode ret = ErrorCode_OK;// print server infomemset(&g_serverDescription, 0, sizeof(g_serverDescription));ret = g_pClient->GetServerDescription(&g_serverDescription);if (ret != ErrorCode_OK || !g_serverDescription.HostPresent){printf("Unable to connect to server. Host not present. Exiting.");return 1;}printf("\n[SampleClient] Server application info:\n");printf("Application: %s (ver. %d.%d.%d.%d)\n", g_serverDescription.szHostApp, g_serverDescription.HostAppVersion[0],g_serverDescription.HostAppVersion[1], g_serverDescription.HostAppVersion[2], g_serverDescription.HostAppVersion[3]);printf("NatNet Version: %d.%d.%d.%d\n", g_serverDescription.NatNetVersion[0], g_serverDescription.NatNetVersion[1],g_serverDescription.NatNetVersion[2], g_serverDescription.NatNetVersion[3]);printf("Client IP:%s\n", g_connectParams.localAddress);printf("Server IP:%s\n", g_connectParams.serverAddress);printf("Server Name:%s\n", g_serverDescription.szHostComputerName);// get mocap frame rateret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes);if (ret == ErrorCode_OK){float fRate = *((float*)pResult);printf("Mocap Framerate : %3.2f\n", fRate);}elseprintf("Error getting frame rate.\n");// get # of analog samples per mocap frame of dataret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame", &pResult, &nBytes);if (ret == ErrorCode_OK){g_analogSamplesPerMocapFrame = *((int*)pResult);printf("Analog Samples Per Mocap Frame : %d\n", g_analogSamplesPerMocapFrame);}elseprintf("Error getting Analog frame rate.\n");}return ErrorCode_OK;
}void resetClient()
{int iSuccess;printf("\n\nre-setting Client\n\n.");iSuccess = g_pClient->Disconnect();if (iSuccess != 0)printf("error un-initting Client\n");iSuccess = g_pClient->Connect(g_connectParams);if (iSuccess != 0)printf("error re-initting Client\n");
}

这个程序如果能够正常编译就可以实时输出Motive的追踪数据了，下面就可以开始往Vrpn里接入了。

2.VRPN添加设备

跟之前的一样，还是那几个文件，这次是参照DTrack写的，把DTrack相关的文件先复制一份出来，把名字改一下，然后就添加了一个叫vrpn_Tracker_Motive的设备了。vrpn_Generic_server_object这类代码的修改跟前面添加Joystick大同小异，就不赘述了，参考前文修改一下即可。其中需要注意的是setup那个函数，就是从配置文件读取构造函数参数的那个。由于Natnet自己通过网络轮询直接找到了服务器的位置并建立了连接，所以就不用像Dtrack那样设置一堆参数控制端口啥的，具体需要什么参数得看应用需求。

下边重点看vrpn_Tracker_Motive.h和vrpn_Tracker_Motive.c两个文件。

首先是.h

// vrpn_Tracker_Motive.h #ifndef VRPN_TRACKER_MOTIVE_H
#define VRPN_TRACKER_MOTIVE_H#include "vrpn_Configure.h"             // for VRPN_API
#include "vrpn_Shared.h"                // for timevalclass VRPN_API vrpn_Connection;
// There is a problem with linking on SGI related to the use of standard
// libraries.
#ifndef sgi#include <stdio.h>                      // for NULL
#include <vector>                       // for vector#include "vrpn_Analog.h"                // for vrpn_Analog
#include "vrpn_Button.h"                // for vrpn_Button_Filter
#include "vrpn_Tracker.h"               // for vrpn_Tracker#include <iostream>
#include <inttypes.h>
#include <stdlib.h>
#include <string.h>#ifdef _WIN32
#   include <conio.h>
#else
#   include <unistd.h>
#   include <termios.h>
#endif#include <NatNetTypes.h>
#include <NatNetCAPI.h>
#include <NatNetClient.h>
//这个是比较坑的，Natnet本身不是源码，得依赖他的静态库，所以需要在代码里添加这个静态库的引用，而且，在编译server的时候还得在server工程中添加这个库的目录
//更坑的是，Natnet还有个dll的动态库，回头就算是编译好了，也得把动态库加到exe目录下才能用。
#pragma comment(lib, "NatNetLib.lib")
// --------------------------------------------------------------------------
// VRPN class:class VRPN_API vrpn_Tracker_Motive : public vrpn_Tracker, public vrpn_Button_Filter, public vrpn_Analog
{public:// Constructor:
// name (i): device name
// c (i): vrpn_Connectionvrpn_Tracker_Motive(const char *name, vrpn_Connection *c);//由于Natnet建立连接的时候不需要指定ip和端口所以就不需要什么配置参数~vrpn_Tracker_Motive();//vrpn的主循环，启动后自动执行，这是第二个比较坑的地方，因为Natnet采用的是绑定CallBack函数并在其中执行处理的方式，所以不能在Mainloop中获取数据//同时，由于类内的成员函数都会隐含一个this指针，所以如果将CallBack函数定义在类里会出现参数不匹配的问题//但是，如果不把CallBack函数写在类里，就会出现成员变量获取不了的问题，所以只能通过this指针把类本身传递进入CallBack函数中再进行处理。virtual void mainloop();//由于CallBack函数没法直接访问类中的参数和函数，所以需要创建几个接口方便调用，下边几个都是void callMainLoop();void setId(int id);void setLoc(float x,float y, float z);void setRot(float x, float y, float z, float w);void encodeAndTrans(struct timeval timestamp);private:bool motive_init();//初始化连接并挂载CallBack函数bool motive_exit(void);//退出并卸载相关对象int ConnectClient();//init中需要的函数void resetClient();//断开从连，暂时没用到};#endif#endif

然后是.c

// usually the following should work:#ifndef _WIN32#define OS_UNIX  // for Unix (Linux, Irix, ...)
#else#define OS_WIN   // for MS Windows (2000, XP, ...)
#endif#include <stdio.h>                      // for NULL, printf, fprintf, etc
#include <stdlib.h>                     // for strtod, exit, free, malloc, etc
#include <string.h>                     // for strncmp, memset, strcat, etc#include "quat.h"                       // for Q_RAD_TO_DEG, etc
#include "vrpn_Connection.h"            // for vrpn_CONNECTION_LOW_LATENCY, etc
#include "vrpn_Shared.h"                // for timeval, INVALID_SOCKET, etc
#include "vrpn_Tracker_Motive.h"
#include "vrpn_Types.h"                 // for vrpn_float64
#include "vrpn_MessageMacros.h"         // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR// There is a problem with linking on SGIs related to standard libraries.
//#ifndef sgi// --------------------------------------------------------------------------
// Globals:
// Natnet Prameters
int IdOffset = 65535;//用于提取ID的后16位
int AdjId = 0;//用于存储修正后的Id
NatNetClient* g_pClient = NULL;
int g_analogSamplesPerMocapFrame = 0;
sServerDescription g_serverDescription;
std::vector<sNatNetDiscoveredServer> g_discoveredServers;
sNatNetClientConnectParams g_connectParams;
static const ConnectionType kDefaultConnectionType = ConnectionType_Multicast;
char g_discoveredMulticastGroupAddr[kNatNetIpv4AddrStrLenMax] =NATNET_DEFAULT_MULTICAST_ADDRESS;
// Declaration of callback function
void NATNET_CALLCONV MessageHandler(Verbosity msgType, const char* msg);
void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData);
void NATNET_CALLCONV ServerDiscoveredCallback(const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext);// --------------------------------------------------------------------------
// Constructor:
// name (i): device name
// c (i): vrpn_Connection//变量是通过vrpn_Generic_server_object.h中的setup函数设定，setup函数是通过Config文件读取相关参数
vrpn_Tracker_Motive::vrpn_Tracker_Motive(const char *name, vrpn_Connection *c) :vrpn_Tracker(name, c),    vrpn_Button_Filter(name, c),vrpn_Analog(name, c)
{// init:Start to retrive data from Motiveif(!motive_init()){exit(EXIT_FAILURE);}
}// Destructor:vrpn_Tracker_Motive::~vrpn_Tracker_Motive() {motive_exit();
}// --------------------------------------------------------------------------
// Main loop:// This function should be called each time through the main loop
// of the server code. It checks for a report from the tracker and
// sends it if there is one.void vrpn_Tracker_Motive::mainloop() { //本来应该在这处理的，但是由于Natnet使用方式的问题只能移出去//有个坑在于这个线程即使不做处理也会有影响，如果不Sleep的话就会持续打断数据处理线程//测试后发现，把这个线程挂起一段时间就可以解决，但是也不能挂起太长//如果挂起几秒会出现Client端无法接受数据的问题，所以这也是个坑，如果有好的解决方法可以交流一下Sleep(100); }// --------------------------------------------------------------------------
// Class functions:// Initializing communication with Motive start the DataProcessing:
//
// return value (o): initialization was successful (boolean)bool vrpn_Tracker_Motive::motive_init()
{/*Try to find the Server*/// print version infounsigned char ver[4];NatNet_GetVersion(ver);printf("NatNet Sample Client (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1],ver[2], ver[3]);// Install logging callbackNatNet_SetLogCallback(MessageHandler);// create NatNet clientg_pClient = new NatNetClient();// set the frame callback handler，在这里绑定CallBack函数并把this指针输入进去g_pClient->SetFrameReceivedCallback(DataHandler, this); // this function will receive data from the serverprintf("Start looking for servers on the local network.\n");// try to find the serverNatNetDiscoveryHandle discovery;NatNet_CreateAsyncServerDiscovery(&discovery, ServerDiscoveredCallback);// build the connection parametersunsigned int serverIndex = 0;// Make sure at least one server is foundwhile (g_discoveredServers.size() < 1) {std::cout << "searching for Server" << std::endl;}/*Try to connect to the server*/const sNatNetDiscoveredServer& discoveredServer =g_discoveredServers[serverIndex];if (discoveredServer.serverDescription.bConnectionInfoValid) {// Build the connection parameters.
#ifdef _WIN32_snprintf_s(
#elsesnprintf(
#endifg_discoveredMulticastGroupAddr,sizeof g_discoveredMulticastGroupAddr,"%" PRIu8 ".%" PRIu8 ".%" PRIu8 ".%" PRIu8 "",discoveredServer.serverDescription.ConnectionMulticastAddress[0],discoveredServer.serverDescription.ConnectionMulticastAddress[1],discoveredServer.serverDescription.ConnectionMulticastAddress[2],discoveredServer.serverDescription.ConnectionMulticastAddress[3]);g_connectParams.connectionType =discoveredServer.serverDescription.ConnectionMulticast? ConnectionType_Multicast: ConnectionType_Unicast;g_connectParams.serverCommandPort = discoveredServer.serverCommandPort;g_connectParams.serverDataPort =discoveredServer.serverDescription.ConnectionDataPort;g_connectParams.serverAddress = discoveredServer.serverAddress;g_connectParams.localAddress = discoveredServer.localAddress;g_connectParams.multicastAddress = g_discoveredMulticastGroupAddr;}else {// We're missing some info because it's a legacy server.// Guess the defaults and make a best effort attempt to connect.g_connectParams.connectionType = kDefaultConnectionType;g_connectParams.serverCommandPort = discoveredServer.serverCommandPort;g_connectParams.serverDataPort = 0;g_connectParams.serverAddress = discoveredServer.serverAddress;g_connectParams.localAddress = discoveredServer.localAddress;g_connectParams.multicastAddress = NULL;}NatNet_FreeAsyncServerDiscovery(discovery);int iResult;// Connect to MotiveiResult = ConnectClient();if (iResult != ErrorCode_OK) {printf("Error initializing client.  See log for details.  Exiting");return 1;}else {printf("Client initialized and ready.\n");}// Send/receive Contextvoid* response;int nBytes;printf("[SampleClient] Sending Test Request\n");iResult = g_pClient->SendMessageAndWait("TestRequest", &response, &nBytes);if (iResult == ErrorCode_OK) {printf("[SampleClient] Received: %s", (char*)response);}// Retrieve Data Descriptions from Motiveprintf("\n\n[SampleClient] Requesting Data Descriptions...");sDataDescriptions* pDataDefs = NULL;iResult = g_pClient->GetDataDescriptionList(&pDataDefs);if (iResult != ErrorCode_OK || pDataDefs == NULL) {printf("[SampleClient] Unable to retrieve Data Descriptions.");return -2;}else {printf("[SampleClient] Received %d Data Descriptions:\n",pDataDefs->nDataDescriptions);for (int i = 0; i < pDataDefs->nDataDescriptions; i++) {if (pDataDefs->arrDataDescriptions[i].type ==Descriptor_RigidBody) {// RigidBodysRigidBodyDescription* pRB =pDataDefs->arrDataDescriptions[i].Data.RigidBodyDescription;printf("RigidBody Name : %s\n", pRB->szName);printf("RigidBody ID : %d\n", pRB->ID);printf("RigidBody Parent ID : %d\n", pRB->parentID);printf("Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx,pRB->offsety, pRB->offsetz);if (pRB->MarkerPositions != NULL &&pRB->MarkerRequiredLabels != NULL) {for (int markerIdx = 0; markerIdx < pRB->nMarkers;++markerIdx) {const MarkerData& markerPosition =pRB->MarkerPositions[markerIdx];const int markerRequiredLabel =pRB->MarkerRequiredLabels[markerIdx];printf("\tMarker #%d:\n", markerIdx);printf("\t\tPosition: %.2f, %.2f, %.2f\n",markerPosition[0], markerPosition[1],markerPosition[2]);if (markerRequiredLabel != 0) {printf("\t\tRequired active label: %d\n",markerRequiredLabel);}}}}else if (pDataDefs->arrDataDescriptions[i].type ==Descriptor_Skeleton) {// SkeletonsSkeletonDescription* pSK =pDataDefs->arrDataDescriptions[i].Data.SkeletonDescription;printf("Skeleton Name : %s\n", pSK->szName);printf("Skeleton ID : %d\n", pSK->skeletonID);printf("RigidBody (Bone) Count : %d\n", pSK->nRigidBodies);for (int j = 0; j < pSK->nRigidBodies; j++) {sRigidBodyDescription* pRB = &pSK->RigidBodies[j];printf("  RigidBody Name : %s\n", pRB->szName);printf("  RigidBody ID : %d\n", pRB->ID);printf("  RigidBody Parent ID : %d\n", pRB->parentID);printf("  Parent Offset : %3.2f,%3.2f,%3.2f\n",pRB->offsetx, pRB->offsety, pRB->offsetz);}}else if (pDataDefs->arrDataDescriptions[i].type ==Descriptor_Device) {// Peripheral DevicesDeviceDescription* pDevice =pDataDefs->arrDataDescriptions[i].Data.DeviceDescription;printf("Device Name : %s\n", pDevice->strName);printf("Device Serial : %s\n", pDevice->strSerialNo);printf("Device ID : %d\n", pDevice->ID);printf("Device Channel Count : %d\n", pDevice->nChannels);for (int iChannel = 0; iChannel < pDevice->nChannels;iChannel++)printf("\tChannel %d : %s\n", iChannel,pDevice->szChannelNames[iChannel]);}else {//printf("Unknown data type.");// Unknown}}}return true;
}// Deinitializing communication with Motive:
//
// return value (o): deinitialization was successful (boolean)bool vrpn_Tracker_Motive::motive_exit(void)
{// Done - clean up.if (g_pClient) {g_pClient->Disconnect();delete g_pClient;g_pClient = NULL;}return true;
}// Call mainloop:
void vrpn_Tracker_Motive::callMainLoop() {// call the generic server mainloop, since we are a server:server_mainloop();
}// Set id of VrpnTracker:void vrpn_Tracker_Motive::setId(int id){// d_sensor是父类vrpn_Tracker的成员变量，每次读入id数据写到这里就行d_sensor = id;
}// Set location of VrpnTracker:
//
// unit in metervoid vrpn_Tracker_Motive::setLoc(float x, float y, float z) { //pos是父类vrpn_Tracker的成员变量，每次读入位置数据写到这里就行pos[0] = x;pos[1] = y;pos[2] = z;
}// Set rotation of VrpnTracker:
//
// quaternion type with x,y,z wvoid vrpn_Tracker_Motive::setRot(float x, float y, float z, float w)
{// d_quat是父类vrpn_Tracker的成员变量，每次读入旋转数据写到这里就行d_quat[0] = x;d_quat[1] = y;d_quat[2] = z;d_quat[3] = w;
}// Encode And transmit the recorded datas:void vrpn_Tracker_Motive::encodeAndTrans(struct timeval timestamp)
{//调用父类d_quat的函数，将d_quat,pos以及d_sensor打包发送给vrpnif (d_connection) {char msgbuf[1000];int len = vrpn_Tracker::encode_to(msgbuf);if (d_connection->pack_message(len, timestamp, position_m_id,d_sender_id, msgbuf,vrpn_CONNECTION_LOW_LATENCY)) {fprintf(stderr,"vrpn_Tracker_DTrack: cannot write message: tossing.\n");}}
}// ----------------------------------------------------------------------------------------------------
// Motive process related function// MessageHandler receives NatNet error/debug messages
void NATNET_CALLCONV MessageHandler(Verbosity msgType,const char* msg)
{// Optional: Filter out debug messagesif (msgType < Verbosity_Info) {return;}printf("\n[NatNetLib]");switch (msgType) {case Verbosity_Debug:printf(" [DEBUG]");break;case Verbosity_Info:printf("  [INFO]");break;case Verbosity_Warning:printf("  [WARN]");break;case Verbosity_Error:printf(" [ERROR]");break;default:printf(" [?????]");break;}printf(": %s\n", msg);
}// DataHandler receives data from the server
// This function is called by NatNet when a frame of mocap data is available
//核心数据处理代码，循环、接收、处理、发送都在这里
void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData)
{//获取指向自己的指针vrpn_Tracker_Motive* pMotive = (vrpn_Tracker_Motive*)pUserData;pMotive->callMainLoop();// get time stamp:struct timeval timestamp;  vrpn_gettimeofday(&timestamp, NULL);int i = 0;// FrameOfMocapData params//bool bIsRecording = ((data->params & 0x01) != 0);//bool bTrackedModelsChanged = ((data->params & 0x02) != 0);//if (bIsRecording) printf("RECORDING\n");//if (bTrackedModelsChanged) printf("Models Changed.\n");// Rigid Bodies//printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);for (i = 0; i < data->nRigidBodies; i++) {// params// 0x01 : bool, rigid body was successfully tracked in this framebool bTrackingValid = data->RigidBodies[i].params & 0x01;//Set the 6Dof tracking datapMotive->setId(data->RigidBodies[i].ID);//the id less than 100 refer to a rigid bodypMotive->setLoc(data->RigidBodies[i].x, data->RigidBodies[i].y, data->RigidBodies[i].z);pMotive->setRot(data->RigidBodies[i].qx, data->RigidBodies[i].qy,data->RigidBodies[i].qz, data->RigidBodies[i].qw);//Encode and send to vrpnpMotive->encodeAndTrans(timestamp);}// Skeletons//printf("Skeletons [Count=%d]\n", data->nSkeletons);for (i = 0; i < data->nSkeletons; i++) {sSkeletonData skData = data->Skeletons[i];/*printf("Skeleton [ID=%d  Bone count=%d]\n", skData.skeletonID,skData.nRigidBodies);*/for (int j = 0; j < skData.nRigidBodies; j++) {sRigidBodyData rbData = skData.RigidBodyData[j];// To identify the skeleton with the Id, 1xx for first skeleton,2xx for the second//为了区分刚体和人体，我在id上进行了一些处理，刚体的id就是自己的id，人体的id在提取后16位后统一加上100*人体骨架编号，以此区分AdjId = rbData.ID & IdOffset;AdjId += 100 * (skData.skeletonID + 1);pMotive->setId(AdjId); pMotive->setLoc(rbData.x, rbData.y, rbData.z);pMotive->setRot(rbData.qx, rbData.qy, rbData.qz, rbData.qw);/*printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",rbData.ID, rbData.x, rbData.y, rbData.z, rbData.qx,rbData.qy, rbData.qz, rbData.qw);*/// Encode and send to vrpnpMotive->encodeAndTrans(timestamp);}       }/*// labeled markers - this includes all markers (Active, Passive, and// 'unlabeled' (markers with no asset but a PointCloud ID)bool bOccluded;     // marker was not visible (occluded) in this framebool bPCSolved;     // reported position provided by point cloud solvebool bModelSolved;  // reported position provided by model solvebool bHasModel;     // marker has an associated asset in the data streambool bUnlabeled;    // marker is 'unlabeled', but has a point cloud ID that// matches Motive PointCloud ID (In Motive 3D View)bool bActiveMarker; // marker is an actively labeled LED marker//printf("Markers [Count=%d]\n", data->nLabeledMarkers);for (i = 0; i < data->nLabeledMarkers; i++) {bOccluded = ((data->LabeledMarkers[i].params & 0x01) != 0);bPCSolved = ((data->LabeledMarkers[i].params & 0x02) != 0);bModelSolved = ((data->LabeledMarkers[i].params & 0x04) != 0);bHasModel = ((data->LabeledMarkers[i].params & 0x08) != 0);bUnlabeled = ((data->LabeledMarkers[i].params & 0x10) != 0);bActiveMarker = ((data->LabeledMarkers[i].params & 0x20) != 0);sMarker marker = data->LabeledMarkers[i];// Marker ID Scheme:// Active Markers://   ID = ActiveID, correlates to RB ActiveLabels list// Passive Markers://   If Asset with Legacy Labels//      AssetID   (Hi Word)//      MemberID   (Lo Word)//   Else//      PointCloud IDint modelID, markerID;NatNet_DecodeID(marker.ID, &modelID, &markerID);char szMarkerType[512];if (bActiveMarker)strcpy(szMarkerType, "Active");else if (bUnlabeled)strcpy(szMarkerType, "Unlabeled");elsestrcpy(szMarkerType, "Labeled");printf("%s Marker [ModelID=%d, MarkerID=%d, Occluded=%d, PCSolved=%d, ""ModelSolved=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n",szMarkerType, modelID, markerID, bOccluded, bPCSolved,bModelSolved, marker.size, marker.x, marker.y, marker.z);}*/// force plates/*printf("Force Plate [Count=%d]\n", data->nForcePlates);for (int iPlate = 0; iPlate < data->nForcePlates; iPlate++) {printf("Force Plate %d\n", data->ForcePlates[iPlate].ID);for (int iChannel = 0; iChannel < data->ForcePlates[iPlate].nChannels;iChannel++) {printf("\tChannel %d:\t", iChannel);if (data->ForcePlates[iPlate].ChannelData[iChannel].nFrames == 0) {printf("\tEmpty Frame\n");}else if (data->ForcePlates[iPlate].ChannelData[iChannel].nFrames !=g_analogSamplesPerMocapFrame) {printf("\tPartial Frame [Expected:%d   Actual:%d]\n",g_analogSamplesPerMocapFrame,data->ForcePlates[iPlate].ChannelData[iChannel].nFrames);}for (int iSample = 0;iSample <data->ForcePlates[iPlate].ChannelData[iChannel].nFrames;iSample++)printf("%3.2f\t", data->ForcePlates[iPlate].ChannelData[iChannel].Values[iSample]);printf("\n");}}*/// devices for Buttons/*printf("Device [Count=%d]\n", data->nDevices);for (int iDevice = 0; iDevice < data->nDevices; iDevice++) {printf("Device %d\n", data->Devices[iDevice].ID);for (int iChannel = 0; iChannel < data->Devices[iDevice].nChannels;iChannel++) {printf("\tChannel %d:\t", iChannel);if (data->Devices[iDevice].ChannelData[iChannel].nFrames == 0) {printf("\tEmpty Frame\n");}else if (data->Devices[iDevice].ChannelData[iChannel].nFrames !=g_analogSamplesPerMocapFrame) {printf("\tPartial Frame [Expected:%d   Actual:%d]\n",g_analogSamplesPerMocapFrame,data->Devices[iDevice].ChannelData[iChannel].nFrames);}for (int iSample = 0;iSample < data->Devices[iDevice].ChannelData[iChannel].nFrames;iSample++)printf("%3.2f\t", data->Devices[iDevice].ChannelData[iChannel].Values[iSample]);printf("\n");}}*/
}void NATNET_CALLCONV ServerDiscoveredCallback(const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext)
{char serverHotkey = '.';if (g_discoveredServers.size() < 9) {serverHotkey = static_cast<char>('1' + g_discoveredServers.size());}const char* warning = "";if (pDiscoveredServer->serverDescription.bConnectionInfoValid == false) {warning = " (WARNING: Legacy server, could not autodetect settings. ""Auto-connect may not work reliably.)";}printf("[%c] %s %d.%d at %s%s\n", serverHotkey,pDiscoveredServer->serverDescription.szHostApp,pDiscoveredServer->serverDescription.HostAppVersion[0],pDiscoveredServer->serverDescription.HostAppVersion[1],pDiscoveredServer->serverAddress, warning);g_discoveredServers.push_back(*pDiscoveredServer);
}// Establish a NatNet Client connection
int vrpn_Tracker_Motive::ConnectClient()
{// Release previous serverg_pClient->Disconnect();// Init Client and connect to NatNet serverint retCode = g_pClient->Connect(g_connectParams);if (retCode != ErrorCode_OK) {printf("Unable to connect to server.  Error code: %d. Exiting",retCode);return ErrorCode_Internal;}else {// connection succeededvoid* pResult;int nBytes = 0;ErrorCode ret = ErrorCode_OK;// print server infomemset(&g_serverDescription, 0, sizeof(g_serverDescription));ret = g_pClient->GetServerDescription(&g_serverDescription);if (ret != ErrorCode_OK || !g_serverDescription.HostPresent) {printf("Unable to connect to server. Host not present. Exiting.");return 1;}printf("\n[SampleClient] Server application info:\n");printf("Application: %s (ver. %d.%d.%d.%d)\n",g_serverDescription.szHostApp,g_serverDescription.HostAppVersion[0],g_serverDescription.HostAppVersion[1],g_serverDescription.HostAppVersion[2],g_serverDescription.HostAppVersion[3]);printf("NatNet Version: %d.%d.%d.%d\n",g_serverDescription.NatNetVersion[0],g_serverDescription.NatNetVersion[1],g_serverDescription.NatNetVersion[2],g_serverDescription.NatNetVersion[3]);printf("Client IP:%s\n", g_connectParams.localAddress);printf("Server IP:%s\n", g_connectParams.serverAddress);printf("Server Name:%s\n", g_serverDescription.szHostComputerName);// get mocap frame rateret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes);if (ret == ErrorCode_OK) {float fRate = *((float*)pResult);printf("Mocap Framerate : %3.2f\n", fRate);}elseprintf("Error getting frame rate.\n");// get # of analog samples per mocap frame of dataret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame",&pResult, &nBytes);if (ret == ErrorCode_OK) {g_analogSamplesPerMocapFrame = *((int*)pResult);printf("Analog Samples Per Mocap Frame : %d\n",g_analogSamplesPerMocapFrame);}elseprintf("Error getting Analog frame rate.\n");}return ErrorCode_OK;
}void vrpn_Tracker_Motive::resetClient()
{int iSuccess;printf("\n\nre-setting Client\n\n.");iSuccess = g_pClient->Disconnect();if (iSuccess != 0) printf("error un-initting Client\n");iSuccess = g_pClient->Connect(g_connectParams);if (iSuccess != 0) printf("error re-initting Client\n");
}//#endif

3.小结

至此就完成了追踪设备的添加，总体跟添加一个Joystick没什么太大区别，就是发送数据有些差异。其中有一些坑我在备注里写了，在这再小结一下：

1.Natnet Sdk本身不是源码，需要lib和dll库的配合才能用，lib库需要在编译的时候添加到vrpn_Tracker_Motive以及vrpn_server的工程中才能通过编译。dll需要在运行的时候添加到系统环境变量或者编译好的vrpn_server.exe目录下。

2.Natnet Sdk本身采用的CallBack的形式进行数据处理，这样就跟vrpn的mainloop起了冲突，一种做法是在Class中添加变量，通过CallBack函数进行数值更新，然后再在mainloop中进行打包发送，这样得做两个线程间的同步，而且由于线程不是自己创建的，改起来可能会比较麻烦。我用了第二种方法，就是抛弃mainloop，直接在CallBack线程中把所有活都干了，mainloop就是空跑。这样可以用，但是发现存在点问题，就是如果mainloop不加等待的话会不断中断Callback线程，导致数据拿不全，所以我在mainloop里加了个100毫秒的sleep，加完发现暂时没问题了，如果有更好的解决方案欢迎大家交流。

3.由于采用了Callback函数的方式处理，函数声明的位置有点不好处理。由于需要调用vrpn_Tracker_Motive以及其父类的参数和方法，所以最好是作为成员变量声明在类中。但是由于成员变量会隐含this指针，这会导致绑定CallBack函数时出现参数不匹配的编译错误。所以只能写在类的外边，再通过this指针把类本身传递给回调函数进行处理。

4.在处理骨架中骨骼的ID时出现了三个小问题。

问题一：在Motive中，会按照每次运行软件后打开人体骨架的顺序为骨架设置ID，例如我们有a,b,c三个tak文件，其中各记录了一个人的追踪数据，当我们按照abc顺序加载文件后，a文件中骨架的ID会被设置为1，b被设置为2，c被设置为3，此后，只要不关闭软件，不论按照何种顺序再次打开abc文件，他们对应的id值都是不变的。如果关闭软件，再次打开后，骨架的id值会被重置，id会依照新的打开顺序进行设定，如下次的打开顺序为bac，那么id对应为a-2,b-1,c-3。另外有一点需要注意，如果上述abc中存在两个文件使用了相同的骨骼的话，尽管所在文件不同，他们也会被设置为同一ID。

问题二：从Natnet拿到的骨骼id数据包含了骨架ID与骨骼ID两部分信息，NatNet存储ID使用了一个32位的int类型，其中前16位代表骨架ID，后16位代表骨骼ID，所以在使用中需要将骨骼ID提取出来。

问题三：由于Rigidbody的ID是根据设定值定的，所以可能会出现100以上的情况，由于我们自己定义百位代表骨骼ID所以在设置Rigidbody的ID时要注意不能超过100。如果有特殊情况需要用到100以上的RigidBody编号，则可以将原始的ID传递过去，然后在客户端进行解析，拆分骨骼ID。

4.补充

2021.08.27补充：空跑果然还是有问题，如果要同时接收多个设备的数据，比如optitrack和xsense的数据，这样空跑就会造成卡顿。卡顿的原因推测是mainloop里vrpn自己做了线程同步，让mainloop空跑，自己开线程去发送就会使同步机制失效，导致线程间的干扰，所以最后还是改成先在本地保存，然后在mainloop中统一发送的方式。修改后的代码如下：

首先是.h文件

// vrpn_Tracker_Motive.h #ifndef VRPN_TRACKER_MOTIVE_H
#define VRPN_TRACKER_MOTIVE_H#include "vrpn_Configure.h"             // for VRPN_API
#include "vrpn_Shared.h"                // for timevalclass VRPN_API vrpn_Connection;
// There is a problem with linking on SGI related to the use of standard
// libraries.
#ifndef sgi#include <stdio.h>                      // for NULL
#include <vector>                       // for vector
#include <map>#include "vrpn_Analog.h"                // for vrpn_Analog
#include "vrpn_Button.h"                // for vrpn_Button_Filter
#include "vrpn_Tracker.h"               // for vrpn_Tracker#include <iostream>
#include <inttypes.h>
#include <stdlib.h>
#include <string.h>#ifdef _WIN32
#   include <conio.h>
#else
#   include <unistd.h>
#   include <termios.h>
#endif#include <NatNetTypes.h>
#include <NatNetCAPI.h>
#include <NatNetClient.h>
//这个是比较坑的，Natnet本身不是源码，得依赖他的静态库，所以需要在代码里添加这个静态库的引用，而且，在编译server的时候还得在server工程中添加这个库的目录
//更坑的是，Natnet还有个dll的动态库，回头就算是编译好了，也得把动态库加到exe目录下才能用。
#pragma comment(lib, "NatNetLib.lib")
// --------------------------------------------------------------------------
// VRPN class:class VRPN_API vrpn_Tracker_Motive : public vrpn_Tracker, public vrpn_Button_Filter, public vrpn_Analog
{public:// Constructor:
// name (i): device name
// c (i): vrpn_Connectionvrpn_Tracker_Motive(const char *name, vrpn_Connection *c);//由于Natnet建立连接的时候不需要指定ip和端口所以就不需要什么配置参数~vrpn_Tracker_Motive();//vrpn的主循环，启动后自动执行，这是第二个比较坑的地方，因为Natnet采用的是绑定CallBack函数并在其中执行处理的方式，所以不能在Mainloop中获取数据//同时，由于类内的成员函数都会隐含一个this指针，所以如果将CallBack函数定义在类里会出现参数不匹配的问题//但是，如果不把CallBack函数写在类里，就会出现成员变量获取不了的问题，所以只能通过this指针把类本身传递进入CallBack函数中再进行处理。virtual void mainloop();//由于CallBack函数没法直接访问类中的参数和函数，所以需要创建几个接口方便调用，下边几个都是void callMainLoop();void setId(int id);void setLoc(float x,float y, float z);void setRot(float x, float y, float z, float w);void encodeAndTrans(struct timeval timestamp);struct posQuat {float px;float py;float pz;float qx;float qy;float qz;float qw;};std::map<int, posQuat> targetMap;//用于本地存储刚体数据的字典std::map<int, posQuat>::iterator iter = targetMap.begin();//用于索引字典数据的迭代器bool motive_init();//初始化连接并挂载CallBack函数bool motive_exit(void);//退出并卸载相关对象int ConnectClient();//init中需要的函数void resetClient();//断开从连，暂时没用到};#endif#endif

然后是.c文件

// usually the following should work:#ifndef _WIN32#define OS_UNIX  // for Unix (Linux, Irix, ...)
#else#define OS_WIN   // for MS Windows (2000, XP, ...)
#endif#include <stdio.h>                      // for NULL, printf, fprintf, etc
#include <stdlib.h>                     // for strtod, exit, free, malloc, etc
#include <string.h>                     // for strncmp, memset, strcat, etc#include "quat.h"                       // for Q_RAD_TO_DEG, etc
#include "vrpn_Connection.h"            // for vrpn_CONNECTION_LOW_LATENCY, etc
#include "vrpn_Shared.h"                // for timeval, INVALID_SOCKET, etc
#include "vrpn_Tracker_Motive.h"
#include "vrpn_Types.h"                 // for vrpn_float64
#include "vrpn_MessageMacros.h"         // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR// There is a problem with linking on SGIs related to standard libraries.
//#ifndef sgi// --------------------------------------------------------------------------
// Globals:
// Natnet Prameters
int IdOffset = 65535;//For extracting the low 16 bit of the Id
int AdjId = 0;//for saving the result of the Adjust Id
NatNetClient* g_pClient = NULL;
int g_analogSamplesPerMocapFrame = 0;
sServerDescription g_serverDescription;
std::vector<sNatNetDiscoveredServer> g_discoveredServers;
sNatNetClientConnectParams g_connectParams;
static const ConnectionType kDefaultConnectionType = ConnectionType_Multicast;
char g_discoveredMulticastGroupAddr[kNatNetIpv4AddrStrLenMax] =NATNET_DEFAULT_MULTICAST_ADDRESS;
// Declaration of callback function
void NATNET_CALLCONV MessageHandler(Verbosity msgType, const char* msg);
void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData);
void NATNET_CALLCONV ServerDiscoveredCallback(const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext);// --------------------------------------------------------------------------
// Constructor:
// name (i): device name
// c (i): vrpn_Connection//变量是通过vrpn_Generic_server_object.h中的setup函数设定，setup函数是通过Config文件读取相关参数
vrpn_Tracker_Motive::vrpn_Tracker_Motive(const char *name, vrpn_Connection *c) :vrpn_Tracker(name, c),    vrpn_Button_Filter(name, c),vrpn_Analog(name, c)
{// init:Start to retrive data from Motiveif(!motive_init()){exit(EXIT_FAILURE);}
}// Destructor:vrpn_Tracker_Motive::~vrpn_Tracker_Motive() {motive_exit();
}// --------------------------------------------------------------------------
// Main loop:// This function should be called each time through the main loop
// of the server code. It checks for a report from the tracker and
// sends it if there is one.void vrpn_Tracker_Motive::mainloop() { //1.直接发方案//本来应该在这处理的，但是由于Natnet使用方式的问题只能移出去//有个坑在于这个线程即使不做处理也会有影响，如果不Sleep的话就会持续打断数据处理线程//测试后发现，把这个线程挂起一段时间就可以解决，但是也不能挂起太长//如果挂起几秒会出现Client端无法接受数据的问题，所以这也是个坑，如果有好的解决方法可以交流一下//Sleep(20); //2.先存起来方案struct timeval timestamp;vrpn_gettimeofday(&timestamp, NULL);   //iter = targetMap.begin();iter = targetMap.begin();/*2.1 Manually report*/while (iter != targetMap.end()) {//if (iter->first>103) {setId(iter->first); //printf("Id =%d \n", iter->first);setLoc(iter->second.px, iter->second.py, iter->second.pz);setRot(iter->second.qx, iter->second.qy, iter->second.qz,iter->second.qw);// Encode and send to vrpnencodeAndTrans(timestamp);//}iter++;}/*2.2 Use the defined function*//* while (iter != targetMap.end()) {setId(iter->first);//printf("Id =%d \n", iter->first);setLoc(iter->second.px, iter->second.py, iter->second.pz);setRot(iter->second.qx, iter->second.qy, iter->second.qz,iter->second.qw);// Encode and send to vrpnif (d_connection && iter->first>4) {printf("Id =%d \n", iter->first);char msgbuf[1000];int len = vrpn_Tracker::encode_to(msgbuf);//printf("Id =%d \n", d_sensor);if (d_connection->pack_message(len, timestamp, position_m_id,d_sender_id, msgbuf,vrpn_CONNECTION_LOW_LATENCY)) {fprintf(stderr,"vrpn_Tracker_DTrack: cannot write message: tossing.\n");}}iter++;}*///printf("\nFrameUpdated\n");}// --------------------------------------------------------------------------
// Class functions:// Initializing communication with Motive start the DataProcessing:
//
// return value (o): initialization was successful (boolean)bool vrpn_Tracker_Motive::motive_init()
{/*Try to find the Server*/// print version infounsigned char ver[4];NatNet_GetVersion(ver);printf("NatNet Sample Client (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1],ver[2], ver[3]);// Install logging callbackNatNet_SetLogCallback(MessageHandler);// create NatNet clientg_pClient = new NatNetClient();// set the frame callback handler，在这里绑定CallBack函数并把this指针输入进去g_pClient->SetFrameReceivedCallback(DataHandler, this); // this function will receive data from the serverprintf("Start looking for servers on the local network.\n");// try to find the serverNatNetDiscoveryHandle discovery;NatNet_CreateAsyncServerDiscovery(&discovery, ServerDiscoveredCallback);// build the connection parametersunsigned int serverIndex = 0;// Make sure at least one server is foundwhile (g_discoveredServers.size() < 1) {std::cout << "searching for Server" << std::endl;}/*Try to connect to the server*/const sNatNetDiscoveredServer& discoveredServer =g_discoveredServers[serverIndex];if (discoveredServer.serverDescription.bConnectionInfoValid) {// Build the connection parameters.
#ifdef _WIN32_snprintf_s(
#elsesnprintf(
#endifg_discoveredMulticastGroupAddr,sizeof g_discoveredMulticastGroupAddr,"%" PRIu8 ".%" PRIu8 ".%" PRIu8 ".%" PRIu8 "",discoveredServer.serverDescription.ConnectionMulticastAddress[0],discoveredServer.serverDescription.ConnectionMulticastAddress[1],discoveredServer.serverDescription.ConnectionMulticastAddress[2],discoveredServer.serverDescription.ConnectionMulticastAddress[3]);g_connectParams.connectionType =discoveredServer.serverDescription.ConnectionMulticast? ConnectionType_Multicast: ConnectionType_Unicast;g_connectParams.serverCommandPort = discoveredServer.serverCommandPort;g_connectParams.serverDataPort =discoveredServer.serverDescription.ConnectionDataPort;g_connectParams.serverAddress = discoveredServer.serverAddress;g_connectParams.localAddress = discoveredServer.localAddress;g_connectParams.multicastAddress = g_discoveredMulticastGroupAddr;}else {// We're missing some info because it's a legacy server.// Guess the defaults and make a best effort attempt to connect.g_connectParams.connectionType = kDefaultConnectionType;g_connectParams.serverCommandPort = discoveredServer.serverCommandPort;g_connectParams.serverDataPort = 0;g_connectParams.serverAddress = discoveredServer.serverAddress;g_connectParams.localAddress = discoveredServer.localAddress;g_connectParams.multicastAddress = NULL;}NatNet_FreeAsyncServerDiscovery(discovery);int iResult;// Connect to MotiveiResult = ConnectClient();if (iResult != ErrorCode_OK) {printf("Error initializing client.  See log for details.  Exiting");return 1;}else {printf("Client initialized and ready.\n");}// Send/receive Contextvoid* response;int nBytes;printf("[SampleClient] Sending Test Request\n");iResult = g_pClient->SendMessageAndWait("TestRequest", &response, &nBytes);if (iResult == ErrorCode_OK) {printf("[SampleClient] Received: %s", (char*)response);}// Retrieve Data Descriptions from Motiveprintf("\n\n[SampleClient] Requesting Data Descriptions...");sDataDescriptions* pDataDefs = NULL;iResult = g_pClient->GetDataDescriptionList(&pDataDefs);if (iResult != ErrorCode_OK || pDataDefs == NULL) {printf("[SampleClient] Unable to retrieve Data Descriptions.");return -2;}else {printf("[SampleClient] Received %d Data Descriptions:\n",pDataDefs->nDataDescriptions);for (int i = 0; i < pDataDefs->nDataDescriptions; i++) {if (pDataDefs->arrDataDescriptions[i].type ==Descriptor_RigidBody) {// RigidBodysRigidBodyDescription* pRB =pDataDefs->arrDataDescriptions[i].Data.RigidBodyDescription;printf("RigidBody Name : %s\n", pRB->szName);printf("RigidBody ID : %d\n", pRB->ID);printf("RigidBody Parent ID : %d\n", pRB->parentID);printf("Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx,pRB->offsety, pRB->offsetz);if (pRB->MarkerPositions != NULL &&pRB->MarkerRequiredLabels != NULL) {for (int markerIdx = 0; markerIdx < pRB->nMarkers;++markerIdx) {const MarkerData& markerPosition =pRB->MarkerPositions[markerIdx];const int markerRequiredLabel =pRB->MarkerRequiredLabels[markerIdx];printf("\tMarker #%d:\n", markerIdx);printf("\t\tPosition: %.2f, %.2f, %.2f\n",markerPosition[0], markerPosition[1],markerPosition[2]);if (markerRequiredLabel != 0) {printf("\t\tRequired active label: %d\n",markerRequiredLabel);}}}}else if (pDataDefs->arrDataDescriptions[i].type ==Descriptor_Skeleton) {// SkeletonsSkeletonDescription* pSK =pDataDefs->arrDataDescriptions[i].Data.SkeletonDescription;printf("Skeleton Name : %s\n", pSK->szName);printf("Skeleton ID : %d\n", pSK->skeletonID);printf("RigidBody (Bone) Count : %d\n", pSK->nRigidBodies);for (int j = 0; j < pSK->nRigidBodies; j++) {sRigidBodyDescription* pRB = &pSK->RigidBodies[j];printf("  RigidBody Name : %s\n", pRB->szName);printf("  RigidBody ID : %d\n", pRB->ID);printf("  RigidBody Parent ID : %d\n", pRB->parentID);printf("  Parent Offset : %3.2f,%3.2f,%3.2f\n",pRB->offsetx, pRB->offsety, pRB->offsetz);}}else if (pDataDefs->arrDataDescriptions[i].type ==Descriptor_Device) {// Peripheral DevicesDeviceDescription* pDevice =pDataDefs->arrDataDescriptions[i].Data.DeviceDescription;printf("Device Name : %s\n", pDevice->strName);printf("Device Serial : %s\n", pDevice->strSerialNo);printf("Device ID : %d\n", pDevice->ID);printf("Device Channel Count : %d\n", pDevice->nChannels);for (int iChannel = 0; iChannel < pDevice->nChannels;iChannel++)printf("\tChannel %d : %s\n", iChannel,pDevice->szChannelNames[iChannel]);}else {//printf("Unknown data type.");// Unknown}}}return true;
}// Deinitializing communication with Motive:
//
// return value (o): deinitialization was successful (boolean)bool vrpn_Tracker_Motive::motive_exit(void)
{// Done - clean up.if (g_pClient) {g_pClient->Disconnect();delete g_pClient;g_pClient = NULL;}return true;
}// Call mainloop:
void vrpn_Tracker_Motive::callMainLoop() {// call the generic server mainloop, since we are a server:server_mainloop();
}// Set id of VrpnTracker:void vrpn_Tracker_Motive::setId(int id){// d_sensor是父类vrpn_Tracker的成员变量，每次读入id数据写到这里就行d_sensor = id;
}// Set location of VrpnTracker:
//
// unit in metervoid vrpn_Tracker_Motive::setLoc(float x, float y, float z) { //pos是父类vrpn_Tracker的成员变量，每次读入位置数据写到这里就行pos[0] = x;pos[1] = y;pos[2] = z;
}// Set rotation of VrpnTracker:
//
// quaternion type with x,y,z wvoid vrpn_Tracker_Motive::setRot(float x, float y, float z, float w)
{// d_quat是父类vrpn_Tracker的成员变量，每次读入旋转数据写到这里就行d_quat[0] = x;d_quat[1] = y;d_quat[2] = z;d_quat[3] = w;
}// Encode And transmit the recorded datas:void vrpn_Tracker_Motive::encodeAndTrans(struct timeval timestamp)
{//调用父类d_quat的函数，将d_quat,pos以及d_sensor打包发送给vrpnif (d_connection) {char msgbuf[1000];int len = vrpn_Tracker::encode_to(msgbuf);if (d_connection->pack_message(len, timestamp, position_m_id,d_sender_id, msgbuf,vrpn_CONNECTION_RELIABLE)) //vrpn_CONNECTION_RELIABLE注意这个变量，如果设置为Low lantency，在一次发送数据量较大时会概率性丢失数据{fprintf(stderr,"vrpn_Tracker_DTrack: cannot write message: tossing.\n");}}
}// ----------------------------------------------------------------------------------------------------
// Motive process related function// MessageHandler receives NatNet error/debug messages
void NATNET_CALLCONV MessageHandler(Verbosity msgType,const char* msg)
{// Optional: Filter out debug messagesif (msgType < Verbosity_Info) {return;}printf("\n[NatNetLib]");switch (msgType) {case Verbosity_Debug:printf(" [DEBUG]");break;case Verbosity_Info:printf("  [INFO]");break;case Verbosity_Warning:printf("  [WARN]");break;case Verbosity_Error:printf(" [ERROR]");break;default:printf(" [?????]");break;}printf(": %s\n", msg);
}// DataHandler receives data from the server
// This function is called by NatNet when a frame of mocap data is available
//核心数据处理代码，循环、接收、处理、发送都在这里
void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData)
{//获取指向自己的指针vrpn_Tracker_Motive* pMotive = (vrpn_Tracker_Motive*)pUserData;//pMotive->callMainLoop();//当直接从callback函数中运行时调用//struct timeval timestamp;  //vrpn_gettimeofday(&timestamp, NULL);int i = 0;// FrameOfMocapData params//bool bIsRecording = ((data->params & 0x01) != 0);//bool bTrackedModelsChanged = ((data->params & 0x02) != 0);//if (bIsRecording) printf("RECORDING\n");//if (bTrackedModelsChanged) printf("Models Changed.\n");// Rigid Bodies//printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);for (i = 0; i < data->nRigidBodies; i++) {// params// 0x01 : bool, rigid body was successfully tracked in this framebool bTrackingValid = data->RigidBodies[i].params & 0x01;//Set the 6Dof tracking data//1.直接发送/* pMotive->setId(data->RigidBodies[i].ID); // the id less than 100 refer to a rigid bodypMotive->setLoc(data->RigidBodies[i].x, data->RigidBodies[i].y, data->RigidBodies[i].z);pMotive->setRot(data->RigidBodies[i].qx, data->RigidBodies[i].qy,data->RigidBodies[i].qz, data->RigidBodies[i].qw);//Encode and send to vrpnpMotive->encodeAndTrans(timestamp);*///2.先存起来统一发送//printf("Rigid Body Id =%d \n", data->RigidBodies[i].ID);pMotive->targetMap[data->RigidBodies[i].ID] = {data->RigidBodies[i].x,data->RigidBodies[i].y,data->RigidBodies[i].z,data->RigidBodies[i].qx,data->RigidBodies[i].qy, data->RigidBodies[i].qz,data->RigidBodies[i].qw};}// Skeletons//printf("Skeletons [Count=%d]\n", data->nSkeletons);for (i = 0; i < data->nSkeletons; i++) {sSkeletonData skData = data->Skeletons[i];/* printf("Skeleton [ID=%d  Bone count=%d]\n", skData.skeletonID,skData.nRigidBodies);*/for (int j = 0; j < skData.nRigidBodies; j++) {sRigidBodyData rbData = skData.RigidBodyData[j];// To identify the skeleton with the Id, 1xx for first skeleton,2xx for the second//为了区分刚体和人体，我在id上进行了一些处理，刚体的id就是自己的id，人体的id统一加上100*人体编号，以此区分AdjId = rbData.ID & IdOffset;//AdjId += 100 * (skData.skeletonID + 1);AdjId += 100 * (i + 1);//1.直接发送/* pMotive->setId(AdjId); pMotive->setLoc(rbData.x, rbData.y, rbData.z);pMotive->setRot(rbData.qx, rbData.qy, rbData.qz, rbData.qw);// Encode and send to vrpnpMotive->encodeAndTrans(timestamp);*///2.先存起来//printf("Skeleton Id =%d \n", AdjId);pMotive->targetMap[AdjId] = {rbData.x,          rbData.y,rbData.z,rbData.qx,rbData.qy,rbData.qz,rbData.qw};}       }// labeled markers - this includes all markers (Active, Passive, and// 'unlabeled' (markers with no asset but a PointCloud ID)bool bOccluded;     // marker was not visible (occluded) in this framebool bPCSolved;     // reported position provided by point cloud solvebool bModelSolved;  // reported position provided by model solvebool bHasModel;     // marker has an associated asset in the data streambool bUnlabeled;    // marker is 'unlabeled', but has a point cloud ID that// matches Motive PointCloud ID (In Motive 3D View)bool bActiveMarker; // marker is an actively labeled LED marker//printf("Markers [Count=%d]\n", data->nLabeledMarkers);for (i = 0; i < data->nLabeledMarkers; i++) {bOccluded = ((data->LabeledMarkers[i].params & 0x01) != 0);bPCSolved = ((data->LabeledMarkers[i].params & 0x02) != 0);bModelSolved = ((data->LabeledMarkers[i].params & 0x04) != 0);bHasModel = ((data->LabeledMarkers[i].params & 0x08) != 0);bUnlabeled = ((data->LabeledMarkers[i].params & 0x10) != 0);bActiveMarker = ((data->LabeledMarkers[i].params & 0x20) != 0);sMarker marker = data->LabeledMarkers[i];// Marker ID Scheme:// Active Markers://   ID = ActiveID, correlates to RB ActiveLabels list// Passive Markers://   If Asset with Legacy Labels//      AssetID     (Hi Word)//      MemberID   (Lo Word)//   Else//      PointCloud IDint modelID, markerID;NatNet_DecodeID(marker.ID, &modelID, &markerID);// Set the 6Dof tracking data//1.直接发/* pMotive->setId(markerID + 1000); // the id bigger than 1000 refer to a Active pointpMotive->setLoc(marker.x, marker.y, marker.z);pMotive->setRot(bOccluded, bPCSolved, bModelSolved, marker.size);// Encode and send to vrpnpMotive->encodeAndTrans(timestamp);*///2.先存起来//printf("Active Id =%d \n", markerID + 1000);pMotive->targetMap[markerID + 1000] = {marker.x,  marker.y,  marker.z,0, 0, 0, 1};}// force plates/*printf("Force Plate [Count=%d]\n", data->nForcePlates);for (int iPlate = 0; iPlate < data->nForcePlates; iPlate++) {printf("Force Plate %d\n", data->ForcePlates[iPlate].ID);for (int iChannel = 0; iChannel < data->ForcePlates[iPlate].nChannels;iChannel++) {printf("\tChannel %d:\t", iChannel);if (data->ForcePlates[iPlate].ChannelData[iChannel].nFrames == 0) {printf("\tEmpty Frame\n");}else if (data->ForcePlates[iPlate].ChannelData[iChannel].nFrames !=g_analogSamplesPerMocapFrame) {printf("\tPartial Frame [Expected:%d   Actual:%d]\n",g_analogSamplesPerMocapFrame,data->ForcePlates[iPlate].ChannelData[iChannel].nFrames);}for (int iSample = 0;iSample <data->ForcePlates[iPlate].ChannelData[iChannel].nFrames;iSample++)printf("%3.2f\t", data->ForcePlates[iPlate].ChannelData[iChannel].Values[iSample]);printf("\n");}}*/// devices for Buttons/*printf("Device [Count=%d]\n", data->nDevices);for (int iDevice = 0; iDevice < data->nDevices; iDevice++) {printf("Device %d\n", data->Devices[iDevice].ID);for (int iChannel = 0; iChannel < data->Devices[iDevice].nChannels;iChannel++) {printf("\tChannel %d:\t", iChannel);if (data->Devices[iDevice].ChannelData[iChannel].nFrames == 0) {printf("\tEmpty Frame\n");}else if (data->Devices[iDevice].ChannelData[iChannel].nFrames !=g_analogSamplesPerMocapFrame) {printf("\tPartial Frame [Expected:%d   Actual:%d]\n",g_analogSamplesPerMocapFrame,data->Devices[iDevice].ChannelData[iChannel].nFrames);}for (int iSample = 0;iSample < data->Devices[iDevice].ChannelData[iChannel].nFrames;iSample++)printf("%3.2f\t", data->Devices[iDevice].ChannelData[iChannel].Values[iSample]);printf("\n");}}*/
}void NATNET_CALLCONV ServerDiscoveredCallback(const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext)
{char serverHotkey = '.';if (g_discoveredServers.size() < 9) {serverHotkey = static_cast<char>('1' + g_discoveredServers.size());}const char* warning = "";if (pDiscoveredServer->serverDescription.bConnectionInfoValid == false) {warning = " (WARNING: Legacy server, could not autodetect settings. ""Auto-connect may not work reliably.)";}printf("[%c] %s %d.%d at %s%s\n", serverHotkey,pDiscoveredServer->serverDescription.szHostApp,pDiscoveredServer->serverDescription.HostAppVersion[0],pDiscoveredServer->serverDescription.HostAppVersion[1],pDiscoveredServer->serverAddress, warning);g_discoveredServers.push_back(*pDiscoveredServer);
}// Establish a NatNet Client connection
int vrpn_Tracker_Motive::ConnectClient()
{// Release previous serverg_pClient->Disconnect();// Init Client and connect to NatNet serverint retCode = g_pClient->Connect(g_connectParams);if (retCode != ErrorCode_OK) {printf("Unable to connect to server.  Error code: %d. Exiting",retCode);return ErrorCode_Internal;}else {// connection succeededvoid* pResult;int nBytes = 0;ErrorCode ret = ErrorCode_OK;// print server infomemset(&g_serverDescription, 0, sizeof(g_serverDescription));ret = g_pClient->GetServerDescription(&g_serverDescription);if (ret != ErrorCode_OK || !g_serverDescription.HostPresent) {printf("Unable to connect to server. Host not present. Exiting.");return 1;}printf("\n[SampleClient] Server application info:\n");printf("Application: %s (ver. %d.%d.%d.%d)\n",g_serverDescription.szHostApp,g_serverDescription.HostAppVersion[0],g_serverDescription.HostAppVersion[1],g_serverDescription.HostAppVersion[2],g_serverDescription.HostAppVersion[3]);printf("NatNet Version: %d.%d.%d.%d\n",g_serverDescription.NatNetVersion[0],g_serverDescription.NatNetVersion[1],g_serverDescription.NatNetVersion[2],g_serverDescription.NatNetVersion[3]);printf("Client IP:%s\n", g_connectParams.localAddress);printf("Server IP:%s\n", g_connectParams.serverAddress);printf("Server Name:%s\n", g_serverDescription.szHostComputerName);// get mocap frame rateret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes);if (ret == ErrorCode_OK) {float fRate = *((float*)pResult);printf("Mocap Framerate : %3.2f\n", fRate);}elseprintf("Error getting frame rate.\n");// get # of analog samples per mocap frame of dataret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame",&pResult, &nBytes);if (ret == ErrorCode_OK) {g_analogSamplesPerMocapFrame = *((int*)pResult);printf("Analog Samples Per Mocap Frame : %d\n",g_analogSamplesPerMocapFrame);}elseprintf("Error getting Analog frame rate.\n");}return ErrorCode_OK;
}void vrpn_Tracker_Motive::resetClient()
{int iSuccess;printf("\n\nre-setting Client\n\n.");iSuccess = g_pClient->Disconnect();if (iSuccess != 0) printf("error un-initting Client\n");iSuccess = g_pClient->Connect(g_connectParams);if (iSuccess != 0) printf("error re-initting Client\n");
}//#endif

2021.08.30补充，改完代码后最近测试的时候总是有卡顿的现象，排查原因的时候发现，使用vrpn_printdevice打印数据的时候数据不全，以人体为例，经常会丢失一部分骨骼数据。经过分析发现，当只输出骨骼的时候出现问题的地方每次都是第17个数据，也就是前16个数据都没问题，但是到第17个的时候就会概率性丢失。我先从vrpn发送端开始找问题，发现在发送前数据都是完好的，说明不是读取存储数据出的问题。然后我直接一次性发送21个自定义的数据，仍然存在问题，说明跟数据结构和从optitrack获取数据没有关系，同时排除了数据写入线程与数据读取线程间冲突的可能。然后，问题基本定位到发送数据的代码，也就是encodeAndTrans这个函数，一开始怀疑是不是msgbuf太小了，就把数据大小改成了10000，也没用。又看了看里面的encode_to函数，也没发现问题。最后，也就只剩下pack_message了，里面也就一个可以配置的变量，我看我选的是vrpn_CONNECTION_LOW_LATENCY，点进去看了一下有个vrpn_CONNECTION_RELIABLE，然后就怀疑是不是为了保证实时性数据没有校验，换了之后好了。得出结论，如果数据不多，mainloop里一个循环需要发送数据小于16个，可以选择低延时的参数，再多建议选择其他选项，以保证数据的完整性。

Vrpn源码浅析（三）-添加optitrack追踪设备相关推荐

Vrpn源码浅析（二）-自定义创建JoyStick设备
好记性不如烂笔头.上次简单研究了一下VRPN的代码结构,梳理了一下创建自定义VRPN设备的总体流程.本来打算研究一下自定义tracker类设备,把optitrack添加进去,但是临时需要获取飞行手柄的 ...
13个Vue3中的全局API的源码浅析汇总整理
前言不知不觉vue-next的版本已经来到了3.1.2,最近对照着源码学习vue3的全局Api,边学习边整理了下来,希望可以和大家一起进步. 我们以官方定义.用法.源码浅析三个维度来一起看看它们.下 ...
Gradle 庖丁解牛（构建源头源码浅析）
1 背景陆陆续续一年多,总是有人问 Gradle 构建,总是发现很多人用 Gradle 是迷糊状态的,于是最近准备来一个"Gradle 庖丁解牛"系列,一方面作为自己的总结,一方 ...
内核启动流程分析(四)源码浅析
目录 kernel(四)源码浅析建立工程启动简析 head.s 入口点查询处理器查询机器ID 启动MMU 其他操作 start_kernel 处理命令行分区 kernel(四)源码浅析建立 ...
【flink】Flink 1.12.2 源码浅析 : Task数据输入
1.概述转载:Flink 1.12.2 源码浅析 : Task数据输入在 Task 中,InputGate 是对输入的封装,InputGate 是和 JobGraph 中 JobEdge 一一对应 ...
【flink】Flink 1.12.2 源码浅析 :Task数据输出
1.概述转载:Flink 1.12.2 源码浅析 :Task数据输出 Stream的计算模型采用的是PUSH模式, 上游主动向下游推送数据, 上下游之间采用生产者-消费者模式, 下游收到数据触发计算 ...
【flink】Flink 1.12.2 源码浅析 : yarn-per-job模式解析从脚本到主类
1.概述转载:Flink 1.12.2 源码浅析 : yarn-per-job模式解析 [一] 可以去看原文.这里是补充专栏.请看原文 2. 前言主要针对yarn-per-job模式进行代码分析. ...
openedge-hub模块请求处理源码浅析——百度BIE边缘侧openedge项目源码阅读（2）
前言在openedge-hub模块启动源码浅析--百度BIE边缘侧openedge项目源码阅读(1)一文中浅析了openedge-hub模块的启动过程,openedge-hub为每一个连接的请求创建 ...
深入探索Android 启动优化（七） - JetPack App Startup 使用及源码浅析
本文首发我的微信公众号:徐公,想成为一名优秀的 Android 开发者,需要一份完备的知识体系,在这里,让我们一起成长,变得更好~. 前言前一阵子,写了几篇 Android 启动优化的文章,主要是 ...

Vrpn源码浅析（三）-添加optitrack追踪设备

1.获取设备数据

2.VRPN添加设备

3.小结

4.补充

Vrpn源码浅析（三）-添加optitrack追踪设备相关推荐

最新文章

热门文章