Skip to content

Commit

Permalink
Starting the joypad control server whenever the number of inputs changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@S-Dafarra
S-Dafarra committed Nov 14, 2024
1 parent 0af8329 commit c45a68f
Show file tree
Hide file tree
Showing 2 changed files with 128 additions and 119 deletions.
242 changes: 125 additions & 117 deletions src/devices/openxrheadset/OpenXrHeadset.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -390,78 +390,76 @@ bool yarp::dev::OpenXrHeadset::close()

bool yarp::dev::OpenXrHeadset::threadInit()
{
std::lock_guard<std::mutex> lock(m_mutex);

if (!m_openXrInterface.initialize(m_openXrInterfaceSettings))
{
std::lock_guard<std::mutex> lock(m_mutex);
yCError(OPENXRHEADSET) << "Failed to initialize OpenXr interface.";
return false;
}

if (!m_openXrInterface.initialize(m_openXrInterfaceSettings))
auto getLayer = [this]() -> std::shared_ptr<IOpenXrQuadLayer>
{
if (m_useNativeQuadLayers)
{
yCError(OPENXRHEADSET) << "Failed to initialize OpenXr interface.";
return false;
return m_openXrInterface.addHeadFixedQuadLayer();
}

auto getLayer = [this]() -> std::shared_ptr<IOpenXrQuadLayer>
{
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);
Expand All @@ -471,52 +469,52 @@ bool yarp::dev::OpenXrHeadset::threadInit()

void yarp::dev::OpenXrHeadset::threadRelease()
{
{
std::lock_guard<std::mutex> lock(m_mutex);

if (m_closed)
return;

for (auto& hud : m_huds)
{
hud.layer.close();
}
std::lock_guard<std::mutex> 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<std::mutex> lock(m_mutex);

bool shouldResetJoypadServer = false;

if (m_openXrInterface.isRunning())
{
if (!m_eyesManager.update()) {
Expand Down Expand Up @@ -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())
Expand All @@ -601,6 +611,12 @@ void yarp::dev::OpenXrHeadset::run()
close();
return;
}

if (shouldResetJoypadServer)
{
startJoypadControlServer();
}

}

bool yarp::dev::OpenXrHeadset::startService()
Expand Down Expand Up @@ -1028,50 +1044,42 @@ bool yarp::dev::OpenXrHeadset::resetTransforms()

bool yarp::dev::OpenXrHeadset::startJoypadControlServer()
{
std::lock_guard<std::mutex> lock(m_mutex);

stopJoypadControlServer();

{
std::lock_guard<std::mutex> 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<yarp::dev::PolyDriver>();
m_joypadControlServerPtr = std::make_unique<yarp::dev::PolyDriver>();

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;
}

void yarp::dev::OpenXrHeadset::stopJoypadControlServer()
{
std::lock_guard<std::mutex> lock(m_mutex);

if (m_joypadControlServerWrapper)
{
m_joypadControlServerWrapper->detach();
Expand Down
5 changes: 3 additions & 2 deletions src/devices/openxrheadset/OpenXrHeadset.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/
Expand All @@ -273,8 +276,6 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
*/
void stopJoypadControlServer();

private:

struct GuiParam
{
float width;
Expand Down

0 comments on commit c45a68f

Please sign in to comment.