From fb48763101827be70106c9c51f3b66b100528d0c Mon Sep 17 00:00:00 2001 From: Oliver Glandberger Date: Mon, 14 Sep 2026 07:13:06 +0200 Subject: [PATCH 1/2] Initial fix with added error message, updated tests. --- RTProtocol.cpp | 8 ++++- SettingsDeserializer.cpp | 30 +++++++++-------- Tests/6dParametersTests.cpp | 64 +++++++++++++++++++++++++++++++++++++ Tests/Data/6d.h | 58 +++++++++++++++++++++++++++++++++ Tests/TestUtils.h | 17 ++++++++-- 5 files changed, 159 insertions(+), 18 deletions(-) diff --git a/RTProtocol.cpp b/RTProtocol.cpp index 1e25d02..9176fc7 100644 --- a/RTProtocol.cpp +++ b/RTProtocol.cpp @@ -1742,7 +1742,13 @@ bool CRTProtocol::Read6DOFSettings(bool& bDataAvailable) } SettingsDeserializer deserializer(data, mMajorVersion, mMinorVersion); - return deserializer.Deserialize6DOFSettings(m6DOFSettings, mGeneralSettings, bDataAvailable); + if (!deserializer.Deserialize6DOFSettings(m6DOFSettings, mGeneralSettings, bDataAvailable)) + { + sprintf(mErrorStr, "Failed to parse 6D settings for protocol version %d.%d.", mMajorVersion, mMinorVersion); + return false; + } + + return true; } bool CRTProtocol::ReadGazeVectorSettings(bool& bDataAvailable) diff --git a/SettingsDeserializer.cpp b/SettingsDeserializer.cpp index f674c5a..f83400a 100644 --- a/SettingsDeserializer.cpp +++ b/SettingsDeserializer.cpp @@ -1197,9 +1197,9 @@ namespace bool TryReadSetPointsOld(Deserializer& deserializer, std::vector& target) { target.clear(); - for (auto pointElem : ChildElementRange{deserializer, "Bone"}) + for (auto pointElem : ChildElementRange{deserializer, "Point"}) { - SBodyPoint point; + SBodyPoint point{}; if (!pointElem.TryReadElementFloat("X", point.fX)) { @@ -1231,7 +1231,8 @@ namespace && elem.TryReadElementString("Third", targetThird); } - return false; + // Optional element; complete or absent. + return true; } } @@ -1304,12 +1305,12 @@ bool SettingsDeserializer::Deserialize6DOFSettings(std::vectorFindChild("Bodies")) + if (!sixDofElem.FindChild("Bodies")) { return false; } - for (auto bodyElem : ChildElementRange{*mDeserializer, "Body"}) + for (auto bodyElem : ChildElementRange{sixDofElem, "Body"}) { SSettings6DOFBody bodySettings6Dof{}; @@ -1320,18 +1321,19 @@ bool SettingsDeserializer::Deserialize6DOFSettings(std::vector 1 || mMinorVersion > 15) + settings6Dof.push_back(bodySettings6Dof); + } + + if (mMajorVersion > 1 || mMinorVersion > 15) + { + if (!TryReadSetEuler(sixDofElem, generalSettings.eulerRotations[0], + generalSettings.eulerRotations[1], generalSettings.eulerRotations[2])) { - if (!TryReadSetEuler(*mDeserializer, generalSettings.eulerRotations[0], - generalSettings.eulerRotations[1], generalSettings.eulerRotations[2])) - { - return false; - } + return false; } - - settings6Dof.push_back(bodySettings6Dof); - dataAvailable = true; } + + dataAvailable = true; } return true; diff --git a/Tests/6dParametersTests.cpp b/Tests/6dParametersTests.cpp index f2a869d..71328d8 100644 --- a/Tests/6dParametersTests.cpp +++ b/Tests/6dParametersTests.cpp @@ -196,3 +196,67 @@ TEST_CASE("GetSettings6DOFTest") CHECK(VerifySettings6DOF(settings6DOF)); } + +TEST_CASE("GetSettings6DOFLegacyTest") +{ + auto [protocol, network] = utils::CreateTestContext(1, 17); + + network->PrepareResponse("GetParameters 6D", qualisys_cpp_sdk::tests::data::Get6DSettingsLegacyTest, + CRTPacket::PacketXML); + + bool dataAvailable = false; + if (!protocol->Read6DOFSettings(dataAvailable)) + { + FAIL(protocol->GetErrorString()); + } + + CHECK(dataAvailable); + + REQUIRE_EQ(2, protocol->Get6DOFBodyCount()); + CHECK_EQ("test", std::string(protocol->Get6DOFBodyName(0))); + CHECK_EQ("TestTwo", std::string(protocol->Get6DOFBodyName(1))); + + // Get6DOFBodySettings is gated to 1.21+, hence the per-field accessors. + CHECK_EQ(65280, protocol->Get6DOFBodyColor(0)); + CHECK_EQ(255, protocol->Get6DOFBodyColor(1)); + + REQUIRE_EQ(2, protocol->Get6DOFBodyPointCount(0)); + REQUIRE_EQ(1, protocol->Get6DOFBodyPointCount(1)); + + CRTProtocol::SPoint point{}; + REQUIRE(protocol->Get6DOFBodyPoint(0, 0, point)); + CHECK_EQ(-67.9271545f, point.fX); + CHECK_EQ(29.8100796f, point.fY); + CHECK_EQ(-1.59297097f, point.fZ); + + REQUIRE(protocol->Get6DOFBodyPoint(0, 1, point)); + CHECK_EQ(-95.0230560f, point.fX); + + REQUIRE(protocol->Get6DOFBodyPoint(1, 0, point)); + CHECK_EQ(68.3161087f, point.fX); + + // Below 1.21 the Euler angles arrive with the 6D settings, not the general ones. + std::string first, second, third; + protocol->GetEulerAngles(first, second, third); + CHECK_EQ("Roll", first); + CHECK_EQ("Pitch", second); + CHECK_EQ("Yaw", third); +} + +TEST_CASE("GetSettings6DOFLegacyNoBodiesTest") +{ + auto [protocol, network] = utils::CreateTestContext(1, 17); + + network->PrepareResponse("GetParameters 6D", qualisys_cpp_sdk::tests::data::Get6DSettingsLegacyNoBodiesTest, + CRTPacket::PacketXML); + + bool dataAvailable = false; + if (!protocol->Read6DOFSettings(dataAvailable)) + { + FAIL(protocol->GetErrorString()); + } + + // An empty list, not a failure. + CHECK(dataAvailable); + CHECK_EQ(0, protocol->Get6DOFBodyCount()); +} diff --git a/Tests/Data/6d.h b/Tests/Data/6d.h index c68e5eb..8510926 100644 --- a/Tests/Data/6d.h +++ b/Tests/Data/6d.h @@ -153,6 +153,64 @@ namespace qualisys_cpp_sdk::tests::data +)XMLDATA"; + + // Pre-1.21 wire format, as QTM's legacy 6D writer emits it. + inline const char* Get6DSettingsLegacyTest = R"XMLDATA( + + + 2 + + test + true + 65280 + + -67.927157 + 29.810079 + -1.592971 + 0 + 0 + + + -95.023059 + 1.742391 + -19.541355 + 0 + 0 + + + + TestTwo + true + 255 + + 68.316106 + 29.173523 + -1.829020 + 0 + 0 + + + + Roll + Pitch + Yaw + + + +)XMLDATA"; + + inline const char* Get6DSettingsLegacyNoBodiesTest = R"XMLDATA( + + + 0 + + Roll + Pitch + Yaw + + + )XMLDATA"; inline const char* Set6DSettingsTest = R"XMLDATA( diff --git a/Tests/TestUtils.h b/Tests/TestUtils.h index f64b47b..14cdd4b 100644 --- a/Tests/TestUtils.h +++ b/Tests/TestUtils.h @@ -30,12 +30,14 @@ namespace qualisys_cpp_sdk::tests::utils std::stringstream stringStream; std::stringstream outputStream; std::vector messageAndResponses; + int majorVersion = MAJOR_VERSION; + int minorVersion = MINOR_VERSION; bool Connect(const char*, unsigned short) override { QueueResponse("QTM RT Interface connected", CRTPacket::EPacketType::PacketCommand); PrepareResponse("Version ", std::string{ - "Version set to " + std::to_string(MAJOR_VERSION) + "." + std::to_string(MINOR_VERSION) + "Version set to " + std::to_string(majorVersion) + "." + std::to_string(minorVersion) }, CRTPacket::EPacketType::PacketCommand); connected = true; return connected; @@ -121,6 +123,13 @@ namespace qualisys_cpp_sdk::tests::utils } public: + // Must be called before Connect, which triggers the handshake. + void SetProtocolVersion(int major, int minor) + { + majorVersion = major; + minorVersion = minor; + } + void QueueResponse(const char* str, CRTPacket::EPacketType p) { auto dataSize = static_cast(strlen(str) + 1); @@ -192,15 +201,17 @@ namespace qualisys_cpp_sdk::tests::utils DummyXmlNetwork* mNetwork; }; - inline TestContext CreateTestContext() + inline TestContext CreateTestContext(int majorVersion = MAJOR_VERSION, int minorVersion = MINOR_VERSION) { auto networkDummy = new DummyXmlNetwork{}; + networkDummy->SetProtocolVersion(majorVersion, minorVersion); auto protocol = std::make_unique(); protocol->OverrideNetwork(networkDummy->GetInterfacePtr()); - if (!protocol->Connect("")) + // Negotiation off, so the requested version is the one actually used. + if (!protocol->Connect("", CRTProtocol::cDefaultBasePort, nullptr, majorVersion, minorVersion, false, false)) { FAIL(protocol->GetErrorString()); } From fc47b8d30e2b48758b37a737f406836851294bed Mon Sep 17 00:00:00 2001 From: Oliver Glandberger Date: Mon, 14 Sep 2026 16:44:24 +0200 Subject: [PATCH 2/2] Remove new tests. --- Tests/6dParametersTests.cpp | 64 ------------------------------------- Tests/Data/6d.h | 58 --------------------------------- Tests/TestUtils.h | 17 ++-------- 3 files changed, 3 insertions(+), 136 deletions(-) diff --git a/Tests/6dParametersTests.cpp b/Tests/6dParametersTests.cpp index 71328d8..f2a869d 100644 --- a/Tests/6dParametersTests.cpp +++ b/Tests/6dParametersTests.cpp @@ -196,67 +196,3 @@ TEST_CASE("GetSettings6DOFTest") CHECK(VerifySettings6DOF(settings6DOF)); } - -TEST_CASE("GetSettings6DOFLegacyTest") -{ - auto [protocol, network] = utils::CreateTestContext(1, 17); - - network->PrepareResponse("GetParameters 6D", qualisys_cpp_sdk::tests::data::Get6DSettingsLegacyTest, - CRTPacket::PacketXML); - - bool dataAvailable = false; - if (!protocol->Read6DOFSettings(dataAvailable)) - { - FAIL(protocol->GetErrorString()); - } - - CHECK(dataAvailable); - - REQUIRE_EQ(2, protocol->Get6DOFBodyCount()); - CHECK_EQ("test", std::string(protocol->Get6DOFBodyName(0))); - CHECK_EQ("TestTwo", std::string(protocol->Get6DOFBodyName(1))); - - // Get6DOFBodySettings is gated to 1.21+, hence the per-field accessors. - CHECK_EQ(65280, protocol->Get6DOFBodyColor(0)); - CHECK_EQ(255, protocol->Get6DOFBodyColor(1)); - - REQUIRE_EQ(2, protocol->Get6DOFBodyPointCount(0)); - REQUIRE_EQ(1, protocol->Get6DOFBodyPointCount(1)); - - CRTProtocol::SPoint point{}; - REQUIRE(protocol->Get6DOFBodyPoint(0, 0, point)); - CHECK_EQ(-67.9271545f, point.fX); - CHECK_EQ(29.8100796f, point.fY); - CHECK_EQ(-1.59297097f, point.fZ); - - REQUIRE(protocol->Get6DOFBodyPoint(0, 1, point)); - CHECK_EQ(-95.0230560f, point.fX); - - REQUIRE(protocol->Get6DOFBodyPoint(1, 0, point)); - CHECK_EQ(68.3161087f, point.fX); - - // Below 1.21 the Euler angles arrive with the 6D settings, not the general ones. - std::string first, second, third; - protocol->GetEulerAngles(first, second, third); - CHECK_EQ("Roll", first); - CHECK_EQ("Pitch", second); - CHECK_EQ("Yaw", third); -} - -TEST_CASE("GetSettings6DOFLegacyNoBodiesTest") -{ - auto [protocol, network] = utils::CreateTestContext(1, 17); - - network->PrepareResponse("GetParameters 6D", qualisys_cpp_sdk::tests::data::Get6DSettingsLegacyNoBodiesTest, - CRTPacket::PacketXML); - - bool dataAvailable = false; - if (!protocol->Read6DOFSettings(dataAvailable)) - { - FAIL(protocol->GetErrorString()); - } - - // An empty list, not a failure. - CHECK(dataAvailable); - CHECK_EQ(0, protocol->Get6DOFBodyCount()); -} diff --git a/Tests/Data/6d.h b/Tests/Data/6d.h index 8510926..c68e5eb 100644 --- a/Tests/Data/6d.h +++ b/Tests/Data/6d.h @@ -153,64 +153,6 @@ namespace qualisys_cpp_sdk::tests::data -)XMLDATA"; - - // Pre-1.21 wire format, as QTM's legacy 6D writer emits it. - inline const char* Get6DSettingsLegacyTest = R"XMLDATA( - - - 2 - - test - true - 65280 - - -67.927157 - 29.810079 - -1.592971 - 0 - 0 - - - -95.023059 - 1.742391 - -19.541355 - 0 - 0 - - - - TestTwo - true - 255 - - 68.316106 - 29.173523 - -1.829020 - 0 - 0 - - - - Roll - Pitch - Yaw - - - -)XMLDATA"; - - inline const char* Get6DSettingsLegacyNoBodiesTest = R"XMLDATA( - - - 0 - - Roll - Pitch - Yaw - - - )XMLDATA"; inline const char* Set6DSettingsTest = R"XMLDATA( diff --git a/Tests/TestUtils.h b/Tests/TestUtils.h index 14cdd4b..f64b47b 100644 --- a/Tests/TestUtils.h +++ b/Tests/TestUtils.h @@ -30,14 +30,12 @@ namespace qualisys_cpp_sdk::tests::utils std::stringstream stringStream; std::stringstream outputStream; std::vector messageAndResponses; - int majorVersion = MAJOR_VERSION; - int minorVersion = MINOR_VERSION; bool Connect(const char*, unsigned short) override { QueueResponse("QTM RT Interface connected", CRTPacket::EPacketType::PacketCommand); PrepareResponse("Version ", std::string{ - "Version set to " + std::to_string(majorVersion) + "." + std::to_string(minorVersion) + "Version set to " + std::to_string(MAJOR_VERSION) + "." + std::to_string(MINOR_VERSION) }, CRTPacket::EPacketType::PacketCommand); connected = true; return connected; @@ -123,13 +121,6 @@ namespace qualisys_cpp_sdk::tests::utils } public: - // Must be called before Connect, which triggers the handshake. - void SetProtocolVersion(int major, int minor) - { - majorVersion = major; - minorVersion = minor; - } - void QueueResponse(const char* str, CRTPacket::EPacketType p) { auto dataSize = static_cast(strlen(str) + 1); @@ -201,17 +192,15 @@ namespace qualisys_cpp_sdk::tests::utils DummyXmlNetwork* mNetwork; }; - inline TestContext CreateTestContext(int majorVersion = MAJOR_VERSION, int minorVersion = MINOR_VERSION) + inline TestContext CreateTestContext() { auto networkDummy = new DummyXmlNetwork{}; - networkDummy->SetProtocolVersion(majorVersion, minorVersion); auto protocol = std::make_unique(); protocol->OverrideNetwork(networkDummy->GetInterfacePtr()); - // Negotiation off, so the requested version is the one actually used. - if (!protocol->Connect("", CRTProtocol::cDefaultBasePort, nullptr, majorVersion, minorVersion, false, false)) + if (!protocol->Connect("")) { FAIL(protocol->GetErrorString()); }