Added possibility to have the GUI following the eyes (#42)

* Added possibility to have the GUI following the eyes * Bump version for release
ami-iit · Nov 15, 2023 · 0ae87fe · 0ae87fe
1 parent c81176c
commit 0ae87fe
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 2 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,7 +8,7 @@ cmake_minimum_required(VERSION 3.12)
 
 project(yarp-device-openxrheadset
         LANGUAGES C CXX
-        VERSION 0.0.2)
+        VERSION 0.0.3)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -140,6 +140,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
                 m_huds.back().z       = -std::max(0.01, std::abs(guip.find("z").asFloat64())); //make sure that z is negative and that is at least 0.01 in modulus
                 std::transform(groupName.begin(), groupName.end(), groupName.begin(), ::tolower);
                 m_huds.back().portName = m_prefix + "/" + guip.check("port_id", yarp::os::Value(groupName)).toString();
+                m_huds.back().followEyes = guip.check("follow_eyes") && (guip.find("follow_eyes").isNull() || guip.find("follow_eyes").asBool());
                 m_huds.back().visibility = visibility;
             }
         }
@@ -523,6 +524,26 @@ void yarp::dev::OpenXrHeadset::run()
 
         for (GuiParam& gui : m_huds)
         {
+            if (gui.followEyes)
+            {
+                Eigen::Quaternionf newRotation;
+                if (gui.visibility == IOpenXrQuadLayer::Visibility::LEFT_EYE)
+                {
+                    newRotation = m_eyesManager.getLeftEyeDesiredRotation();
+                }
+                else if (gui.visibility == IOpenXrQuadLayer::Visibility::RIGHT_EYE)
+                {
+                    newRotation = m_eyesManager.getRightEyeDesiredRotation();
+                }
+                else
+                {
+                    newRotation = averageRotation;
+                }
+
+                Eigen::Vector3f guiPosition = { gui.x, gui.y, gui.z };
+                gui.layer.setPose(newRotation * guiPosition, newRotation);
+            }
+
             if (!gui.layer.updateTexture()) {
                 yCError(OPENXRHEADSET) << "Failed to update" << gui.portName << "display texture.";
             }

diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h
@@ -264,7 +264,8 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
         float         x;
         float         y;
         float         z;
-        std::string    portName;
+        std::string   portName;
+        bool          followEyes;
         IOpenXrQuadLayer::Visibility visibility;
         ImagePortToQuadLayer<yarp::sig::ImageOf<yarp::sig::PixelRgba>> layer;
     };