Skip to content

Commit 5afe487

Browse files
EmilRosenquistEmilRosenquist
authored
Merge pull request #20 from qualisys/qtm_repo_merge
Merge latest upstream/qtm changes
2 parents fdb72f4 + 5161ed9 commit 5afe487

File tree

9 files changed

+492
-256
lines changed

9 files changed

+492
-256
lines changed

‎Network.cpp

+67-57
Original file line numberDiff line numberDiff line change
@@ -263,113 +263,123 @@ unsigned short CNetwork::GetUdpBroadcastServerPort()
263263
return GetUdpServerPort(mUDPBroadcastSocket);
264264
}
265265

266+
266267
// Receive a data packet. Data is stored in a local static buffer
267-
// Returns number of bytes in received message, 0 on timeout or -1 if there is an error.
268-
int CNetwork::Receive(char* rtDataBuff, int dataBufSize, bool header, int timeout, unsigned int *ipAddr)
268+
CNetwork::Response CNetwork::Receive(SOCKET socket, SOCKET udpSocket, char* rtDataBuff, int dataBufSize, bool header, int timeoutMicroseconds, unsigned int *ipAddr)
269269
{
270-
int recieved = 0;
270+
int received = 0;
271271
sockaddr_in source_addr;
272272
socklen_t fromlen = sizeof(source_addr);
273273

274274
fd_set readFDs, exceptFDs;
275275
FD_ZERO(&readFDs);
276276
FD_ZERO(&exceptFDs);
277277

278-
if (mSocket != INVALID_SOCKET)
278+
if (socket != INVALID_SOCKET)
279279
{
280-
FD_SET(mSocket, &readFDs);
281-
FD_SET(mSocket, &exceptFDs);
280+
FD_SET(socket, &readFDs);
281+
FD_SET(socket, &exceptFDs);
282282
}
283-
if (mUDPSocket != INVALID_SOCKET)
283+
if (udpSocket != INVALID_SOCKET)
284284
{
285-
FD_SET(mUDPSocket, &readFDs);
286-
FD_SET(mUDPSocket, &exceptFDs);
287-
}
288-
if (mUDPBroadcastSocket != INVALID_SOCKET)
289-
{
290-
FD_SET(mUDPBroadcastSocket, &readFDs);
291-
FD_SET(mUDPBroadcastSocket, &exceptFDs);
285+
FD_SET(udpSocket, &readFDs);
286+
FD_SET(udpSocket, &exceptFDs);
292287
}
293288

294289
TIMEVAL* pTimeval;
295290
TIMEVAL sTimeval;
296291

297-
if (timeout < 0)
292+
if (timeoutMicroseconds < 0)
298293
{
299294
pTimeval = nullptr;
300295
}
301296
else
302297
{
303-
sTimeval.tv_sec = timeout / 1000000;
304-
sTimeval.tv_usec = timeout % 1000000;
298+
sTimeval.tv_sec = timeoutMicroseconds / 1000000;
299+
sTimeval.tv_usec = timeoutMicroseconds % 1000000;
305300
pTimeval = &sTimeval;
306301
}
307302

308303
#ifdef _WIN32
309304
const int nfds = 0;
310305
#else
311-
const int nfds = std::max(mSocket, std::max(mUDPSocket, mUDPBroadcastSocket)) + 1;
306+
const int nfds = std::max(socket, udpSocket) + 1;
312307
#endif
313308

314309
// Wait for activity on the TCP and UDP sockets.
315310
int selectRes = select(nfds, &readFDs, nullptr, &exceptFDs, pTimeval);
311+
312+
if (selectRes == SOCKET_ERROR)
313+
{
314+
SetErrorString();
315+
return Response(CNetwork::ResponseType::error, 0);
316+
}
316317
if (selectRes == 0)
317318
{
318-
return 0; // Select timeout.
319+
return Response(CNetwork::ResponseType::timeout, 0);
319320
}
320-
if (selectRes > 0)
321+
322+
if (FD_ISSET(socket, &exceptFDs))
321323
{
322-
if (FD_ISSET(mSocket, &exceptFDs))
324+
// General socket error
325+
FD_CLR(socket, &exceptFDs);
326+
SetErrorString();
327+
return Response(CNetwork::ResponseType::error, 0);
328+
}
329+
else if (FD_ISSET(socket, &readFDs))
330+
{
331+
received = recv(socket, rtDataBuff, header ? 8 : dataBufSize, 0);
332+
FD_CLR(socket, &readFDs);
333+
if (selectRes == SOCKET_ERROR)
323334
{
324-
// General socket error
325-
FD_CLR(mSocket, &exceptFDs);
326335
SetErrorString();
327-
recieved = SOCKET_ERROR;
328-
}
329-
else if (FD_ISSET(mSocket, &readFDs))
330-
{
331-
recieved = recv(mSocket, rtDataBuff, header ? 8 : dataBufSize, 0);
332-
FD_CLR(mSocket, &readFDs);
336+
return Response(CNetwork::ResponseType::error, 0);
333337
}
334-
else if (FD_ISSET(mUDPSocket, &exceptFDs))
338+
if (received == 0)
335339
{
336-
// General socket error
337-
FD_CLR(mUDPSocket, &exceptFDs);
338-
SetErrorString();
339-
recieved = SOCKET_ERROR;
340+
return Response(CNetwork::ResponseType::disconnect, 0);
340341
}
341-
else if (FD_ISSET(mUDPSocket, &readFDs))
342+
return Response(CNetwork::ResponseType::success, received);
343+
}
344+
else if (FD_ISSET(udpSocket, &exceptFDs))
345+
{
346+
// General socket error
347+
FD_CLR(udpSocket, &exceptFDs);
348+
SetErrorString();
349+
return Response(CNetwork::ResponseType::error, 0);
350+
}
351+
else if (FD_ISSET(udpSocket, &readFDs))
352+
{
353+
received = recvfrom(udpSocket, rtDataBuff, dataBufSize, 0, (sockaddr*)&source_addr, &fromlen);
354+
FD_CLR(udpSocket, &readFDs);
355+
if (ipAddr)
342356
{
343-
recieved = recvfrom(mUDPSocket, rtDataBuff, dataBufSize, 0, (sockaddr*)&source_addr, &fromlen);
344-
FD_CLR(mUDPSocket, &readFDs);
357+
*ipAddr = source_addr.sin_addr.s_addr;
345358
}
346-
else if (FD_ISSET(mUDPBroadcastSocket, &exceptFDs))
359+
if (selectRes == SOCKET_ERROR)
347360
{
348-
// General socket error
349-
FD_CLR(mUDPBroadcastSocket, &exceptFDs);
350361
SetErrorString();
351-
recieved = SOCKET_ERROR;
362+
return Response(CNetwork::ResponseType::error, 0);
352363
}
353-
else if (FD_ISSET(mUDPBroadcastSocket, &readFDs))
364+
if (received == 0)
354365
{
355-
recieved = recvfrom(mUDPBroadcastSocket, rtDataBuff, dataBufSize, 0, (sockaddr*)&source_addr, &fromlen);
356-
FD_CLR(mUDPBroadcastSocket, &readFDs);
357-
if (ipAddr)
358-
{
359-
*ipAddr = source_addr.sin_addr.s_addr;
360-
}
366+
return Response(CNetwork::ResponseType::disconnect, 0);
361367
}
368+
return Response(CNetwork::ResponseType::success, received);
362369
}
363-
else
364-
{
365-
recieved = -1;
366-
}
370+
return Response(CNetwork::ResponseType::error, 0);
371+
}
367372

