此代码中的迭代 运行 在哪里？
Where is the iteration running in this code?
我正在使用 NatNet SDK 通过 tcp 连接从摄像机接收数据。在下面的示例代码中，它可以在等待 while(c=_getchar()) 循环时使用 DataHandler 函数打印流数据。我不知道为什么代码既可以等待键盘输入，又可以同时在 DataHandler 函数中实时打印相机数据。我在多线程中没有看到代码运行。
我的第二个问题是如何在主函数的for循环中获取流数据。如果有人能告诉我如何在 main 函数中使用那个 DataHandler 来获取数据，那就太好了。
//=============================================================================
// Copyright ?2014 NaturalPoint, Inc. All Rights Reserved.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall NaturalPoint, Inc. or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//=============================================================================
/*
SampleClient.cpp
This program connects to a NatNet server, receives a data stream, and writes that data stream
to an ascii file. The purpose is to illustrate using the NatNetClient class.
Usage [optional]:
SampleClient [ServerIP] [LocalIP] [OutputFilename]
[ServerIP] IP address of the server (e.g. 192.168.0.107) ( defaults to local machine)
[OutputFilename] Name of points file (pts) to write out. defaults to Client-output.pts
*/
#include <stdio.h>
#include <tchar.h>
#include <conio.h>
#include <winsock2.h>
#include "NatNetTypes.h"
#include "NatNetClient.h"
#pragma warning( disable : 4996 )
void _WriteHeader(FILE* fp, sDataDescriptions* pBodyDefs);
void _WriteFrame(FILE* fp, sFrameOfMocapData* data);
void _WriteFooter(FILE* fp);
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData); // receives data from the server
void __cdecl MessageHandler(int msgType, char* msg); // receives NatNet error mesages
void resetClient();
int CreateClient(int iConnectionType);
unsigned int MyServersDataPort = 3130;
unsigned int MyServersCommandPort = 3131;
int iConnectionType = ConnectionType_Multicast;
//int iConnectionType = ConnectionType_Unicast;
NatNetClient* theClient;
FILE* fp;
char szMyIPAddress[128] = "";
char szServerIPAddress[128] = "";
int analogSamplesPerMocapFrame = 0;
int _tmain(int argc, _TCHAR* argv[])
{
int iResult;
// parse command line args
if(argc>1)
{
strcpy(szServerIPAddress, argv[1]); // specified on command line
printf("Connecting to server at %s...\n", szServerIPAddress);
}
else
{
strcpy(szServerIPAddress, ""); // not specified - assume server is local machine
printf("Connecting to server at LocalMachine\n");
}
if(argc>2)
{
strcpy(szMyIPAddress, argv[2]); // specified on command line
printf("Connecting from %s...\n", szMyIPAddress);
}
else
{
strcpy(szMyIPAddress, ""); // not specified - assume server is local machine
printf("Connecting from LocalMachine...\n");
}
// Create NatNet Client
iResult = CreateClient(iConnectionType);
if(iResult != ErrorCode_OK)
{
printf("Error initializing client. See log for details. Exiting");
return 1;
}
else
{
printf("Client initialized and ready.\n");
}
// send/receive test request
printf("[SampleClient] Sending Test Request\n");
void* response;
int nBytes;
iResult = theClient->SendMessageAndWait("TestRequest", &response, &nBytes);
if (iResult == ErrorCode_OK)
{
printf("[SampleClient] Received: %s", (char*)response);
}
// Retrieve Data Descriptions from server
printf("\n\n[SampleClient] Requesting Data Descriptions...");
sDataDescriptions* pDataDefs = NULL;
int nBodies = theClient->GetDataDescriptions(&pDataDefs);
if(!pDataDefs)
{
printf("[SampleClient] Unable to retrieve Data Descriptions.");
}
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);
}
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
{
printf("Unknown data type.");
// Unknown
}
}
}
// Create data file for writing received stream into
char szFile[MAX_PATH];
char szFolder[MAX_PATH];
GetCurrentDirectory(MAX_PATH, szFolder);
if(argc > 3)
sprintf(szFile, "%s\%s", szFolder, argv[3]);
else
sprintf(szFile, "%s\Client-output.pts",szFolder);
fp = fopen(szFile, "w");
if(!fp)
{
printf("error opening output file %s. Exiting.", szFile);
exit(1);
}
if(pDataDefs)
_WriteHeader(fp, pDataDefs);
// 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));
theClient->GetServerDescription(&ServerDescription);
if(!ServerDescription.HostPresent)
{
printf("Unable to connect to server. Host not present. Exiting.");
return 1;
}
break;
case 'f':
{
sFrameOfMocapData* pData = theClient->GetLastFrameOfData();
printf("Most Recent Frame: %d", pData->iFrame);
}
break;
case 'm': // change to multicast
iConnectionType = ConnectionType_Multicast;
iResult = CreateClient(iConnectionType);
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
iConnectionType = ConnectionType_Unicast;
iResult = CreateClient(iConnectionType);
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 = CreateClient(iConnectionType);
break;
case 'd' : // disconnect
// note: applies to unicast connections only - indicates to Motive to stop sending packets to that client endpoint
iResult = theClient->SendMessageAndWait("Disconnect", &response, &nBytes);
if (iResult == ErrorCode_OK)
printf("[SampleClient] Disconnected");
break;
default:
break;
}
if(bExit)
break;
}
// Done - clean up.
theClient->Uninitialize();
_WriteFooter(fp);
fclose(fp);
return ErrorCode_OK;
}
// Establish a NatNet Client connection
int CreateClient(int iConnectionType)
{
// release previous server
if(theClient)
{
theClient->Uninitialize();
delete theClient;
}
// create NatNet client
theClient = new NatNetClient(iConnectionType);
// set the callback handlers
theClient->SetVerbosityLevel(Verbosity_Warning);
theClient->SetMessageCallback(MessageHandler);
theClient->SetDataCallback( DataHandler, theClient ); // this function will receive data from the server
// [optional] use old multicast group
//theClient->SetMulticastAddress("224.0.0.1");
// print version info
unsigned char ver[4];
theClient->NatNetVersion(ver);
printf("NatNet Sample Client (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1], ver[2], ver[3]);
// Init Client and connect to NatNet server
// to use NatNet default port assignments
int retCode = theClient->Initialize(szMyIPAddress, szServerIPAddress);
// to use a different port for commands and/or data:
//int retCode = theClient->Initialize(szMyIPAddress, szServerIPAddress, MyServersCommandPort, MyServersDataPort);
if (retCode != ErrorCode_OK)
{
printf("Unable to connect to server. Error code: %d. Exiting", retCode);
return ErrorCode_Internal;
}
else
{
// get # of analog samples per mocap frame of data
void* pResult;
int ret = 0;
int nBytes = 0;
ret = theClient->SendMessageAndWait("AnalogSamplesPerMocapFrame", &pResult, &nBytes);
if (ret == ErrorCode_OK)
{
analogSamplesPerMocapFrame = *((int*)pResult);
printf("Analog Samples Per Mocap Frame : %d", analogSamplesPerMocapFrame);
}
// print server info
sServerDescription ServerDescription;
memset(&ServerDescription, 0, sizeof(ServerDescription));
theClient->GetServerDescription(&ServerDescription);
if(!ServerDescription.HostPresent)
{
printf("Unable to connect to server. Host not present. Exiting.");
return 1;
}
printf("[SampleClient] Server application info:\n");
printf("Application: %s (ver. %d.%d.%d.%d)\n", ServerDescription.szHostApp, ServerDescription.HostAppVersion[0],
ServerDescription.HostAppVersion[1],ServerDescription.HostAppVersion[2],ServerDescription.HostAppVersion[3]);
printf("NatNet Version: %d.%d.%d.%d\n", ServerDescription.NatNetVersion[0], ServerDescription.NatNetVersion[1],
ServerDescription.NatNetVersion[2], ServerDescription.NatNetVersion[3]);
printf("Client IP:%s\n", szMyIPAddress);
printf("Server IP:%s\n", szServerIPAddress);
printf("Server Name:%s\n\n", ServerDescription.szHostComputerName);
}
return ErrorCode_OK;
}
// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
NatNetClient* pClient = (NatNetClient*) pUserData;
if(fp)
_WriteFrame(fp,data);
int i=0;
printf("FrameID : %d\n", data->iFrame);
printf("Timestamp : %3.2lf\n", data->fTimestamp);
printf("Latency : %3.2lf\n", data->fLatency);
// 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;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char szTimecode[128] = "";
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
printf("Timecode : %s\n", szTimecode);
// Other Markers
printf("Other Markers [Count=%d]\n", data->nOtherMarkers);
for(i=0; i < data->nOtherMarkers; i++)
{
printf("Other Marker %d : %3.2f\t%3.2f\t%3.2f\n",
i,
data->OtherMarkers[i][0],
data->OtherMarkers[i][1],
data->OtherMarkers[i][2]);
}
// 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);
printf("\tRigid body markers [Count=%d]\n", data->RigidBodies[i].nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(data->RigidBodies[i].MarkerIDs)
printf("MarkerID:%d", data->RigidBodies[i].MarkerIDs[iMarker]);
if(data->RigidBodies[i].MarkerSizes)
printf("\tMarkerSize:%3.2f", data->RigidBodies[i].MarkerSizes[iMarker]);
if(data->RigidBodies[i].Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
// 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, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw );
printf("\tRigid body markers [Count=%d]\n", rbData.nMarkers);
for(int iMarker=0; iMarker < rbData.nMarkers; iMarker++)
{
printf("\t\t");
if(rbData.MarkerIDs)
printf("MarkerID:%d", rbData.MarkerIDs[iMarker]);
if(rbData.MarkerSizes)
printf("\tMarkerSize:%3.2f", rbData.MarkerSizes[iMarker]);
if(rbData.Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
}
// labeled markers
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
printf("Labeled 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);
sMarker marker = data->LabeledMarkers[i];
int modelID, markerID;
theClient->DecodeID(marker.ID, &modelID, &markerID);
printf("Labeled Marker [ModelID=%d, MarkerID=%d, Occluded=%d, PCSolved=%d, ModelSolved=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n",
modelID, markerID, bOccluded, bPCSolved, bModelSolved, marker.size, marker.x, marker.y, marker.z);
}
// force plates
if(data->nForcePlates==0)
{
printf("No Plates\n");
}
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 != analogSamplesPerMocapFrame)
{
printf("\tPartial Frame [Expected:%d Actual:%d]\n", 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");
}
}
}
// MessageHandler receives NatNet error/debug messages
void __cdecl MessageHandler(int msgType, char* msg)
{
printf("\n%s\n", msg);
}
/* File writing routines */
void _WriteHeader(FILE* fp, sDataDescriptions* pBodyDefs)
{
int i=0;
if(!pBodyDefs->arrDataDescriptions[0].type == Descriptor_MarkerSet)
return;
sMarkerSetDescription* pMS = pBodyDefs->arrDataDescriptions[0].Data.MarkerSetDescription;
fprintf(fp, "<MarkerSet>\n\n");
fprintf(fp, "<Name>\n%s\n</Name>\n\n", pMS->szName);
fprintf(fp, "<Markers>\n");
for(i=0; i < pMS->nMarkers; i++)
{
fprintf(fp, "%s\n", pMS->szMarkerNames[i]);
}
fprintf(fp, "</Markers>\n\n");
fprintf(fp, "<Data>\n");
fprintf(fp, "Frame#\t");
for(i=0; i < pMS->nMarkers; i++)
{
fprintf(fp, "M%dX\tM%dY\tM%dZ\t", i, i, i);
}
fprintf(fp,"\n");
}
void _WriteFrame(FILE* fp, sFrameOfMocapData* data)
{
fprintf(fp, "%d", data->iFrame);
for(int i =0; i < data->MocapData->nMarkers; i++)
{
fprintf(fp, "\t%.5f\t%.5f\t%.5f", data->MocapData->Markers[i][0], data->MocapData->Markers[i][1], data->MocapData->Markers[i][2]);
}
fprintf(fp, "\n");
}
void _WriteFooter(FILE* fp)
{
fprintf(fp, "</Data>\n\n");
fprintf(fp, "</MarkerSet>\n");
}
void resetClient()
{
int iSuccess;
printf("\n\nre-setting Client\n\n.");
iSuccess = theClient->Uninitialize();
if(iSuccess != 0)
printf("error un-initting Client\n");
iSuccess = theClient->Initialize(szMyIPAddress, szServerIPAddress);
if(iSuccess != 0)
printf("error re-initting Client\n");
}
很可能 NatNetClient() 或 client->Initialize() 正在后台创建新线程。由于我无权访问他们的代码，因此无法可靠地进行验证。
您可以在 main 中创建同步外观，并始终通过 NatNetClient::SendMessageAndWait 发送和接收消息
要么
要异步接收数据，您需要想出一种方法来轮询 stdin（用于 getch）并同时等待锁。我不确定如何在 Microsoft 控制台上执行此操作。如果你弄明白了，那么当处理程序获得控制权时，将消息从队列中取出到 link-list 并唤醒互斥锁以通知 main.
NatNet 示例分拆出一个不同的线程，用于使用 DataHandler 函数处理数据。下一行是发生这种情况的地方：
theClient->SetDataCallback( DataHandler, theClient );
我建议严格在 DataHandler 函数中进行处理。但是，如果有必要在线程之间进行通信，那么值得研究一下 C++ 11 库
#include <threads>
#include <mutex>
具体来说mutex用于在C++中锁定和解锁线程。您可以创建一个全局变量并使用 DataHandler 函数写入它，然后在您的主函数中使用该全局变量。问题是你可能同时读取和写入数据，因为 main 和 DataHandler 在不同的线程上。因此，您可以在读取或写入该全局变量时使用互斥库来锁定线程（这意味着在执行该过程时只允许该线程 运行）。
注意：NatNet 示例实际上使用 UDP 传输而不是 TCP。
我正在使用 NatNet SDK 通过 tcp 连接从摄像机接收数据。在下面的示例代码中，它可以在等待 while(c=_getchar()) 循环时使用 DataHandler 函数打印流数据。我不知道为什么代码既可以等待键盘输入，又可以同时在 DataHandler 函数中实时打印相机数据。我在多线程中没有看到代码运行。
我的第二个问题是如何在主函数的for循环中获取流数据。如果有人能告诉我如何在 main 函数中使用那个 DataHandler 来获取数据，那就太好了。
//=============================================================================
// Copyright ?2014 NaturalPoint, Inc. All Rights Reserved.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall NaturalPoint, Inc. or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//=============================================================================
/*
SampleClient.cpp
This program connects to a NatNet server, receives a data stream, and writes that data stream
to an ascii file. The purpose is to illustrate using the NatNetClient class.
Usage [optional]:
SampleClient [ServerIP] [LocalIP] [OutputFilename]
[ServerIP] IP address of the server (e.g. 192.168.0.107) ( defaults to local machine)
[OutputFilename] Name of points file (pts) to write out. defaults to Client-output.pts
*/
#include <stdio.h>
#include <tchar.h>
#include <conio.h>
#include <winsock2.h>
#include "NatNetTypes.h"
#include "NatNetClient.h"
#pragma warning( disable : 4996 )
void _WriteHeader(FILE* fp, sDataDescriptions* pBodyDefs);
void _WriteFrame(FILE* fp, sFrameOfMocapData* data);
void _WriteFooter(FILE* fp);
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData); // receives data from the server
void __cdecl MessageHandler(int msgType, char* msg); // receives NatNet error mesages
void resetClient();
int CreateClient(int iConnectionType);
unsigned int MyServersDataPort = 3130;
unsigned int MyServersCommandPort = 3131;
int iConnectionType = ConnectionType_Multicast;
//int iConnectionType = ConnectionType_Unicast;
NatNetClient* theClient;
FILE* fp;
char szMyIPAddress[128] = "";
char szServerIPAddress[128] = "";
int analogSamplesPerMocapFrame = 0;
int _tmain(int argc, _TCHAR* argv[])
{
int iResult;
// parse command line args
if(argc>1)
{
strcpy(szServerIPAddress, argv[1]); // specified on command line
printf("Connecting to server at %s...\n", szServerIPAddress);
}
else
{
strcpy(szServerIPAddress, ""); // not specified - assume server is local machine
printf("Connecting to server at LocalMachine\n");
}
if(argc>2)
{
strcpy(szMyIPAddress, argv[2]); // specified on command line
printf("Connecting from %s...\n", szMyIPAddress);
}
else
{
strcpy(szMyIPAddress, ""); // not specified - assume server is local machine
printf("Connecting from LocalMachine...\n");
}
// Create NatNet Client
iResult = CreateClient(iConnectionType);
if(iResult != ErrorCode_OK)
{
printf("Error initializing client. See log for details. Exiting");
return 1;
}
else
{
printf("Client initialized and ready.\n");
}
// send/receive test request
printf("[SampleClient] Sending Test Request\n");
void* response;
int nBytes;
iResult = theClient->SendMessageAndWait("TestRequest", &response, &nBytes);
if (iResult == ErrorCode_OK)
{
printf("[SampleClient] Received: %s", (char*)response);
}
// Retrieve Data Descriptions from server
printf("\n\n[SampleClient] Requesting Data Descriptions...");
sDataDescriptions* pDataDefs = NULL;
int nBodies = theClient->GetDataDescriptions(&pDataDefs);
if(!pDataDefs)
{
printf("[SampleClient] Unable to retrieve Data Descriptions.");
}
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);
}
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
{
printf("Unknown data type.");
// Unknown
}
}
}
// Create data file for writing received stream into
char szFile[MAX_PATH];
char szFolder[MAX_PATH];
GetCurrentDirectory(MAX_PATH, szFolder);
if(argc > 3)
sprintf(szFile, "%s\%s", szFolder, argv[3]);
else
sprintf(szFile, "%s\Client-output.pts",szFolder);
fp = fopen(szFile, "w");
if(!fp)
{
printf("error opening output file %s. Exiting.", szFile);
exit(1);
}
if(pDataDefs)
_WriteHeader(fp, pDataDefs);
// 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));
theClient->GetServerDescription(&ServerDescription);
if(!ServerDescription.HostPresent)
{
printf("Unable to connect to server. Host not present. Exiting.");
return 1;
}
break;
case 'f':
{
sFrameOfMocapData* pData = theClient->GetLastFrameOfData();
printf("Most Recent Frame: %d", pData->iFrame);
}
break;
case 'm': // change to multicast
iConnectionType = ConnectionType_Multicast;
iResult = CreateClient(iConnectionType);
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
iConnectionType = ConnectionType_Unicast;
iResult = CreateClient(iConnectionType);
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 = CreateClient(iConnectionType);
break;
case 'd' : // disconnect
// note: applies to unicast connections only - indicates to Motive to stop sending packets to that client endpoint
iResult = theClient->SendMessageAndWait("Disconnect", &response, &nBytes);
if (iResult == ErrorCode_OK)
printf("[SampleClient] Disconnected");
break;
default:
break;
}
if(bExit)
break;
}
// Done - clean up.
theClient->Uninitialize();
_WriteFooter(fp);
fclose(fp);
return ErrorCode_OK;
}
// Establish a NatNet Client connection
int CreateClient(int iConnectionType)
{
// release previous server
if(theClient)
{
theClient->Uninitialize();
delete theClient;
}
// create NatNet client
theClient = new NatNetClient(iConnectionType);
// set the callback handlers
theClient->SetVerbosityLevel(Verbosity_Warning);
theClient->SetMessageCallback(MessageHandler);
theClient->SetDataCallback( DataHandler, theClient ); // this function will receive data from the server
// [optional] use old multicast group
//theClient->SetMulticastAddress("224.0.0.1");
// print version info
unsigned char ver[4];
theClient->NatNetVersion(ver);
printf("NatNet Sample Client (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1], ver[2], ver[3]);
// Init Client and connect to NatNet server
// to use NatNet default port assignments
int retCode = theClient->Initialize(szMyIPAddress, szServerIPAddress);
// to use a different port for commands and/or data:
//int retCode = theClient->Initialize(szMyIPAddress, szServerIPAddress, MyServersCommandPort, MyServersDataPort);
if (retCode != ErrorCode_OK)
{
printf("Unable to connect to server. Error code: %d. Exiting", retCode);
return ErrorCode_Internal;
}
else
{
// get # of analog samples per mocap frame of data
void* pResult;
int ret = 0;
int nBytes = 0;
ret = theClient->SendMessageAndWait("AnalogSamplesPerMocapFrame", &pResult, &nBytes);
if (ret == ErrorCode_OK)
{
analogSamplesPerMocapFrame = *((int*)pResult);
printf("Analog Samples Per Mocap Frame : %d", analogSamplesPerMocapFrame);
}
// print server info
sServerDescription ServerDescription;
memset(&ServerDescription, 0, sizeof(ServerDescription));
theClient->GetServerDescription(&ServerDescription);
if(!ServerDescription.HostPresent)
{
printf("Unable to connect to server. Host not present. Exiting.");
return 1;
}
printf("[SampleClient] Server application info:\n");
printf("Application: %s (ver. %d.%d.%d.%d)\n", ServerDescription.szHostApp, ServerDescription.HostAppVersion[0],
ServerDescription.HostAppVersion[1],ServerDescription.HostAppVersion[2],ServerDescription.HostAppVersion[3]);
printf("NatNet Version: %d.%d.%d.%d\n", ServerDescription.NatNetVersion[0], ServerDescription.NatNetVersion[1],
ServerDescription.NatNetVersion[2], ServerDescription.NatNetVersion[3]);
printf("Client IP:%s\n", szMyIPAddress);
printf("Server IP:%s\n", szServerIPAddress);
printf("Server Name:%s\n\n", ServerDescription.szHostComputerName);
}
return ErrorCode_OK;
}
// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
NatNetClient* pClient = (NatNetClient*) pUserData;
if(fp)
_WriteFrame(fp,data);
int i=0;
printf("FrameID : %d\n", data->iFrame);
printf("Timestamp : %3.2lf\n", data->fTimestamp);
printf("Latency : %3.2lf\n", data->fLatency);
// 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;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char szTimecode[128] = "";
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
printf("Timecode : %s\n", szTimecode);
// Other Markers
printf("Other Markers [Count=%d]\n", data->nOtherMarkers);
for(i=0; i < data->nOtherMarkers; i++)
{
printf("Other Marker %d : %3.2f\t%3.2f\t%3.2f\n",
i,
data->OtherMarkers[i][0],
data->OtherMarkers[i][1],
data->OtherMarkers[i][2]);
}
// 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);
printf("\tRigid body markers [Count=%d]\n", data->RigidBodies[i].nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(data->RigidBodies[i].MarkerIDs)
printf("MarkerID:%d", data->RigidBodies[i].MarkerIDs[iMarker]);
if(data->RigidBodies[i].MarkerSizes)
printf("\tMarkerSize:%3.2f", data->RigidBodies[i].MarkerSizes[iMarker]);
if(data->RigidBodies[i].Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
// 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, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw );
printf("\tRigid body markers [Count=%d]\n", rbData.nMarkers);
for(int iMarker=0; iMarker < rbData.nMarkers; iMarker++)
{
printf("\t\t");
if(rbData.MarkerIDs)
printf("MarkerID:%d", rbData.MarkerIDs[iMarker]);
if(rbData.MarkerSizes)
printf("\tMarkerSize:%3.2f", rbData.MarkerSizes[iMarker]);
if(rbData.Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
}
// labeled markers
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
printf("Labeled 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);
sMarker marker = data->LabeledMarkers[i];
int modelID, markerID;
theClient->DecodeID(marker.ID, &modelID, &markerID);
printf("Labeled Marker [ModelID=%d, MarkerID=%d, Occluded=%d, PCSolved=%d, ModelSolved=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n",
modelID, markerID, bOccluded, bPCSolved, bModelSolved, marker.size, marker.x, marker.y, marker.z);
}
// force plates
if(data->nForcePlates==0)
{
printf("No Plates\n");
}
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 != analogSamplesPerMocapFrame)
{
printf("\tPartial Frame [Expected:%d Actual:%d]\n", 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");
}
}
}
// MessageHandler receives NatNet error/debug messages
void __cdecl MessageHandler(int msgType, char* msg)
{
printf("\n%s\n", msg);
}
/* File writing routines */
void _WriteHeader(FILE* fp, sDataDescriptions* pBodyDefs)
{
int i=0;
if(!pBodyDefs->arrDataDescriptions[0].type == Descriptor_MarkerSet)
return;
sMarkerSetDescription* pMS = pBodyDefs->arrDataDescriptions[0].Data.MarkerSetDescription;
fprintf(fp, "<MarkerSet>\n\n");
fprintf(fp, "<Name>\n%s\n</Name>\n\n", pMS->szName);
fprintf(fp, "<Markers>\n");
for(i=0; i < pMS->nMarkers; i++)
{
fprintf(fp, "%s\n", pMS->szMarkerNames[i]);
}
fprintf(fp, "</Markers>\n\n");
fprintf(fp, "<Data>\n");
fprintf(fp, "Frame#\t");
for(i=0; i < pMS->nMarkers; i++)
{
fprintf(fp, "M%dX\tM%dY\tM%dZ\t", i, i, i);
}
fprintf(fp,"\n");
}
void _WriteFrame(FILE* fp, sFrameOfMocapData* data)
{
fprintf(fp, "%d", data->iFrame);
for(int i =0; i < data->MocapData->nMarkers; i++)
{
fprintf(fp, "\t%.5f\t%.5f\t%.5f", data->MocapData->Markers[i][0], data->MocapData->Markers[i][1], data->MocapData->Markers[i][2]);
}
fprintf(fp, "\n");
}
void _WriteFooter(FILE* fp)
{
fprintf(fp, "</Data>\n\n");
fprintf(fp, "</MarkerSet>\n");
}
void resetClient()
{
int iSuccess;
printf("\n\nre-setting Client\n\n.");
iSuccess = theClient->Uninitialize();
if(iSuccess != 0)
printf("error un-initting Client\n");
iSuccess = theClient->Initialize(szMyIPAddress, szServerIPAddress);
if(iSuccess != 0)
printf("error re-initting Client\n");
}
很可能 NatNetClient() 或 client->Initialize() 正在后台创建新线程。由于我无权访问他们的代码，因此无法可靠地进行验证。
您可以在 main 中创建同步外观，并始终通过 NatNetClient::SendMessageAndWait 发送和接收消息 要么 要异步接收数据，您需要想出一种方法来轮询 stdin（用于 getch）并同时等待锁。我不确定如何在 Microsoft 控制台上执行此操作。如果你弄明白了，那么当处理程序获得控制权时，将消息从队列中取出到 link-list 并唤醒互斥锁以通知 main.
NatNet 示例分拆出一个不同的线程，用于使用 DataHandler 函数处理数据。下一行是发生这种情况的地方：
theClient->SetDataCallback( DataHandler, theClient );
我建议严格在 DataHandler 函数中进行处理。但是，如果有必要在线程之间进行通信，那么值得研究一下 C++ 11 库
#include <threads>
#include <mutex>
具体来说mutex用于在C++中锁定和解锁线程。您可以创建一个全局变量并使用 DataHandler 函数写入它，然后在您的主函数中使用该全局变量。问题是你可能同时读取和写入数据，因为 main 和 DataHandler 在不同的线程上。因此，您可以在读取或写入该全局变量时使用互斥库来锁定线程（这意味着在执行该过程时只允许该线程 运行）。
注意：NatNet 示例实际上使用 UDP 传输而不是 TCP。