好记性不如烂笔头，之前进行了源码的简单分析并尝试添加了joystick这类包含analog以及button类型数据的设备。这次我们更近一步，尝试添加最为复杂的追踪设备。本次添加的设备为optitrack，由于需要用到人体追踪的数据，所以使用了提供的Natnet SDK进行数据接入，接入的数据包含一般刚体与人体骨骼数据两种类别（SDK也支持手柄等数据的接入，但是目前还没有需求，就没有加，以后用到再加）。
整体流程跟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 info
unsigned 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 callback
NatNet_SetLogCallback(MessageHandler);
// create NatNet client
g_pClient = new NatNetClient();
// set the frame callback handler 绑定一个Callback函数，在运行过程中，这个函数会自己启一个线程去处理，独立于主线程之外。
g_pClient->SetFrameReceivedCallback(DataHandler, g_pClient); // this function will receive data from the server
printf("Looking for servers on the local network.\n");
// try to find the server
NatNetDiscoveryHandle discovery;
NatNet_CreateAsyncServerDiscovery(&discovery, ServerDiscoveredCallback);
// build the connection parameters
unsigned int serverIndex = 0;
//Make sure at least one server is found
while (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(
#else
snprintf(
#endif
g_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 Motive
iResult = 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 Motive
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.");
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)
{
// MarkerSet
sMarkerSetDescription* 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)
{
// RigidBody
sRigidBodyDescription* 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)
{
// Skeleton
sSkeletonDescription* 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 Plate
sForcePlateDescription* 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 Device
sDeviceDescription* 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 multicast
g_connectParams.connectionType = ConnectionType_Multicast;
iResult = ConnectClient();
if (iResult == ErrorCode_OK)
printf("Client connection type changed to Multicast.\n\n");
else
printf("Error changing client connection type to Multicast.\n\n");
break;
case 'u': // change to unicast
g_connectParams.connectionType = ConnectionType_Unicast;
iResult = ConnectClient();
if (iResult == ErrorCode_OK)
printf("Client connection type changed to Unicast.\n\n");
else
printf("Error changing client connection type to Unicast.\n\n");
break;
case 'c': // connect
iResult = ConnectClient();
break;
case 'd': // disconnect
// note: applies to unicast connections only - indicates to Motive to stop sending packets to that client endpoint
iResult = 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 messages
if (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)
{
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 params
bool 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 values
int hour, minute, second, frame, subframe;
NatNet_DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char 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 frame
bool 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 frame
bool bPCSolved; // reported position provided by point cloud solve
bool bModelSolved; // reported position provided by model solve
bool bHasModel; // marker has an associated asset in the data stream
bool 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 ID
int modelID, markerID;
NatNet_DecodeID(marker.ID, &modelID, &markerID);
char szMarkerType[512];
if (bActiveMarker)
strcpy(szMarkerType, "Active");
else if (bUnlabeled)
strcpy(szMarkerType, "Unlabeled");
else
strcpy(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
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 ConnectClient()
{
// Release previous server
g_pClient->Disconnect();
// Init Client and connect to NatNet server
int 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 succeeded
void* pResult;
int nBytes = 0;
ErrorCode ret = ErrorCode_OK;
// print server info
memset(&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 rate
ret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes);
if (ret == ErrorCode_OK)
{
float fRate = *((float*)pResult);
printf("Mocap Framerate : %3.2f\n", fRate);
}
else
printf("Error getting frame rate.\n");
// get # of analog samples per mocap frame of data
ret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame", &pResult, &nBytes);
if (ret == ErrorCode_OK)
{
g_analogSamplesPerMocapFrame = *((int*)pResult);
printf("Analog Samples Per Mocap Frame : %d\n", g_analogSamplesPerMocapFrame);
}
else
printf("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里接入了。
跟之前的一样，还是那几个文件，这次是参照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 timeval
class 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_Connection
vrpn_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 Motive
if(!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 info
unsigned 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 callback
NatNet_SetLogCallback(MessageHandler);
// create NatNet client
g_pClient = new NatNetClient();
// set the frame callback handler，在这里绑定CallBack函数并把this指针输入进去
g_pClient->SetFrameReceivedCallback(DataHandler, this); // this function will receive data from the server
printf("Start looking for servers on the local network.\n");
// try to find the server
NatNetDiscoveryHandle discovery;
NatNet_CreateAsyncServerDiscovery(&discovery, ServerDiscoveredCallback);
// build the connection parameters
unsigned int serverIndex = 0;
// Make sure at least one server is found
while (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(
#else
snprintf(
#endif
g_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 Motive
iResult = 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 Motive
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.");
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) {
// RigidBody
sRigidBodyDescription* 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) {
// Skeleton
sSkeletonDescription* 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 Device
sDeviceDescription* 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 meter
void 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 w
void 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打包发送给vrpn
if (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 messages
if (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(×tamp, 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 frame
bool bTrackingValid = data->RigidBodies[i].params & 0x01;
//Set the 6Dof tracking data
pMotive->setId(data->RigidBodies[i].ID);//the id less than 100 refer to a rigid body
pMotive->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 vrpn
pMotive->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 vrpn
pMotive->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 frame
bool bPCSolved; // reported position provided by point cloud solve
bool bModelSolved; // reported position provided by model solve
bool bHasModel; // marker has an associated asset in the data stream
bool 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 ID
int modelID, markerID;
NatNet_DecodeID(marker.ID, &modelID, &markerID);
char szMarkerType[512];
if (bActiveMarker)
strcpy(szMarkerType, "Active");
else if (bUnlabeled)
strcpy(szMarkerType, "Unlabeled");
else
strcpy(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 server
g_pClient->Disconnect();
// Init Client and connect to NatNet server
int 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 succeeded
void* pResult;
int nBytes = 0;
ErrorCode ret = ErrorCode_OK;
// print server info
memset(&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 rate
ret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes);
if (ret == ErrorCode_OK) {
float fRate = *((float*)pResult);
printf("Mocap Framerate : %3.2f\n", fRate);
}
else
printf("Error getting frame rate.\n");
// get # of analog samples per mocap frame of data
ret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame",
&pResult, &nBytes);
if (ret == ErrorCode_OK) {
g_analogSamplesPerMocapFrame = *((int*)pResult);
printf("Analog Samples Per Mocap Frame : %d\n",
g_analogSamplesPerMocapFrame);
}
else
printf("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
至此就完成了追踪设备的添加，总体跟添加一个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。
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 timeval
class 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_Connection
vrpn_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 Motive
if(!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(×tamp, 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 vrpn
encodeAndTrans(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 vrpn
if (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 info
unsigned 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 callback
NatNet_SetLogCallback(MessageHandler);
// create NatNet client
g_pClient = new NatNetClient();
// set the frame callback handler，在这里绑定CallBack函数并把this指针输入进去
g_pClient->SetFrameReceivedCallback(DataHandler, this); // this function will receive data from the server
printf("Start looking for servers on the local network.\n");
// try to find the server
NatNetDiscoveryHandle discovery;
NatNet_CreateAsyncServerDiscovery(&discovery, ServerDiscoveredCallback);
// build the connection parameters
unsigned int serverIndex = 0;
// Make sure at least one server is found
while (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(
#else
snprintf(
#endif
g_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 Motive
iResult = 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 Motive
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.");
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) {
// RigidBody
sRigidBodyDescription* 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) {
// Skeleton
sSkeletonDescription* 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 Device
sDeviceDescription* 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 meter
void 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 w
void 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打包发送给vrpn
if (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 messages
if (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(×tamp, 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 frame
bool 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 body
pMotive->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 vrpn
pMotive->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 vrpn
pMotive->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 frame
bool bPCSolved; // reported position provided by point cloud solve
bool bModelSolved; // reported position provided by model solve
bool bHasModel; // marker has an associated asset in the data stream
bool 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 ID
int 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 point
pMotive->setLoc(marker.x, marker.y, marker.z);
pMotive->setRot(bOccluded, bPCSolved, bModelSolved, marker.size);
// Encode and send to vrpn
pMotive->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 server
g_pClient->Disconnect();
// Init Client and connect to NatNet server
int 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 succeeded
void* pResult;
int nBytes = 0;
ErrorCode ret = ErrorCode_OK;
// print server info
memset(&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 rate
ret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes);
if (ret == ErrorCode_OK) {
float fRate = *((float*)pResult);
printf("Mocap Framerate : %3.2f\n", fRate);
}
else
printf("Error getting frame rate.\n");
// get # of analog samples per mocap frame of data
ret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame",
&pResult, &nBytes);
if (ret == ErrorCode_OK) {
g_analogSamplesPerMocapFrame = *((int*)pResult);
printf("Analog Samples Per Mocap Frame : %d\n",
g_analogSamplesPerMocapFrame);
}
else
printf("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个，可以选择低延时的参数，再多建议选择其他选项，以保证数据的完整性。