368-
if (recieved == -1)
369-
{
370-
SetErrorString();
371-
}
372-
return recieved;
373+
374+
CNetwork::Response CNetwork::Receive(char* rtDataBuff, int dataBufSize, bool header, int timeoutMicroseconds, unsigned int *ipAddr)
375+
{
376+
return Receive(mSocket, mUDPSocket, rtDataBuff, dataBufSize, header, timeoutMicroseconds, ipAddr);
377+
}
378+
379+
380+
CNetwork::Response CNetwork::ReceiveUdpBroadcast(char* rtDataBuff, int dataBufSize, int timeoutMicroseconds, unsigned int *ipAddr)
381+
{
382+
return Receive(static_cast<SOCKET>(SOCKET_ERROR), mUDPBroadcastSocket, rtDataBuff, dataBufSize, false, timeoutMicroseconds, ipAddr);
373383
}
374384

375385

‎Network.h

+24-1
Original file line numberDiff line numberDiff line change
@@ -9,16 +9,38 @@
99
#define SOCKET int
1010
#endif
1111

12+
#include <vector>
13+
1214
class CNetwork
1315
{
1416
public:
17+
enum class ResponseType
18+
{
19+
success,
20+
timeout,
21+
disconnect,
22+
error
23+
};
24+
25+
struct Response
26+
{
27+
int received;
28+
ResponseType type;
29+
30+
Response(ResponseType type_, int received_) : received(received_), type(type_) {}
31+
operator bool() { return type == ResponseType::success; }
32+
operator ResponseType() { return type; }
33+
};
34+
1535
CNetwork();
1636
~CNetwork();
1737
bool Connect(const char* pServerAddr, unsigned short nPort);
1838
void Disconnect();
1939
bool Connected() const;
2040
bool CreateUDPSocket(unsigned short &nUDPPort, bool bBroadcast = false);
21-
int Receive(char* rtDataBuff, int nDataBufSize, bool bHeader, int nTimeout, unsigned int *ipAddr = nullptr);
41+
42+
Response Receive(char* rtDataBuff, int nDataBufSize, bool bHeader, int timeoutMicroseconds, unsigned int *ipAddr = nullptr);
43+
Response ReceiveUdpBroadcast(char* rtDataBuff, int nDataBufSize, int timeoutMicroseconds, unsigned int *ipAddr = nullptr);
2244
bool Send(const char* pSendBuf, int nSize);
2345
bool SendUDPBroadcast(const char* pSendBuf, int nSize, short nPort, unsigned int nFilterAddr = 0);
2446
char* GetErrorString();
@@ -28,6 +50,7 @@ class CNetwork
2850
unsigned short GetUdpBroadcastServerPort();
2951

3052
private:
53+
Response Receive(SOCKET socket, SOCKET udpSocket, char* rtDataBuff, int nDataBufSize, bool bHeader, int timeoutMicroseconds, unsigned int *ipAddr = nullptr);
3154
bool InitWinsock();
3255
void SetErrorString();
3356
unsigned short GetUdpServerPort(SOCKET nSocket);

‎RTClientExample/Operations.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -79,13 +79,13 @@ void COperations::MonitorEvents()
7979

8080
while (mpoInput->CheckKeyPressed() == false)
8181
{
82-
int nRecv = mpoRTProtocol->ReceiveRTPacket(ePacketType, false);
82+
auto response = mpoRTProtocol->Receive(ePacketType, false);
8383

84-
if (nRecv == -1 || ePacketType == CRTPacket::PacketError)
84+
if (response == CNetwork::ResponseType::error || ePacketType == CRTPacket::PacketError)
8585
{
8686
break;
8787
}
88-
if (nRecv > 0 && ePacketType == CRTPacket::PacketEvent)
88+
if (response == CNetwork::ResponseType::success && ePacketType == CRTPacket::PacketEvent)
8989
{
9090
printf("#%d ", nEventCount++);
9191

@@ -789,7 +789,7 @@ void COperations::DataTransfer(CInput::EOperation operation)
789789
bAbort = (mpoRTProtocol->GetCurrentFrame(nComponentType, componentOptions) == false);
790790
}
791791

792-
if (mpoRTProtocol->ReceiveRTPacket(ePacketType, true) > 0)
792+
if (mpoRTProtocol->Receive(ePacketType, true) == CNetwork::ResponseType::success)
793793
{
794794
switch (ePacketType)
795795
{

‎RTClientExample/OutputSettings.cpp

+28-29
Original file line numberDiff line numberDiff line change
@@ -591,7 +591,8 @@ void COutput::Print3DSettings(CRTProtocol* poRTProtocol)
591591
for (unsigned int iLabel = 0; iLabel < nCount; iLabel++)
592592
{
593593
printf("Marker %2d: %s", iLabel + 1, poRTProtocol->Get3DLabelName(iLabel));
594-
printf(" Color: %.6X\n", poRTProtocol->Get3DLabelColor(iLabel));
594+
printf(" Color: %.6X", poRTProtocol->Get3DLabelColor(iLabel));
595+
printf(" Type: %s\n", poRTProtocol->Get3DTrajectoryType(iLabel));
595596
}
596597

597598
nCount = poRTProtocol->Get3DBoneCount();
@@ -717,6 +718,8 @@ void COutput::PrintGazeVectorSettings(CRTProtocol* poRTProtocol)
717718
printf("Gaze vector #%d\n", iVector + 1);
718719
printf(" Name: %s\n", poRTProtocol->GetGazeVectorName(iVector));
719720
printf(" Frequency: %.1f\n", poRTProtocol->GetGazeVectorFrequency(iVector));
721+
printf(" Hardware sync: %s\n", poRTProtocol->GetGazeVectorHardwareSyncUsed(iVector) ? "True" : "False");
722+
printf(" Filter: %s\n", poRTProtocol->GetGazeVectorFilterUsed(iVector) ? "True" : "False");
720723
printf("\n");
721724
}
722725
}
@@ -735,6 +738,7 @@ void COutput::PrintEyeTrackerSettings(CRTProtocol* poRTProtocol)
735738
printf("Eye tracker #%d\n", eyeTracker + 1);
736739
printf(" Name: %s\n", poRTProtocol->GetEyeTrackerName(eyeTracker));
737740
printf(" Frequency: %.1f\n", poRTProtocol->GetEyeTrackerFrequency(eyeTracker));
741+
printf(" Hardware sync: %s\n", poRTProtocol->GetGazeVectorHardwareSyncUsed(eyeTracker) ? "True" : "False");
738742
printf("\n");
739743
}
740744
}
@@ -948,15 +952,15 @@ void COutput::PrintSkeletonSettings(CRTProtocol* poRTProtocol, bool skeletonGlob
948952

949953
if ((majorVersion > 1 || minorVersion > 20) && poRTProtocol->GetSkeleton(iSkeleton, skeleton))
950954
{
951-
printf("\nSkeleton Name: %s Solver: %s Scale: %f", skeleton.name.c_str(), skeleton.solver.c_str(), skeleton.scale);
955+
printf("\nSkeleton Name: %s Scale: %f", skeleton.name.c_str(), skeleton.scale);
952956

953957
std::function<void(const CRTProtocol::SSettingsSkeletonSegmentHierarchical, uint32_t&)> recurseSegments = [&](const CRTProtocol::SSettingsSkeletonSegmentHierarchical& segment, uint32_t& level)
954958
{
955959
level++;
956960
std::string indent = std::string(level * 3, ' ');
957961

958962
printf("\n");
959-
printf("%sSegment name: %s ID: %d\n", indent.c_str(), segment.name.c_str(), segment.id);
963+
printf("%sSegment name: %s ID: %d Solver: %s\n", indent.c_str(), segment.name.c_str(), segment.id, segment.solver.c_str());
960964
printf("%s Position: %.2f, %.2f, %.2f\n", indent.c_str(), segment.position.x, segment.position.y, segment.position.z);
961965
printf("%s Rotation: %.2f, %.2f, %.2f %.2f\n", indent.c_str(), segment.rotation.x, segment.rotation.y, segment.rotation.z, segment.rotation.w);
962966

@@ -969,35 +973,30 @@ void COutput::PrintSkeletonSettings(CRTProtocol* poRTProtocol, bool skeletonGlob
969973
printf("%s Default Rotation: %.2f, %.2f, %.2f %.2f\n", indent.c_str(), segment.defaultRotation.x, segment.defaultRotation.y, segment.defaultRotation.z, segment.defaultRotation.w);
970974
}
971975

972-
if (segment.dofRotation.x.IsValid())
976+
if (!segment.degreesOfFreedom.empty())
973977
{
974-
printf("%s Degrees of freedom Rotation X: ", indent.c_str());
975-
printf("%.4f - %.4f\n", segment.dofRotation.x.lowerBound, segment.dofRotation.x.upperBound);
978+
printf("%s Degrees of freedom:\n", indent.c_str());
976979
}
977-
if (segment.dofRotation.y.IsValid())
978-
{
979-
printf("%s Degrees of freedom Rotation Y: ", indent.c_str());
980-
printf("%.4f - %.4f\n", segment.dofRotation.y.lowerBound, segment.dofRotation.y.upperBound);
981-
}
982-
if (segment.dofRotation.z.IsValid())
983-
{
984-
printf("%s Degrees of freedom Rotation Z: ", indent.c_str());
985-
printf("%.4f - %.4f\n", segment.dofRotation.z.lowerBound, segment.dofRotation.z.upperBound);
986-
}
987-
if (segment.dofTranslation.x.IsValid())
988-
{
989-
printf("%s Degrees of freedom Translation X: ", indent.c_str());
990-
printf("%.4f - %.4f\n", segment.dofTranslation.x.lowerBound, segment.dofTranslation.x.upperBound);
991-
}
992-
if (segment.dofTranslation.y.IsValid())
993-
{
994-
printf("%s Degrees of freedom Translation Y: ", indent.c_str());
995-
printf("%.4f - %.4f\n", segment.dofTranslation.y.lowerBound, segment.dofTranslation.y.upperBound);
996-
}
997-
if (segment.dofTranslation.z.IsValid())
980+
981+
for (auto dof : segment.degreesOfFreedom)
998982
{
999-
printf("%s Degrees of freedom Translation Z: ", indent.c_str());
1000-
printf("%.4f - %.4f\n", segment.dofTranslation.z.lowerBound, segment.dofTranslation.z.upperBound);
983+
if (!std::isnan(dof.lowerBound) || !std::isnan(dof.upperBound) || !dof.couplings.empty() || !std::isnan(dof.goalValue) || !std::isnan(dof.goalWeight))
984+
{
985+
printf("%s %s: ", indent.c_str(), CRTProtocol::SkeletonDofToString(dof.type));
986+
if (!std::isnan(dof.lowerBound) || !std::isnan(dof.upperBound))
987+
{
988+
printf("Constraint: %.4f, %.4f ", dof.lowerBound, dof.upperBound);
989+
}
990+
for (auto& coupling : dof.couplings)
991+
{
992+
printf("Coupling: %s, %s, %.4f ", coupling.segment.c_str(), CRTProtocol::SkeletonDofToString(coupling.degreeOfFreedom), coupling.coefficient);
993+
}
994+
if (!std::isnan(dof.goalValue) || !std::isnan(dof.goalWeight))
995+
{
996+
printf("Goal: %.4f, %.4f", dof.goalValue, dof.goalWeight);
997+
}
998+
printf("\n");
999+
}
10011000
}
10021001

10031002
if (segment.endpoint.IsValid())

‎RTClientExample/RTClientExample.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#include "Operations.h"
22

33
#define LATEST_SELECTABLE_MAJOR_VERSION 1
4-
#define LATEST_SELECTABLE_MINOR_VERSION 21
4+
#define LATEST_SELECTABLE_MINOR_VERSION 22
55
#define PROGRAM_VERSION 0
66

77
int main(int argc, char **argv)

‎RTPacket.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
#endif
1515

1616
#define MAJOR_VERSION 1
17-
#define MINOR_VERSION 21
17+
#define MINOR_VERSION 22
1818

1919
class DLL_EXPORT CRTPacket
2020
{

0 commit comments

Comments
 (0)