diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp index 08a3b5d..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(); } @@ -251,6 +255,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_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()); m_openXrInterfaceSettings.posesPredictionInMs = cfg.check("vr_poses_prediction_in_ms", yarp::os::Value(0.0)).asFloat64(); @@ -427,7 +433,7 @@ bool yarp::dev::OpenXrHeadset::threadInit() } gui.layer.setVisibility(gui.visibility); gui.layer.setDimensions(gui.width, gui.height); - gui.layer.setPosition({gui.x, gui.y, gui.z}); + gui.layer.setPosition({ gui.x, gui.y, gui.z }); } for (LabelLayer& label : m_labels) @@ -440,7 +446,7 @@ bool yarp::dev::OpenXrHeadset::threadInit() } 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.setPosition({ label.x, label.y, label.z }); } for (SlideLayer& slide : m_slides) @@ -452,75 +458,76 @@ bool yarp::dev::OpenXrHeadset::threadInit() } 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.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)) + 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); + } + 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(); - m_closed = true; + if (m_tfPublisher) + { + m_driver.close(); + m_tfPublisher = nullptr; + } + + m_rpcPort.close(); + + 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()) { @@ -583,9 +590,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()) @@ -605,6 +624,12 @@ void yarp::dev::OpenXrHeadset::run() close(); return; } + + if (shouldResetJoypadServer && m_autoJoypadControlServer) + { + startJoypadControlServer(); + } + } bool yarp::dev::OpenXrHeadset::startService() @@ -1029,3 +1054,62 @@ bool yarp::dev::OpenXrHeadset::resetTransforms() m_posesManager.reset(); return true; } + +bool yarp::dev::OpenXrHeadset::restartJoypadControlServer() +{ + return startJoypadControlServer(); +} + +bool yarp::dev::OpenXrHeadset::startJoypadControlServer() +{ + stopJoypadControlServer(); + + { + 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->open(options)) + { + 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_joypadServerMutex); + + 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..5379aa4 100644 --- a/src/devices/openxrheadset/OpenXrHeadset.h +++ b/src/devices/openxrheadset/OpenXrHeadset.h @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -263,8 +264,24 @@ 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: + /** + * Opens the joypad control server. It reopens it if already opened. + */ + bool startJoypadControlServer(); + + /** + * Closes the joypad control server. + */ + void stopJoypadControlServer(); + struct GuiParam { float width; @@ -332,7 +349,12 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver, std::vector m_axes; std::vector m_thumbsticks; - std::mutex m_mutex; + bool m_autoJoypadControlServer{ false }; + std::unique_ptr m_joypadControlServerPtr; + yarp::dev::IWrapper* m_joypadControlServerWrapper = nullptr; + yarp::dev::PolyDriver m_thisDevice; + + std::mutex m_mutex, m_joypadServerMutex; }; 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(); }