Merge pull request #18 from ami-iit/trackers

Add the possibility to use the trackers in OpenXR

CMakeLists.txt

@@ -86,7 +86,7 @@ endif()
 find_package(YCM REQUIRED)
 find_package(YARP 3.4 COMPONENTS os sig dev math idl_tools REQUIRED)
 find_package(Threads REQUIRED)
-find_package(OpenXR 1.0 REQUIRED)
+find_package(OpenXR 1.0.20 REQUIRED)
 find_package(GLFW3 REQUIRED)
 find_package(GLEW REQUIRED) #Helps with the OpenGL configuration on Windows
 find_package(Eigen3 REQUIRED)

src/devices/openxrheadset/AdditionalPosesPublisher.cpp

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2022 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 <AdditionalPosesPublisher.h>
+#include <OpenXrHeadsetLogComponent.h>
+#include <OpenXrYarpUtilities.h>
+#include <yarp/os/LogStream.h>
+
+void AdditionalPosesPublisher::initialize(yarp::dev::IFrameTransform *tfPublisher, const std::vector<Label>& labels, const std::string &rootFrame)
+{
+    m_tfPublisher = tfPublisher;
+    m_rootFrame = rootFrame;
+
+    for (auto& label : labels)
+    {
+        m_additionalPoses[label.original].label = label.modified;
+    }
+}
+
+std::vector<OpenXrInterface::NamedPoseVelocity> &AdditionalPosesPublisher::inputs()
+{
+    return m_additionalPosesInputList;
+}
+
+void AdditionalPosesPublisher::publishFrames()
+{
+    for (auto& inputPose : m_additionalPosesInputList)
+    {
+        AdditionalPoseInfo& poseInfo = m_additionalPoses[inputPose.name];
+        poseInfo.active = true;
+        poseInfo.data = inputPose;
+    }
+
+    for (auto& poseIt : m_additionalPoses)
+    {
+        AdditionalPoseInfo& poseInfo = poseIt.second;
+        if (poseInfo.active)
+        {
+            if (poseInfo.data.pose.positionValid && poseInfo.data.pose.rotationValid)
+            {
+                poseInfo.lastWarningTime = 0.0;
+                poseInfo.warningCount = 0;
+
+                if (!poseInfo.publishedOnce)
+                {
+                    poseInfo.localPose.resize(4,4);
+                    poseInfo.localPose.eye();
+                    if (poseInfo.label.empty())
+                    {
+                        poseInfo.label = poseInfo.data.name;
+                    }
+                    poseInfo.publishedOnce = true;
+                }
+                poseToYarpMatrix(poseInfo.data.pose, poseInfo.localPose);
+
+                if (!m_tfPublisher->setTransform(poseInfo.label, m_rootFrame, poseInfo.localPose))
+                {
+                    yCWarning(OPENXRHEADSET) << "Failed to publish" << poseInfo.label << "frame.";
+                }
+
+                poseInfo.data.pose.positionValid = false; //We invalidate the data after use. This is to detect if some pose do not get updated.
+                poseInfo.data.pose.rotationValid = false;
+            }
+            else if (poseInfo.publishedOnce)
+            {
+                //Publish old pose
+                if (!m_tfPublisher->setTransform(poseInfo.label, m_rootFrame, poseInfo.localPose))
+                {
+                    yCWarning(OPENXRHEADSET) << "Failed to publish" << poseInfo.label << "frame.";
+                }
+
+                if (poseInfo.warningCount == 0 || yarp::os::Time::now() - poseInfo.lastWarningTime > 5.0)
+                {
+                    yCWarning(OPENXRHEADSET) << poseInfo.label << " is not valid. Publishing its last known pose.";
+                    poseInfo.lastWarningTime = yarp::os::Time::now();
+                    poseInfo.warningCount++;
+                }
+
+                if (poseInfo.warningCount > 6)
+                {
+                    yCWarning(OPENXRHEADSET) << poseInfo.label << " was not valid for 30s. Deactivated.";
+                    poseInfo.lastWarningTime = 0.0;
+                    poseInfo.warningCount = 0;
+                    poseInfo.active = false;
+                }
+            }
+        }
+    }
+}

