From 0af83293e7bf9879e221d967dfb7a42effd87223 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 14 Nov 2024 16:12:13 +0100 Subject: [PATCH 1/5] Added methods to create the JoypadControlServer device internally --- src/devices/openxrheadset/OpenXrHeadset.cpp | 128 ++++++++++++++------ src/devices/openxrheadset/OpenXrHeadset.h | 14 +++ 2 files changed, 105 insertions(+), 37 deletions(-) diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index 08a3b5d..16eb426 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.cpp +++ b/src/devices/openxrheadset/OpenXrHeadset.cpp @@ -455,16 +455,6 @@ bool yarp::dev::OpenXrHeadset::threadInit() slide.layer.setPosition({slide.x, slide.y, slide.z}); slide.layer.setImage(slide.options.initialSlide); } - } - - for (size_t i = 0; i < 10 && m_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()); - } - - { - std::lock_guard lock(m_mutex); this->yarp().attachAsServer(this->m_rpcPort); if(!m_rpcPort.open(m_rpcPortName)) @@ -474,47 +464,53 @@ bool yarp::dev::OpenXrHeadset::threadInit() } } + m_thisDevice.give(this, false); + return true; } void yarp::dev::OpenXrHeadset::threadRelease() { - std::lock_guard lock(m_mutex); + { + std::lock_guard lock(m_mutex); - if (m_closed) - return; + if (m_closed) + return; - for (auto& hud : m_huds) - { - hud.layer.close(); - } + for (auto& hud : m_huds) + { + hud.layer.close(); + } - for (auto& label : m_labels) - { - label.layer.close(); - } + for (auto& label : m_labels) + { + label.layer.close(); + } - for (auto& slide : m_slides) - { - slide.layer.close(); - } + for (auto& slide : m_slides) + { + slide.layer.close(); + } - m_huds.clear(); - m_labels.clear(); - m_slides.clear(); - m_eyesManager.close(); + m_huds.clear(); + m_labels.clear(); + m_slides.clear(); + m_eyesManager.close(); - m_openXrInterface.close(); + m_openXrInterface.close(); - if (m_tfPublisher) - { - m_driver.close(); - m_tfPublisher = nullptr; - } + if (m_tfPublisher) + { + m_driver.close(); + m_tfPublisher = nullptr; + } + + m_rpcPort.close(); - m_rpcPort.close(); + m_closed = true; + } - m_closed = true; + stopJoypadControlServer(); } void yarp::dev::OpenXrHeadset::run() @@ -1029,3 +1025,61 @@ bool yarp::dev::OpenXrHeadset::resetTransforms() m_posesManager.reset(); return true; } + +bool yarp::dev::OpenXrHeadset::startJoypadControlServer() +{ + + stopJoypadControlServer(); + + { + std::lock_guard lock(m_mutex); + + yarp::os::Property options; + options.put("device", "joypadControlServer"); + options.put("name", m_prefix); + options.put("period", getPeriod()); + options.put("use_separate_ports", true); + options.put("stick_as_axis", false); + + m_joypadControlServerPtr = std::make_unique(); + + if (!m_joypadControlServerPtr->open(options) || !m_joypadControlServerPtr->isValid()) + { + yCError(OPENXRHEADSET) << "Failed to open JoypadControlServer with the following options:" << options.toString(); + return false; + } + + + if (!m_joypadControlServerPtr->view(m_joypadControlServerWrapper) || !m_joypadControlServerWrapper) + { + yCError(OPENXRHEADSET) << "Failed to view wrapper interface in joypad control server."; + return false; + } + + if (!m_joypadControlServerWrapper->attach(&m_thisDevice)) + { + yCError(OPENXRHEADSET) << "Failed to attach the device to the joypad control server."; + + return false; + } + + } + + return true; +} + +void yarp::dev::OpenXrHeadset::stopJoypadControlServer() +{ + std::lock_guard lock(m_mutex); + + if (m_joypadControlServerWrapper) + { + m_joypadControlServerWrapper->detach(); + m_joypadControlServerWrapper = nullptr; + } + if (m_joypadControlServerPtr) + { + m_joypadControlServerPtr->close(); + } + m_joypadControlServerPtr.reset(); +} diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h index bf6336f..df887b2 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/src/devices/openxrheadset/OpenXrHeadset.h @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -262,6 +263,15 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, * as it will reset all the transforms, including the ones that are not published by this module. */ virtual bool resetTransforms() override; + /** + * Opens the joypad control server. It reopens it if already opened. + */ + bool startJoypadControlServer(); + + /** + * Closes the joypad control server. + */ + void stopJoypadControlServer(); private: @@ -332,6 +342,10 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, std::vector m_axes; std::vector m_thumbsticks; + std::unique_ptr m_joypadControlServerPtr; + yarp::dev::IWrapper* m_joypadControlServerWrapper = nullptr; + yarp::dev::PolyDriver m_thisDevice; + std::mutex m_mutex; }; From c45a68f317aabbc0381f9f154c775557ecea27f1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 14 Nov 2024 17:19:08 +0100 Subject: [PATCH 2/5] Starting the joypad control server whenever the number of inputs changes --- src/devices/openxrheadset/OpenXrHeadset.cpp | 242 ++++++++++---------- src/devices/openxrheadset/OpenXrHeadset.h | 5 +- 2 files changed, 128 insertions(+), 119 deletions(-) diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index 16eb426..bfe0fbc 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.cpp +++ b/src/devices/openxrheadset/OpenXrHeadset.cpp @@ -390,78 +390,76 @@ bool yarp::dev::OpenXrHeadset::close() bool yarp::dev::OpenXrHeadset::threadInit() { + std::lock_guard lock(m_mutex); + + if (!m_openXrInterface.initialize(m_openXrInterfaceSettings)) { - std::lock_guard lock(m_mutex); + yCError(OPENXRHEADSET) << "Failed to initialize OpenXr interface."; + return false; + } - if (!m_openXrInterface.initialize(m_openXrInterfaceSettings)) + auto getLayer = [this]() -> std::shared_ptr + { + if (m_useNativeQuadLayers) { - yCError(OPENXRHEADSET) << "Failed to initialize OpenXr interface."; - return false; + return m_openXrInterface.addHeadFixedQuadLayer(); } - - auto getLayer = [this]() -> std::shared_ptr - { - if (m_useNativeQuadLayers) - { - return m_openXrInterface.addHeadFixedQuadLayer(); - } - else - { - return m_openXrInterface.addHeadFixedOpenGLQuadLayer(); - } - }; - - m_eyesManager.options().leftEyeQuadLayer = getLayer(); - m_eyesManager.options().rightEyeQuadLayer = getLayer(); - - if (!m_eyesManager.initialize()) + else { - yCError(OPENXRHEADSET) << "Failed to initialize eyes."; + return m_openXrInterface.addHeadFixedOpenGLQuadLayer(); } + }; - for (GuiParam& gui : m_huds) - { - if (!gui.layer.initialize(getLayer(), gui.portName)) { - yCError(OPENXRHEADSET) << "Cannot initialize" << gui.portName << "display texture."; - return false; - } - gui.layer.setVisibility(gui.visibility); - gui.layer.setDimensions(gui.width, gui.height); - gui.layer.setPosition({gui.x, gui.y, gui.z}); - } + m_eyesManager.options().leftEyeQuadLayer = getLayer(); + m_eyesManager.options().rightEyeQuadLayer = getLayer(); - for (LabelLayer& label : m_labels) - { - label.options.quadLayer = getLayer(); + if (!m_eyesManager.initialize()) + { + yCError(OPENXRHEADSET) << "Failed to initialize eyes."; + } - if (!label.layer.initialize(label.options)) { - yCError(OPENXRHEADSET) << "Cannot initialize" << label.options.portName << "label."; - return false; - } - label.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); - label.layer.setDimensions(label.width, label.height); - label.layer.setPosition({label.x, label.y, label.z}); + for (GuiParam& gui : m_huds) + { + if (!gui.layer.initialize(getLayer(), gui.portName)) { + yCError(OPENXRHEADSET) << "Cannot initialize" << gui.portName << "display texture."; + return false; } + gui.layer.setVisibility(gui.visibility); + gui.layer.setDimensions(gui.width, gui.height); + gui.layer.setPosition({gui.x, gui.y, gui.z}); + } - for (SlideLayer& slide : m_slides) - { - slide.options.quadLayer = getLayer(); - if (!slide.layer.initialize(slide.options)) { - yCError(OPENXRHEADSET) << "Cannot initialize" << slide.options.portName << "slide."; - return false; - } - slide.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); - slide.layer.setDimensions(slide.width, slide.height); - slide.layer.setPosition({slide.x, slide.y, slide.z}); - slide.layer.setImage(slide.options.initialSlide); + for (LabelLayer& label : m_labels) + { + label.options.quadLayer = getLayer(); + + if (!label.layer.initialize(label.options)) { + yCError(OPENXRHEADSET) << "Cannot initialize" << label.options.portName << "label."; + return false; } + label.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); + label.layer.setDimensions(label.width, label.height); + label.layer.setPosition({label.x, label.y, label.z}); + } - this->yarp().attachAsServer(this->m_rpcPort); - if(!m_rpcPort.open(m_rpcPortName)) - { - yCError(OPENXRHEADSET) << "Could not open" << m_rpcPortName << " RPC port."; + for (SlideLayer& slide : m_slides) + { + slide.options.quadLayer = getLayer(); + if (!slide.layer.initialize(slide.options)) { + yCError(OPENXRHEADSET) << "Cannot initialize" << slide.options.portName << "slide."; return false; } + slide.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); + slide.layer.setDimensions(slide.width, slide.height); + slide.layer.setPosition({slide.x, slide.y, slide.z}); + slide.layer.setImage(slide.options.initialSlide); + } + + this->yarp().attachAsServer(this->m_rpcPort); + if(!m_rpcPort.open(m_rpcPortName)) + { + yCError(OPENXRHEADSET) << "Could not open" << m_rpcPortName << " RPC port."; + return false; } m_thisDevice.give(this, false); @@ -471,52 +469,52 @@ bool yarp::dev::OpenXrHeadset::threadInit() void yarp::dev::OpenXrHeadset::threadRelease() { - { - std::lock_guard lock(m_mutex); - - if (m_closed) - return; - - for (auto& hud : m_huds) - { - hud.layer.close(); - } + std::lock_guard lock(m_mutex); - for (auto& label : m_labels) - { - label.layer.close(); - } + if (m_closed) + return; - for (auto& slide : m_slides) - { - slide.layer.close(); - } + for (auto& hud : m_huds) + { + hud.layer.close(); + } - m_huds.clear(); - m_labels.clear(); - m_slides.clear(); - m_eyesManager.close(); + for (auto& label : m_labels) + { + label.layer.close(); + } - m_openXrInterface.close(); + for (auto& slide : m_slides) + { + slide.layer.close(); + } - if (m_tfPublisher) - { - m_driver.close(); - m_tfPublisher = nullptr; - } + m_huds.clear(); + m_labels.clear(); + m_slides.clear(); + m_eyesManager.close(); - m_rpcPort.close(); + m_openXrInterface.close(); - m_closed = true; + if (m_tfPublisher) + { + m_driver.close(); + m_tfPublisher = nullptr; } + m_rpcPort.close(); + stopJoypadControlServer(); + + m_closed = true; } void yarp::dev::OpenXrHeadset::run() { std::lock_guard lock(m_mutex); + bool shouldResetJoypadServer = false; + if (m_openXrInterface.isRunning()) { if (!m_eyesManager.update()) { @@ -579,9 +577,21 @@ void yarp::dev::OpenXrHeadset::run() } m_openXrInterface.draw(); + size_t previousButtonsSize = m_buttons.size(); + size_t previousAxesSize = m_axes.size(); + size_t previousThumbsticksSize = m_thumbsticks.size(); + m_openXrInterface.getButtons(m_buttons); m_openXrInterface.getAxes(m_axes); m_openXrInterface.getThumbsticks(m_thumbsticks); + + if (m_buttons.size() != previousButtonsSize || + m_axes.size() != previousAxesSize || + m_thumbsticks.size() != previousThumbsticksSize) + { + shouldResetJoypadServer = true; + } + m_openXrInterface.getAllPoses(m_posesManager.inputs()); if (m_openXrInterface.shouldResetLocalReferenceSpace()) @@ -601,6 +611,12 @@ void yarp::dev::OpenXrHeadset::run() close(); return; } + + if (shouldResetJoypadServer) + { + startJoypadControlServer(); + } + } bool yarp::dev::OpenXrHeadset::startService() @@ -1028,41 +1044,35 @@ bool yarp::dev::OpenXrHeadset::resetTransforms() bool yarp::dev::OpenXrHeadset::startJoypadControlServer() { + std::lock_guard lock(m_mutex); - stopJoypadControlServer(); - - { - std::lock_guard lock(m_mutex); - - yarp::os::Property options; - options.put("device", "joypadControlServer"); - options.put("name", m_prefix); - options.put("period", getPeriod()); - options.put("use_separate_ports", true); - options.put("stick_as_axis", false); + yarp::os::Property options; + options.put("device", "joypadControlServer"); + options.put("name", m_prefix); + options.put("period", getPeriod()); + options.put("use_separate_ports", true); + options.put("stick_as_axis", false); - m_joypadControlServerPtr = std::make_unique(); + m_joypadControlServerPtr = std::make_unique(); - if (!m_joypadControlServerPtr->open(options) || !m_joypadControlServerPtr->isValid()) - { - yCError(OPENXRHEADSET) << "Failed to open JoypadControlServer with the following options:" << options.toString(); - return false; - } + if (!m_joypadControlServerPtr->open(options) || !m_joypadControlServerPtr->isValid()) + { + yCError(OPENXRHEADSET) << "Failed to open JoypadControlServer with the following options:" << options.toString(); + return false; + } - if (!m_joypadControlServerPtr->view(m_joypadControlServerWrapper) || !m_joypadControlServerWrapper) - { - yCError(OPENXRHEADSET) << "Failed to view wrapper interface in joypad control server."; - return false; - } - - if (!m_joypadControlServerWrapper->attach(&m_thisDevice)) - { - yCError(OPENXRHEADSET) << "Failed to attach the device to the joypad control server."; + if (!m_joypadControlServerPtr->view(m_joypadControlServerWrapper) || !m_joypadControlServerWrapper) + { + yCError(OPENXRHEADSET) << "Failed to view wrapper interface in joypad control server."; + return false; + } - return false; - } + if (!m_joypadControlServerWrapper->attach(&m_thisDevice)) + { + yCError(OPENXRHEADSET) << "Failed to attach the device to the joypad control server."; + return false; } return true; @@ -1070,8 +1080,6 @@ bool yarp::dev::OpenXrHeadset::startJoypadControlServer() void yarp::dev::OpenXrHeadset::stopJoypadControlServer() { - std::lock_guard lock(m_mutex); - if (m_joypadControlServerWrapper) { m_joypadControlServerWrapper->detach(); diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h index df887b2..1203e78 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/src/devices/openxrheadset/OpenXrHeadset.h @@ -263,6 +263,9 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, * as it will reset all the transforms, including the ones that are not published by this module. */ virtual bool resetTransforms() override; + +private: + /** * Opens the joypad control server. It reopens it if already opened. */ @@ -273,8 +276,6 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, */ void stopJoypadControlServer(); -private: - struct GuiParam { float width; From a3ada7d0de621ae8aa9d1bd72a066141b2a05b0d Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 14 Nov 2024 17:40:44 +0100 Subject: [PATCH 3/5] Added option to enable the automatic launching of the joypad control server --- src/devices/openxrheadset/OpenXrHeadset.cpp | 7 +++++++ src/devices/openxrheadset/OpenXrHeadset.h | 1 + 2 files changed, 8 insertions(+) diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index bfe0fbc..cf84972 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.cpp +++ b/src/devices/openxrheadset/OpenXrHeadset.cpp @@ -251,6 +251,8 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg) double period = cfg.check("vr_period", yarp::os::Value(0.011)).asFloat64(); this->setPeriod(period); + m_launchJoypadControlServer = cfg.check("launch_joypad_control_server") && (cfg.find("launch_joypad_control_server").isNull() || cfg.find("launch_joypad_control_server").asBool()); + m_useNativeQuadLayers = cfg.check("use_native_quad_layers") && (cfg.find("use_native_quad_layers").isNull() || cfg.find("use_native_quad_layers").asBool()); m_openXrInterfaceSettings.posesPredictionInMs = cfg.check("vr_poses_prediction_in_ms", yarp::os::Value(0.0)).asFloat64(); @@ -1046,6 +1048,11 @@ bool yarp::dev::OpenXrHeadset::startJoypadControlServer() { std::lock_guard lock(m_mutex); + if (!m_launchJoypadControlServer) + { + return true; + } + yarp::os::Property options; options.put("device", "joypadControlServer"); options.put("name", m_prefix); diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h index 1203e78..7d19dc7 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/src/devices/openxrheadset/OpenXrHeadset.h @@ -343,6 +343,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, std::vector m_axes; std::vector m_thumbsticks; + bool m_launchJoypadControlServer{ false }; std::unique_ptr m_joypadControlServerPtr; yarp::dev::IWrapper* m_joypadControlServerWrapper = nullptr; yarp::dev::PolyDriver m_thisDevice; From e799b660ac392aad83ba2462d45ec9d4260194cc Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 14 Nov 2024 18:43:06 +0100 Subject: [PATCH 4/5] Added a RPC call to run the server manually --- src/devices/openxrheadset/OpenXrHeadset.cpp | 15 ++++++++------- src/devices/openxrheadset/OpenXrHeadset.h | 8 +++++++- .../thrifts/OpenXrHeadsetCommands.thrift | 6 ++++++ 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index cf84972..34628a0 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.cpp +++ b/src/devices/openxrheadset/OpenXrHeadset.cpp @@ -251,7 +251,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg) double period = cfg.check("vr_period", yarp::os::Value(0.011)).asFloat64(); this->setPeriod(period); - m_launchJoypadControlServer = cfg.check("launch_joypad_control_server") && (cfg.find("launch_joypad_control_server").isNull() || cfg.find("launch_joypad_control_server").asBool()); + m_autoJoypadControlServer = cfg.check("joypad_control_server_auto") && (cfg.find("joypad_control_server_auto").isNull() || cfg.find("joypad_control_server_auto").asBool()); m_useNativeQuadLayers = cfg.check("use_native_quad_layers") && (cfg.find("use_native_quad_layers").isNull() || cfg.find("use_native_quad_layers").asBool()); @@ -614,7 +614,7 @@ void yarp::dev::OpenXrHeadset::run() return; } - if (shouldResetJoypadServer) + if (shouldResetJoypadServer && m_autoJoypadControlServer) { startJoypadControlServer(); } @@ -1044,14 +1044,15 @@ bool yarp::dev::OpenXrHeadset::resetTransforms() return true; } -bool yarp::dev::OpenXrHeadset::startJoypadControlServer() +bool yarp::dev::OpenXrHeadset::restartJoypadControlServer() { std::lock_guard lock(m_mutex); + startJoypadControlServer(); +} - if (!m_launchJoypadControlServer) - { - return true; - } +bool yarp::dev::OpenXrHeadset::startJoypadControlServer() +{ + stopJoypadControlServer(); yarp::os::Property options; options.put("device", "joypadControlServer"); diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h index 7d19dc7..1549fb4 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/src/devices/openxrheadset/OpenXrHeadset.h @@ -264,6 +264,12 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, */ virtual bool resetTransforms() override; + /** + * Start the joypad control server. The server will restart if already started. + * @return True if the server is started successfully, false otherwise. + */ + virtual bool restartJoypadControlServer() override; + private: /** @@ -343,7 +349,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, std::vector m_axes; std::vector m_thumbsticks; - bool m_launchJoypadControlServer{ false }; + bool m_autoJoypadControlServer{ false }; std::unique_ptr m_joypadControlServerPtr; yarp::dev::IWrapper* m_joypadControlServerWrapper = nullptr; yarp::dev::PolyDriver m_thisDevice; diff --git a/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift b/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift index cc9fde3..499fe51 100644 --- a/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift +++ b/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift @@ -187,4 +187,10 @@ service OpenXrHeadsetCommands * as it will reset all the transforms, including the ones that are not published by this module. */ bool resetTransforms(); + + /** + * Start the joypad control server. The server will restart if already started. + * @return True if the server is started successfully, false otherwise. + */ + bool restartJoypadControlServer(); } From 056b22ad472543a3f6619f194b1fa43248eb28bd Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 15 Nov 2024 10:53:07 +0100 Subject: [PATCH 5/5] Tests and bugfixes --- src/devices/openxrheadset/OpenXrHeadset.cpp | 226 +++++++++++--------- src/devices/openxrheadset/OpenXrHeadset.h | 2 +- 2 files changed, 121 insertions(+), 107 deletions(-) diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index 34628a0..096f43c 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.cpp +++ b/src/devices/openxrheadset/OpenXrHeadset.cpp @@ -24,6 +24,10 @@ yarp::dev::OpenXrHeadset::OpenXrHeadset() yarp::dev::OpenXrHeadset::~OpenXrHeadset() { + { + std::lock_guard lock(m_joypadServerMutex); + m_thisDevice.close(); + } this->stop(); } @@ -392,123 +396,130 @@ bool yarp::dev::OpenXrHeadset::close() bool yarp::dev::OpenXrHeadset::threadInit() { - std::lock_guard lock(m_mutex); - - if (!m_openXrInterface.initialize(m_openXrInterfaceSettings)) { - yCError(OPENXRHEADSET) << "Failed to initialize OpenXr interface."; - return false; - } + std::lock_guard lock(m_mutex); - auto getLayer = [this]() -> std::shared_ptr - { - if (m_useNativeQuadLayers) + if (!m_openXrInterface.initialize(m_openXrInterfaceSettings)) { - return m_openXrInterface.addHeadFixedQuadLayer(); + yCError(OPENXRHEADSET) << "Failed to initialize OpenXr interface."; + return false; } - else + + auto getLayer = [this]() -> std::shared_ptr { - return m_openXrInterface.addHeadFixedOpenGLQuadLayer(); - } - }; + if (m_useNativeQuadLayers) + { + return m_openXrInterface.addHeadFixedQuadLayer(); + } + else + { + return m_openXrInterface.addHeadFixedOpenGLQuadLayer(); + } + }; - m_eyesManager.options().leftEyeQuadLayer = getLayer(); - m_eyesManager.options().rightEyeQuadLayer = getLayer(); + m_eyesManager.options().leftEyeQuadLayer = getLayer(); + m_eyesManager.options().rightEyeQuadLayer = getLayer(); - if (!m_eyesManager.initialize()) - { - yCError(OPENXRHEADSET) << "Failed to initialize eyes."; - } + if (!m_eyesManager.initialize()) + { + yCError(OPENXRHEADSET) << "Failed to initialize eyes."; + } - for (GuiParam& gui : m_huds) - { - if (!gui.layer.initialize(getLayer(), gui.portName)) { - yCError(OPENXRHEADSET) << "Cannot initialize" << gui.portName << "display texture."; - return false; + for (GuiParam& gui : m_huds) + { + if (!gui.layer.initialize(getLayer(), gui.portName)) { + yCError(OPENXRHEADSET) << "Cannot initialize" << gui.portName << "display texture."; + return false; + } + gui.layer.setVisibility(gui.visibility); + gui.layer.setDimensions(gui.width, gui.height); + gui.layer.setPosition({ gui.x, gui.y, gui.z }); } - gui.layer.setVisibility(gui.visibility); - gui.layer.setDimensions(gui.width, gui.height); - gui.layer.setPosition({gui.x, gui.y, gui.z}); - } - for (LabelLayer& label : m_labels) - { - label.options.quadLayer = getLayer(); + for (LabelLayer& label : m_labels) + { + label.options.quadLayer = getLayer(); - if (!label.layer.initialize(label.options)) { - yCError(OPENXRHEADSET) << "Cannot initialize" << label.options.portName << "label."; - return false; + if (!label.layer.initialize(label.options)) { + yCError(OPENXRHEADSET) << "Cannot initialize" << label.options.portName << "label."; + return false; + } + label.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); + label.layer.setDimensions(label.width, label.height); + label.layer.setPosition({ label.x, label.y, label.z }); } - label.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); - label.layer.setDimensions(label.width, label.height); - label.layer.setPosition({label.x, label.y, label.z}); - } - for (SlideLayer& slide : m_slides) - { - slide.options.quadLayer = getLayer(); - if (!slide.layer.initialize(slide.options)) { - yCError(OPENXRHEADSET) << "Cannot initialize" << slide.options.portName << "slide."; + for (SlideLayer& slide : m_slides) + { + slide.options.quadLayer = getLayer(); + if (!slide.layer.initialize(slide.options)) { + yCError(OPENXRHEADSET) << "Cannot initialize" << slide.options.portName << "slide."; + return false; + } + slide.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); + slide.layer.setDimensions(slide.width, slide.height); + slide.layer.setPosition({ slide.x, slide.y, slide.z }); + slide.layer.setImage(slide.options.initialSlide); + } + + this->yarp().attachAsServer(this->m_rpcPort); + if (!m_rpcPort.open(m_rpcPortName)) + { + yCError(OPENXRHEADSET) << "Could not open" << m_rpcPortName << " RPC port."; return false; } - slide.layer.setVisibility(IOpenXrQuadLayer::Visibility::BOTH_EYES); - slide.layer.setDimensions(slide.width, slide.height); - slide.layer.setPosition({slide.x, slide.y, slide.z}); - slide.layer.setImage(slide.options.initialSlide); } - this->yarp().attachAsServer(this->m_rpcPort); - if(!m_rpcPort.open(m_rpcPortName)) { - yCError(OPENXRHEADSET) << "Could not open" << m_rpcPortName << " RPC port."; - return false; + std::lock_guard lock(m_joypadServerMutex); + m_thisDevice.give(this, false); } - m_thisDevice.give(this, false); - return true; } void yarp::dev::OpenXrHeadset::threadRelease() { - std::lock_guard lock(m_mutex); - - if (m_closed) - return; + stopJoypadControlServer(); - for (auto& hud : m_huds) { - hud.layer.close(); - } + std::lock_guard lock(m_mutex); - for (auto& label : m_labels) - { - label.layer.close(); - } + if (m_closed) + return; - for (auto& slide : m_slides) - { - slide.layer.close(); - } + for (auto& hud : m_huds) + { + hud.layer.close(); + } - m_huds.clear(); - m_labels.clear(); - m_slides.clear(); - m_eyesManager.close(); + for (auto& label : m_labels) + { + label.layer.close(); + } - m_openXrInterface.close(); + for (auto& slide : m_slides) + { + slide.layer.close(); + } - if (m_tfPublisher) - { - m_driver.close(); - m_tfPublisher = nullptr; - } + m_huds.clear(); + m_labels.clear(); + m_slides.clear(); + m_eyesManager.close(); - m_rpcPort.close(); + m_openXrInterface.close(); - stopJoypadControlServer(); + if (m_tfPublisher) + { + m_driver.close(); + m_tfPublisher = nullptr; + } + + m_rpcPort.close(); - m_closed = true; + m_closed = true; + } } void yarp::dev::OpenXrHeadset::run() @@ -1046,41 +1057,42 @@ bool yarp::dev::OpenXrHeadset::resetTransforms() bool yarp::dev::OpenXrHeadset::restartJoypadControlServer() { - std::lock_guard lock(m_mutex); - startJoypadControlServer(); + return startJoypadControlServer(); } bool yarp::dev::OpenXrHeadset::startJoypadControlServer() { stopJoypadControlServer(); - yarp::os::Property options; - options.put("device", "joypadControlServer"); - options.put("name", m_prefix); - options.put("period", getPeriod()); - options.put("use_separate_ports", true); - options.put("stick_as_axis", false); - - m_joypadControlServerPtr = std::make_unique(); - - if (!m_joypadControlServerPtr->open(options) || !m_joypadControlServerPtr->isValid()) { - yCError(OPENXRHEADSET) << "Failed to open JoypadControlServer with the following options:" << options.toString(); - return false; - } + std::lock_guard lock(m_joypadServerMutex); + yarp::os::Property options; + options.put("device", "JoypadControlServer"); + options.put("name", m_prefix); + options.put("period", getPeriod()); + options.put("use_separate_ports", true); + options.put("stick_as_axis", false); + m_joypadControlServerPtr = std::make_unique(); - if (!m_joypadControlServerPtr->view(m_joypadControlServerWrapper) || !m_joypadControlServerWrapper) - { - yCError(OPENXRHEADSET) << "Failed to view wrapper interface in joypad control server."; - return false; - } + if (!m_joypadControlServerPtr->open(options)) + { + yCError(OPENXRHEADSET) << "Failed to open JoypadControlServer with the following options:" << options.toString(); + return false; + } - if (!m_joypadControlServerWrapper->attach(&m_thisDevice)) - { - yCError(OPENXRHEADSET) << "Failed to attach the device to the joypad control server."; + if (!m_joypadControlServerPtr->view(m_joypadControlServerWrapper) || !m_joypadControlServerWrapper) + { + yCError(OPENXRHEADSET) << "Failed to view wrapper interface in joypad control server."; + return false; + } - return false; + if (!m_joypadControlServerWrapper->attach(&m_thisDevice)) + { + yCError(OPENXRHEADSET) << "Failed to attach the device to the joypad control server."; + + return false; + } } return true; @@ -1088,6 +1100,8 @@ bool yarp::dev::OpenXrHeadset::startJoypadControlServer() void yarp::dev::OpenXrHeadset::stopJoypadControlServer() { + std::lock_guard lock(m_joypadServerMutex); + if (m_joypadControlServerWrapper) { m_joypadControlServerWrapper->detach(); diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h index 1549fb4..5379aa4 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/src/devices/openxrheadset/OpenXrHeadset.h @@ -354,7 +354,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, yarp::dev::IWrapper* m_joypadControlServerWrapper = nullptr; yarp::dev::PolyDriver m_thisDevice; - std::mutex m_mutex; + std::mutex m_mutex, m_joypadServerMutex; };