Added more extensions

src/devices/openxrheadset/CMakeLists.txt

@@ -80,6 +80,7 @@ set(yarp_openxrheadset_driver_SRCS
     PosePublisher.cpp
     CustomPosePublisher.cpp
     FilteredPosePublisher.cpp
+    ExpressionsManager.cpp
 )
 
 set(yarp_openxrheadset_driver_HDRS
@@ -91,6 +92,7 @@ set(yarp_openxrheadset_driver_HDRS
     PosePublisher.h
     CustomPosePublisher.h
     FilteredPosePublisher.h
+    ExpressionsManager.h
 )
 
 set (THRIFTS thrifts/OpenXrHeadsetCommands.thrift)

src/devices/openxrheadset/ExpressionsManager.cpp

@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) 2024 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include <ExpressionsManager.h>
+
+bool ExpressionsManager::configure(const std::string& prefix, bool eyeSupported, bool lipSupported, bool gazeSupported)
+{
+    m_eyeSupported = eyeSupported;
+    m_lipSupported = lipSupported;
+    m_gazeSupported = gazeSupported;
+    m_eyeExpressionsPortName = prefix + "/expressions/eye";
+    m_lipExpressionsPortName = prefix + "/expressions/lip";
+    m_gazePortName = prefix + "/expressions/gaze";
+
+    if (m_eyeSupported)
+    {
+        if (!m_eyeExpressionsPort.open(m_eyeExpressionsPortName))
+        {
+            return false;
+        }
+    }
+
+    if (m_lipSupported)
+    {
+        if (!m_lipExpressionsPort.open(m_lipExpressionsPortName))
+        {
+            return false;
+        }
+    }
+
+    if (m_gazeSupported)
+    {
+        if (!m_gazePort.open(m_gazePortName))
+        {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+void ExpressionsManager::setExpressions(const std::vector<float>& eyeExpressions, const std::vector<float>& lipExpressions)
+{
+    if (m_eyeSupported)
+    {
+        yarp::sig::Vector& eyeExpressionsVector = m_eyeExpressionsPort.prepare();
+        eyeExpressionsVector.resize(eyeExpressions.size());
+        for (size_t i = 0; i < eyeExpressions.size(); ++i)
+        {
+            eyeExpressionsVector[i] = eyeExpressions[i];
+        }
+        m_eyeExpressionsPort.write();
+    }
+
+    if (m_lipSupported)
+    {
+        yarp::sig::Vector& lipExpressionsVector = m_lipExpressionsPort.prepare();
+        lipExpressionsVector.resize(lipExpressions.size());
+        for (size_t i = 0; i < lipExpressions.size(); ++i)
+        {
+            lipExpressionsVector[i] = lipExpressions[i];
+        }
+        m_lipExpressionsPort.write();
+    }
+}
+
+void ExpressionsManager::setGaze(const OpenXrInterface::Pose& headPose, const OpenXrInterface::Pose& gaze)
+{
+    if (!m_gazeSupported || !gaze.positionValid || !headPose.positionValid || !headPose.rotationValid)
+    {
+        return;
+    }
+
+    Eigen::Vector3f gazeDirectionInHead = headPose.rotation.inverse() * gaze.rotation * Eigen::Vector3f::UnitZ();
+
+    yarp::sig::Vector& gazeVector = m_gazePort.prepare();
+    gazeVector.resize(3);
+    gazeVector[0] = gazeDirectionInHead.x();
+    gazeVector[1] = gazeDirectionInHead.y();
+    gazeVector[2] = gazeDirectionInHead.z();
+    m_gazePort.write();
+}
+
+void ExpressionsManager::close()
+{
+    m_eyeExpressionsPort.close();
+    m_lipExpressionsPort.close();
+    m_gazePort.close();
+}
+
+std::string ExpressionsManager::getEyeExpressionsPortName() const
+{
+    return m_eyeExpressionsPortName;
+}
+
+std::string ExpressionsManager::getLipExpressionsPortName() const
+{
+    return m_lipExpressionsPortName;
+}
+
+std::string ExpressionsManager::getGazePortName() const
+{
+    return m_gazePortName;
+}

src/devices/openxrheadset/ExpressionsManager.h

@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2024 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#ifndef YARP_DEV_EXPRESSIONSMANAGER_H
+#define YARP_DEV_EXPRESSIONSMANAGER_H
+
+#include <string>
+#include <vector>
+
+#include <yarp/os/BufferedPort.h>
+#include <yarp/sig/Vector.h>
+#include <OpenXrInterface.h>
+
+class ExpressionsManager
+{
+    yarp::os::BufferedPort<yarp::sig::Vector> m_eyeExpressionsPort;
+    yarp::os::BufferedPort<yarp::sig::Vector> m_lipExpressionsPort;
+    yarp::os::BufferedPort<yarp::sig::Vector> m_gazePort;
+    bool m_eyeSupported{ false };
+    bool m_lipSupported{ false };
+    bool m_gazeSupported{ false };
+    std::string m_eyeExpressionsPortName;
+    std::string m_lipExpressionsPortName;
+    std::string m_gazePortName;
+
+public:
+
+    bool configure(const std::string& prefix, bool eyeSupported, bool lipSupported, bool gazeSupported);
+
+    void setExpressions(const std::vector<float>& eyeExpressions, const std::vector<float>& lipExpressions);
+
+    void setGaze(const OpenXrInterface::Pose& headPose, const OpenXrInterface::Pose& gaze);
+
+    void close();
+
+    std::string getEyeExpressionsPortName() const;
+
+    std::string getLipExpressionsPortName() const;
+
+    std::string getGazePortName() const;
+};
+
+
+
+#endif // YARP_DEV_EXPRESSIONSMANAGER_H

src/devices/openxrheadset/FilteredPosePublisher.cpp

@@ -146,6 +146,13 @@ void FilteredPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelo
     {
         return;
     }
-
-    PosePublisher::updateInputPose(filterJumps(input));
+    switch (input.filterType)
+    {
+        case PoseFilterType::JUMP_FILTER:
+            PosePublisher::updateInputPose(filterJumps(input));
+            break;
+        default:
+            PosePublisher::updateInputPose(input);
+            break;
+    }
 }

src/devices/openxrheadset/OpenXrHeadset.cpp

@@ -262,6 +262,11 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     m_openXrInterfaceSettings.posesPredictionInMs = cfg.check("vr_poses_prediction_in_ms", yarp::os::Value(0.0)).asFloat64();
     m_openXrInterfaceSettings.hideWindow = (m_useNativeQuadLayers && !cfg.check("hide_window")) || (cfg.check("hide_window") && (cfg.find("hide_window").isNull() || cfg.find("hide_window").asBool()));
     m_openXrInterfaceSettings.renderInPlaySpace = cfg.check("render_in_play_space") && (cfg.find("render_in_play_space").isNull() || cfg.find("render_in_play_space").asBool());
+    bool noGaze = cfg.check("no_gaze") && (cfg.find("no_gaze").isNull() || cfg.find("no_gaze").asBool());
+    m_openXrInterfaceSettings.useGaze = !noGaze;
+
+    bool noExpressions = cfg.check("no_expressions") && (cfg.find("no_expressions").isNull() || cfg.find("no_expressions").asBool());
+    m_openXrInterfaceSettings.useExpressions = !noExpressions;
 
     m_getStickAsAxis = cfg.check("stick_as_axis", yarp::os::Value(false)).asBool();
     m_rootFrame = cfg.check("tf_root_frame", yarp::os::Value("openxr_origin")).asString();
@@ -462,6 +467,12 @@ bool yarp::dev::OpenXrHeadset::threadInit()
             slide.layer.setImage(slide.options.initialSlide);
         }
 
+        // We know if the expressions are supported only after the initialization of the OpenXrInterface
+        m_expressionsManager.configure(m_prefix,
+                                       m_openXrInterface.eyeExpressionsSupported(),
+                                       m_openXrInterface.lipExpressionsSupported(),
+                                       m_openXrInterface.gazeSupported());
+
         this->yarp().attachAsServer(this->m_rpcPort);
         if (!m_rpcPort.open(m_rpcPortName))
         {
@@ -507,6 +518,7 @@ void yarp::dev::OpenXrHeadset::threadRelease()
         m_labels.clear();
         m_slides.clear();
         m_eyesManager.close();
+        m_expressionsManager.close();
 
         m_openXrInterface.close();
 
@@ -618,6 +630,9 @@ void yarp::dev::OpenXrHeadset::run()
         m_posesManager.setTransformFromRawToRootFrame(m_rootFrameRawHRootFrame);
 
         m_posesManager.publishFrames();
+
+        m_expressionsManager.setExpressions(m_openXrInterface.eyeExpressions(), m_openXrInterface.lipExpressions());
+        m_expressionsManager.setGaze(m_openXrInterface.headPose(), m_openXrInterface.gazePose());
     }
     else
     {
@@ -963,6 +978,13 @@ bool yarp::dev::OpenXrHeadset::setInterCameraDistance(const double distance)
     return m_eyesManager.setInterCameraDistance(distance);
 }
 
+double yarp::dev::OpenXrHeadset::getIPD()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    return m_openXrInterface.ipd();
+}
+
 std::string yarp::dev::OpenXrHeadset::getLeftImageControlPortName()
 {
     std::lock_guard<std::mutex> lock(m_mutex);
@@ -1060,6 +1082,42 @@ bool yarp::dev::OpenXrHeadset::restartJoypadControlServer()
     return startJoypadControlServer();
 }
 