src/devices/openxrheadset/AdditionalPosesPublisher.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2022 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_ADDITIONALPOSESPUBLISHER_H
+#define YARP_DEV_ADDITIONALPOSESPUBLISHER_H
+
+#include <OpenXrInterface.h>
+#include <yarp/dev/IFrameTransform.h>
+#include <string>
+
+class AdditionalPosesPublisher
+{
+    yarp::dev::IFrameTransform* m_tfPublisher;
+    std::string m_rootFrame;
+
+    std::vector<OpenXrInterface::NamedPoseVelocity> m_additionalPosesInputList;
+
+    struct AdditionalPoseInfo
+    {
+        std::string label;
+        double lastWarningTime{0.0};
+        size_t warningCount{0};
+        bool publishedOnce{false};
+        yarp::sig::Matrix localPose;
+        bool active{false};
+        OpenXrInterface::NamedPoseVelocity data;
+    };
+    std::unordered_map<std::string, AdditionalPoseInfo> m_additionalPoses;
+
+public:
+
+    struct Label
+    {
+        std::string original;
+        std::string modified;
+    };
+
+    void initialize(yarp::dev::IFrameTransform* tfPublisher,
+                    const std::vector<Label> &labels,
+                    const std::string& rootFrame);
+
+    std::vector<OpenXrInterface::NamedPoseVelocity>& inputs();
+
+    void publishFrames();
+
+};
+
+
+#endif // YARP_DEV_ADDITIONALPOSESPUBLISHER_H

src/devices/openxrheadset/CMakeLists.txt

@@ -48,13 +48,17 @@ set(yarp_openxrheadset_driver_SRCS
     OpenXrYarpUtilities.cpp
     SingleEyePort.cpp
     EyesManager.cpp
+    AdditionalPosesPublisher.cpp
+    FramePorts.cpp
 )
 
 set(yarp_openxrheadset_driver_HDRS
     OpenXrHeadset.h
     OpenXrYarpUtilities.h
     SingleEyePort.h
     EyesManager.h
+    AdditionalPosesPublisher.h
+    FramePorts.h
 )
 
 set (THRIFTS thrifts/OpenXrHeadsetCommands.thrift)

src/devices/openxrheadset/FramePorts.cpp

