Merge pull request #10451 from druiz17/puck-lost-tracking

Better handing when the pucks lose tracking and openvr still send you a "valid" data

plugins/openvr/src/ViveControllerManager.cpp

@@ -29,13 +29,10 @@
 #include <glm/ext.hpp>
 #include <glm/gtc/quaternion.hpp>
 
-
 #include <controllers/UserInputMapper.h>
 
 #include <controllers/StandardControls.h>
 
-#include "OpenVrHelpers.h"
-
 extern PoseData _nextSimPoseData;
 
 vr::IVRSystem* acquireOpenVrSystem();
@@ -59,6 +56,14 @@ static const int CHEST = 3;
 
 const char* ViveControllerManager::NAME { "OpenVR" };
 
+const std::map<vr::ETrackingResult, QString> TRACKING_RESULT_TO_STRING = {
+    {vr::TrackingResult_Uninitialized, QString("vr::TrackingResult_Uninitialized")},
+    {vr::TrackingResult_Calibrating_InProgress, QString("vr::TrackingResult_Calibrating_InProgess")},
+    {vr::TrackingResult_Calibrating_OutOfRange, QString("TrackingResult_Calibrating_OutOfRange")},
+    {vr::TrackingResult_Running_OK, QString("TrackingResult_Running_Ok")},
+    {vr::TrackingResult_Running_OutOfRange, QString("TrackingResult_Running_OutOfRange")}
+};
+
 static glm::mat4 computeOffset(glm::mat4 defaultToReferenceMat, glm::mat4 defaultJointMat, controller::Pose puckPose) {
     glm::mat4 poseMat = createMatFromQuatAndPos(puckPose.rotation, puckPose.translation);
     glm::mat4 referenceJointMat = defaultToReferenceMat * defaultJointMat;
@@ -69,6 +74,16 @@ static bool sortPucksYPosition(std::pair<uint32_t, controller::Pose> firstPuck,
     return (firstPuck.second.translation.y < secondPuck.second.translation.y);
 }
 
+static QString deviceTrackingResultToString(vr::ETrackingResult trackingResult) {
+    QString result;
+    auto iterator = TRACKING_RESULT_TO_STRING.find(trackingResult);
+
+    if (iterator != TRACKING_RESULT_TO_STRING.end()) {
+        return iterator->second;
+    }
+    return result;
+}
+
 bool ViveControllerManager::isSupported() const {
     return openVrSupported();
 }
@@ -147,6 +162,15 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu
     }
 }
 
+ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) {
+    createPreferences();
+
+    _configStringMap[Config::Auto] =  QString("Auto");
+    _configStringMap[Config::Feet] =  QString("Feet");
+    _configStringMap[Config::FeetAndHips] =  QString("FeetAndHips");
+    _configStringMap[Config::FeetHipsAndChest] =  QString("FeetHipsAndChest");
+}
+
 void ViveControllerManager::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) {
     _poseStateMap.clear();
     _buttonPressedMap.clear();
@@ -209,20 +233,36 @@ void ViveControllerManager::InputDevice::update(float deltaTime, const controlle
     }
 
     updateCalibratedLimbs();
+    _lastSimPoseData = _nextSimPoseData;
 }
 
 void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceIndex, const controller::InputCalibrationData& inputCalibrationData) {
     uint32_t poseIndex = controller::TRACKED_OBJECT_00 + deviceIndex;
-
+    printDeviceTrackingResultChange(deviceIndex);
     if (_system->IsTrackedDeviceConnected(deviceIndex) &&
         _system->GetTrackedDeviceClass(deviceIndex) == vr::TrackedDeviceClass_GenericTracker &&
         _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid &&
         poseIndex <= controller::TRACKED_OBJECT_15) {
 
-        // process pose
+        mat4& mat = mat4();
-        const mat4& mat = _nextSimPoseData.poses[deviceIndex];
+        vec3 linearVelocity = vec3();
-        const vec3 linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex];
+        vec3 angularVelocity = vec3();
-        const vec3 angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex];
+        // check if the device is tracking out of range, then process the correct pose depending on the result.
+        if (_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult != vr::TrackingResult_Running_OutOfRange) {
+            mat = _nextSimPoseData.poses[deviceIndex];
+            linearVelocity = _nextSimPoseData.linearVelocities[deviceIndex];
+            angularVelocity = _nextSimPoseData.angularVelocities[deviceIndex];
+        } else {
+            mat = _lastSimPoseData.poses[deviceIndex];
+            linearVelocity = _lastSimPoseData.linearVelocities[deviceIndex];
+            angularVelocity = _lastSimPoseData.angularVelocities[deviceIndex];
+
+            // make sure that we do not overwrite the pose in the _lastSimPose with incorrect data.
+            _nextSimPoseData.poses[deviceIndex] = _lastSimPoseData.poses[deviceIndex];
+            _nextSimPoseData.linearVelocities[deviceIndex] = _lastSimPoseData.linearVelocities[deviceIndex];
+            _nextSimPoseData.angularVelocities[deviceIndex] = _lastSimPoseData.angularVelocities[deviceIndex];
+
+        }
 
         controller::Pose pose(extractTranslation(mat), glmExtractRotation(mat), linearVelocity, angularVelocity);
 