+bool yarp::dev::OpenXrHeadset::eyeExpressionsEnabled()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_openXrInterface.eyeExpressionsSupported();
+}
+
+std::string yarp::dev::OpenXrHeadset::getEyeExpressionsPortName()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_expressionsManager.getEyeExpressionsPortName();
+}
+
+bool yarp::dev::OpenXrHeadset::lipExpressionsEnabled()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_openXrInterface.lipExpressionsSupported();
+}
+
+std::string yarp::dev::OpenXrHeadset::getLipExpressionsPortName()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_expressionsManager.getLipExpressionsPortName();
+}
+
+bool yarp::dev::OpenXrHeadset::gazeEnabled()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_openXrInterface.gazeSupported();
+}
+
+std::string yarp::dev::OpenXrHeadset::getGazePortName()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+    return m_expressionsManager.getGazePortName();
+}
+
 bool yarp::dev::OpenXrHeadset::startJoypadControlServer()
 {
     stopJoypadControlServer();

src/devices/openxrheadset/OpenXrHeadset.h

@@ -31,6 +31,7 @@
 #include <EyesManager.h>
 #include <SlideManager.h>
 #include <PosesManager.h>
+#include <ExpressionsManager.h>
 #include <thrifts/OpenXrHeadsetCommands.h>
 
 #include <Eigen/Core>
@@ -86,14 +87,14 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
     //OpenXrHeadsetCommands
     /**
      * Get the current interaction profile for the left hand
-     * It returns a string that can be one between none, khr_simple_controller, oculus_touch_controller or htc_vive_controller
+    * It returns a string that can be one between none, khr_simple_controller, oculus_touch_controller, htc_vive_controller, or htc_vive_focus3_controller
      * @return a string indicating the interaction profile in use.
      */
     virtual std::string getLeftHandInteractionProfile() override;
 
     /**
      * Get the current interaction profile for the right hand
-     * It returns a string that can be one between none, khr_simple_controller, oculus_touch_controller or htc_vive_controller
+    * It returns a string that can be one between none, khr_simple_controller, oculus_touch_controller, htc_vive_controller, or htc_vive_focus3_controller
      * @return a string indicating the interaction profile in use.
      */
     virtual std::string getRightHandInteractionProfile() override;
@@ -190,7 +191,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
 
     /**
      * Get the current lateral distance between the visualization of the robot cameras.
-     * @return The IPD in meters.
+     * @return The distance in meters.
      */
     virtual double getInterCameraDistance() override;
 
@@ -201,6 +202,12 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
      */
     virtual bool setInterCameraDistance(const double distance) override;
 
+    /**
+     * Get the current IPD (Inter Pupillary Distance) of the VR eyes.
+     * @return The IPD in meters
+     */
+    virtual double getIPD() override;
+
     /**
      * Get the name of the port trough which it is possible to control the left image.
      * @return the name of the port to control the left image.
@@ -270,6 +277,42 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
      */
     virtual bool restartJoypadControlServer() override;
 
+    /**
+     * Check if the eye expressions are enabled
+     * @return True if the eye expressions are enabled, false otherwise
+     */
+    virtual bool eyeExpressionsEnabled() override;
+
+    /**
+     * Get the name of the port trough which it is possible to get the eye expressions.
+     * @return the name of the port to get the eye expressions.
+     */
+    virtual std::string getEyeExpressionsPortName() override;
+
+    /**
+     * Check if the lip expressions are enabled
+     * @return True if the lip expressions are enabled, false otherwise
+     */
+    virtual bool lipExpressionsEnabled() override;
+
+    /**
+     * Get the name of the port trough which it is possible to get the lip expressions.
+     * @return the name of the port to get the lip expressions.
+     */
+    virtual std::string getLipExpressionsPortName() override;
+
+    /**
+     * Check if the gaze acquisition is enabled
+     * @return True if the gaze acquisition is enabled, false otherwise
+     */
+    virtual bool gazeEnabled() override;
+
+    /**
+     * Get the name of the port trough which it is possible to get the gaze position.
+     * @return the name of the port to get the gaze position.
+     */
+    virtual std::string getGazePortName() override;
+
 private:
 
     /**
@@ -323,6 +366,8 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
 
     EyesManager m_eyesManager;
 
+    ExpressionsManager m_expressionsManager;
+
     std::vector<GuiParam> m_huds;
     std::vector<LabelLayer> m_labels;
     std::vector<SlideLayer> m_slides;