@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2022 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 <FramePorts.h>
+#include <OpenXrHeadsetLogComponent.h>
+#include <OpenXrYarpUtilities.h>
+#include <yarp/os/LogStream.h>
+
+bool FramePorts::open(const std::string &name, const std::string &portPrefix, yarp::dev::IFrameTransform *tfPublisher, const std::string &tfFrame, const std::string &rootFrame)
+{
+    //opening ports
+    std::initializer_list<std::pair<yarp::os::BufferedPort<yarp::os::Bottle>**,
+                                    std::string>> ports =
+    {
+        { &m_orientationPort,                  "quaternion"                   },
+        { &m_positionPort,                     "position"                     },
+        { &m_angularVelocityPort,              "angularVelocity"              },
+        { &m_linearVelocityPort,               "linearVelocity"               }
+    };
+
+    for (auto port : ports)
+    {
+        if (*port.first)
+        {
+            yCError(OPENXRHEADSET) << port.second <<  "is already open.";
+            continue;
+        }
+
+        std::string portName;
+
+        *port.first = new yarp::os::BufferedPort<yarp::os::Bottle>;
+        portName        = portPrefix + "/" + port.second + ":o";
+
+        if (!(*port.first)->open(portName))
+        {
+            yCError(OPENXRHEADSET) << "Cannot open" << portName << "port";
+            close();
+            return false;
+        }
+
+        (*port.first)->setWriteOnly();
+    }
+
+    double timeNow = yarp::os::Time::now();
+    m_lastWarning["quaternion"] = timeNow - 10.0;
+    m_lastWarning["position"] = timeNow - 10.0;
+    m_lastWarning["angularVelocity"] = timeNow - 10.0;
+    m_lastWarning["linearVelocity"] = timeNow - 10.0;
+    m_lastWarning["republish"] = timeNow - 10.0;
+
+    m_name = name;
+    m_tfPublisher = tfPublisher;
+    m_tfFrame = tfFrame;
+    m_rootFrame = rootFrame;
+
+    m_localPose.resize(4,4);
+    m_localPose.eye();
+
+    return true;
+}
+
+void FramePorts::close()
+{
+    m_localPoseValid = false;
+
+    //Closing and deleting ports
+    std::initializer_list<yarp::os::BufferedPort<yarp::os::Bottle>**> ports =
+    {
+        &m_orientationPort,
+        &m_positionPort,
+        &m_angularVelocityPort,
+        &m_linearVelocityPort,
+    };
+
+    for (auto port : ports)
+    {
+        if (*port)
+        {
+            (*port)->close();
+
+            delete *port;
+
+            *port = nullptr;
+        }
+    }
+}
+
+void FramePorts::publishFrame(const OpenXrInterface::Pose &pose, const OpenXrInterface::Velocity &velocity, yarp::os::Stamp &stamp)
+{
+    if (pose.positionValid && pose.rotationValid)
+    {
+        poseToYarpMatrix(pose, m_localPose);
+        m_localPoseValid = true;
+        if (!m_tfPublisher->setTransform(m_tfFrame, m_rootFrame, m_localPose))
+        {
+            yCWarning(OPENXRHEADSET) << "Failed to publish" << m_tfFrame << "frame.";
+        }
+    }
+    else
+    {
+        if (m_localPoseValid)
+        {
+            if (!m_tfPublisher->setTransform(m_tfFrame, m_rootFrame, m_localPose))
+            {
+                yCWarning(OPENXRHEADSET) << "Failed to publish" << m_tfFrame << "frame.";
+            }
+
+            if (yarp::os::Time::now() - m_lastWarning["republish"] > 1.0)
+            {
+                yCWarning(OPENXRHEADSET) << "Publishing last" << m_name << "known pose.";
+                m_lastWarning["republish"] = yarp::os::Time::now();
+            }
+        }
+    }
+
+    if (pose.positionValid)
+    {
+        writeVec3OnPort(m_positionPort, pose.position, stamp);
+    }
+    else
+    {
+        if (yarp::os::Time::now() - m_lastWarning["position"] > 5.0)
+        {
+            yCWarning(OPENXRHEADSET) << m_name << "position not valid.";
+            m_lastWarning["position"] = yarp::os::Time::now();
+        }
+    }
+
+    if (pose.rotationValid)
+    {
+        writeQuaternionOnPort(m_orientationPort, pose.rotation, stamp);
+    }
+    else
+    {
+        if (yarp::os::Time::now() - m_lastWarning["quaternion"] > 5.0)
+        {
+            yCWarning(OPENXRHEADSET) << m_name << "rotation not valid.";
+            m_lastWarning["quaternion"] = yarp::os::Time::now();
+        }
+    }
+
+    if (velocity.linearValid)
+    {
+        writeVec3OnPort(m_linearVelocityPort, velocity.linear, stamp);
+    }
+    else
+    {
+        if (yarp::os::Time::now() - m_lastWarning["linearVelocity"] > 5.0)
+        {
+            yCWarning(OPENXRHEADSET) << m_name << "linear velocity not valid.";
+            m_lastWarning["linearVelocity"] = yarp::os::Time::now();
+        }
+    }
+
+    if (velocity.angularValid)
+    {
+        writeVec3OnPort(m_angularVelocityPort, velocity.angular, stamp);
+    }
+    else
+    {
+        if (yarp::os::Time::now() - m_lastWarning["angularVelocity"] > 5.0)
+        {
+            yCWarning(OPENXRHEADSET) << m_name << "angular velocity not valid.";
+            m_lastWarning["angularVelocity"] = yarp::os::Time::now();
+        }
+    }
+}