@@ -455,6 +495,14 @@ enum ViveButtonChannel {
     RIGHT_APP_MENU
 };
 
+void ViveControllerManager::InputDevice::printDeviceTrackingResultChange(uint32_t deviceIndex) {
+    if (_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult != _lastSimPoseData.vrPoses[deviceIndex].eTrackingResult) {
+        qDebug() << "OpenVR: Device" << deviceIndex << "Tracking Result changed from" <<
+            deviceTrackingResultToString(_lastSimPoseData.vrPoses[deviceIndex].eTrackingResult)
+                 << "to" << deviceTrackingResultToString(_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult);
+    }
+}
+
 bool ViveControllerManager::InputDevice::checkForCalibrationEvent() {
     auto& endOfMap = _buttonPressedMap.end();
     auto& leftTrigger = _buttonPressedMap.find(controller::LT);
@@ -583,25 +631,7 @@ void ViveControllerManager::InputDevice::saveSettings() const {
 }
 
 QString ViveControllerManager::InputDevice::configToString(Config config) {
-    QString currentConfig;
+    return _configStringMap[config];
-    switch (config) {
-        case Config::Auto:
-            currentConfig = "Auto";
-            break;
-
-        case Config::Feet:
-            currentConfig = "Feet";
-            break;
-
-        case Config::FeetAndHips:
-            currentConfig = "FeetAndHips";
-            break;
-
-        case Config::FeetHipsAndChest:
-            currentConfig = "FeetHipsAndChest";
-            break;
-    }
-    return currentConfig;
 }
 
 void ViveControllerManager::InputDevice::setConfigFromString(const QString& value) {
@@ -622,7 +652,7 @@ void ViveControllerManager::InputDevice::createPreferences() {
     static const QString VIVE_PUCKS_CONFIG = "Vive Pucks Configuration";
 
     {
-        auto getter = [this]()->QString { return configToString(_preferedConfig); };
+        auto getter = [this]()->QString { return _configStringMap[_preferedConfig]; };
         auto setter = [this](const QString& value) { setConfigFromString(value); saveSettings(); };
         auto preference = new ComboBoxPreference(VIVE_PUCKS_CONFIG, "Configuration", getter, setter);
         QStringList list = (QStringList() << "Auto" << "Feet" << "FeetAndHips" << "FeetHipsAndChest");

plugins/openvr/src/ViveControllerManager.h

@@ -25,6 +25,7 @@
 #include <plugins/InputPlugin.h>
 #include <RenderArgs.h>
 #include <render/Scene.h>
+#include "OpenVrHelpers.h"
 
 namespace vr {
     class IVRSystem;
@@ -50,7 +51,7 @@ public:
 private:
     class InputDevice : public controller::InputDevice {
     public:
-        InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) { createPreferences(); }
+        InputDevice(vr::IVRSystem*& system);
     private:
         // Device functions
         controller::Input::NamedVector getAvailableInputs() const override;
@@ -76,6 +77,7 @@ private:
         void handleHeadPoseEvent(const controller::InputCalibrationData& inputCalibrationData, const mat4& mat, const vec3& linearVelocity,
                                  const vec3& angularVelocity);
         void partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPsuedoButton, int xPseudoButton, int yPseudoButton);
+        void printDeviceTrackingResultChange(uint32_t deviceIndex);
 
         class FilteredStick {
         public:
@@ -109,6 +111,8 @@ private:
         std::vector<std::pair<uint32_t, controller::Pose>> _validTrackedObjects;
         std::map<uint32_t, glm::mat4> _pucksOffset;
         std::map<int, uint32_t> _jointToPuckMap;
+        std::map<Config, QString> _configStringMap;
+        PoseData _lastSimPoseData;
         // perform an action when the InputDevice mutex is acquired.
         using Locker = std::unique_lock<std::recursive_mutex>;
         template <typename F>