Fix windows and add utilities to visualize frames and test the interface

CMakeLists.txt

@@ -4,7 +4,7 @@
 # This software may be modified and distributed under the terms of the
 # BSD-2-Clause license. See the accompanying LICENSE file for details.
 
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.12)
 
 project(yarp-device-openxrheadset
         LANGUAGES C CXX
@@ -70,7 +70,8 @@ find_package(YCM REQUIRED)
 find_package(YARP 3.4 COMPONENTS os sig dev math REQUIRED)
 find_package(Threads REQUIRED)
 find_package(OpenXR 1.0 REQUIRED)
-find_package(glfw3 REQUIRED)
+find_package(GLFW3 REQUIRED)
+find_package(GLEW REQUIRED) #Helps with the OpenGL configuration on Windows
 find_package(Eigen3 REQUIRED)
 
 ## This is to select the newest version of OpenGL
@@ -92,6 +93,10 @@ if(NOT CMAKE_CONFIGURATION_TYPES)
     endif()
 endif()
 
+option(BUILD_TESTING "Create tests using CMake" OFF)
+if(BUILD_TESTING)
+    enable_testing()
+endif()
 
 if (NOT "${CMAKE_SIZEOF_VOID_P}" EQUAL "8")
     message( FATAL_ERROR "Only 64 bit builds supported." )

src/CMakeLists.txt

@@ -5,3 +5,5 @@
 # BSD-2-Clause license. See the accompanying LICENSE file for details.
 
 add_subdirectory(devices)
+add_subdirectory(utils)
+

src/devices/openxrheadset/CMakeLists.txt

@@ -4,7 +4,7 @@
 # This software may be modified and distributed under the terms of the
 # BSD-2-Clause license. See the accompanying LICENSE file for details.
 
-set(DEPENDS_STRING "OpenXR_FOUND;glfw3_FOUND;OpenGL_FOUND; Eigen3_FOUND")
+set(DEPENDS_STRING "OpenXR_FOUND;GLFW3_FOUND;GLEW_FOUND;OpenGL_FOUND; Eigen3_FOUND")
 
 if (NOT WIN32)
     string(APPEND DEPENDS_STRING ";X11_FOUND")
@@ -59,6 +59,7 @@ target_link_libraries(yarp_openxrheadset
     YARP::YARP_math
     OpenXR::openxr_loader
     glfw
+    GLEW::GLEW
     OpenGL::GL
     Eigen3::Eigen
 )
@@ -69,6 +70,35 @@ endif()
 
 target_include_directories(yarp_openxrheadset PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
+target_compile_features(yarp_openxrheadset PUBLIC cxx_std_20) #C++20 is used for the designated initialization of structs
+
+if(BUILD_TESTING)
+
+    add_executable(openxrheadset_test test/main.cpp ${yarp_openxrheadset_SRCS}  ${yarp_openxrheadset_HDRS})
+
+    target_link_libraries(openxrheadset_test
+        PRIVATE
+        ${YARP_LIBRARIES}
+        OpenXR::openxr_loader
+        glfw
+        GLEW::GLEW
+        OpenGL::GL
+        Eigen3::Eigen
+        )
+
+    if (NOT WIN32)
+        target_link_libraries(openxrheadset_test PRIVATE ${X11_LIBRARIES})
+    endif()
+
+    target_include_directories(openxrheadset_test PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+    target_compile_features(yarp_openxrheadset PUBLIC cxx_std_20) #C++20 is used for the designated initialization of structs
+
+    add_test(NAME openxrheadset_test COMMAND openxrheadset_test)
+
+endif()
+
+
 yarp_install(
   TARGETS yarp_openxrheadset
   EXPORT yarp-device-openxrheadset

src/devices/openxrheadset/OpenGLConfig.h

@@ -11,20 +11,23 @@
 
 #define GL_GLEXT_PROTOTYPES
 #define GL3_PROTOTYPES
+#define NOMINMAX //On windows this avoids issues when using min and max
 
+#include <GL/glew.h>
 #include <GL/gl.h>
-#include <GL/glext.h>
 
 #if defined(_WIN32)
  #define GLFW_EXPOSE_NATIVE_WIN32
  #define GLFW_EXPOSE_NATIVE_WGL
 #elif defined(__APPLE__)
  #define GLFW_EXPOSE_NATIVE_COCOA
  #define GLFW_EXPOSE_NATIVE_NSGL
+#include <GL/glext.h>
  #include <GL/glx.h>
 #elif defined(__linux__)
  #define GLFW_EXPOSE_NATIVE_X11
  #define GLFW_EXPOSE_NATIVE_GLX
+#include <GL/glext.h>
  #include <GL/glx.h>
 #endif
 

src/devices/openxrheadset/OpenXrHeadset.cpp

@@ -98,6 +98,7 @@ bool yarp::dev::OpenXrHeadset::FramePorts::open(const std::string &name, const s
     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;
@@ -112,6 +113,8 @@ bool yarp::dev::OpenXrHeadset::FramePorts::open(const std::string &name, const s
 
 void yarp::dev::OpenXrHeadset::FramePorts::close()
 {
+    m_localPoseValid = false;
+
     //Closing and deleting ports
     std::initializer_list<yarp::os::BufferedPort<yarp::os::Bottle>**> ports =
     {
@@ -139,11 +142,28 @@ void yarp::dev::OpenXrHeadset::FramePorts::publishFrame(const OpenXrInterface::P
     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) << "Publishin last" << m_name << "known pose.";
+                m_lastWarning["republish"] = yarp::os::Time::now();
+            }
+        }
+    }
 
     if (pose.positionValid)
     {
@@ -378,6 +398,12 @@ bool yarp::dev::OpenXrHeadset::threadInit()
         gui.layer.setPosition({gui.x, gui.y, gui.z});
     }
 
+    for (size_t i = 0; i < 10 && openXrInterface.isRunning(); ++i)
+    {
+        run(); //dry run. This is to make sure that the number of buttons is correctly retrieved by the JoypadControlServer
+        yarp::os::Time::delay(this->getPeriod());
+    }
+
     return true;
 }
 

src/devices/openxrheadset/OpenXrHeadset.h

@@ -10,6 +10,7 @@
 #define YARP_DEV_OPENXRHEADSET_H
 
 #include <vector>
+#include <array>
 #include <atomic>
 #include <mutex>
 #include <unordered_map>
@@ -104,6 +105,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
         std::unordered_map<const char*, double> m_lastWarning;
 
         yarp::sig::Matrix m_localPose;
+        bool m_localPoseValid{false};
 
 
     public: