YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
OVRHeadset.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3 *
4 * This library is free software; you can redistribute it and/or
5 * modify it under the terms of the GNU Lesser General Public
6 * License as published by the Free Software Foundation; either
7 * version 2.1 of the License, or (at your option) any later version.
8 *
9 * This library is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * Lesser General Public License for more details.
13 *
14 * You should have received a copy of the GNU Lesser General Public
15 * License along with this library; if not, write to the Free Software
16 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19#define _USE_MATH_DEFINES
20
21#include "OVRHeadset.h"
22#include "InputCallback.h"
23#include "TextureBuffer.h"
24#include "TextureStatic.h"
25#include "TextureBattery.h"
26#include "GLDebug.h"
28
29#include "img-yarp-robot-64.h"
30#include "img-crosshairs.h"
31
33#include <yarp/os/LogStream.h>
34#include <yarp/os/Network.h>
35#include <yarp/os/Stamp.h>
36#include <yarp/os/Time.h>
37#include <yarp/sig/Image.h>
38#include <yarp/os/Property.h>
39#include <yarp/os/SystemClock.h>
40#include <yarp/math/Math.h>
41
42#include <cmath>
43#include <mutex>
44#include <unordered_map>
45#include <OVR_CAPI_Util.h>
46#include <OVR_Math.h>
47
48#if defined(_WIN32)
49#include <dxgi.h> // for GetDefaultAdapterLuid
50#pragma comment(lib, "dxgi.lib")
51#endif
52YARP_CONSTEXPR unsigned int AXIS_COUNT = 8;
53YARP_CONSTEXPR unsigned int STICK_COUNT = 2;
54YARP_CONSTEXPR unsigned int BUTTON_COUNT = 13;
55
56#if defined(_WIN32)
57 #define GLFW_EXPOSE_NATIVE_WIN32
58 #define GLFW_EXPOSE_NATIVE_WGL
59 #define OVR_OS_WIN32
60#elif defined(__APPLE__)
61 #define GLFW_EXPOSE_NATIVE_COCOA
62 #define GLFW_EXPOSE_NATIVE_NSGL
63 #define OVR_OS_MAC
64#elif defined(__linux__)
65 #define GLFW_EXPOSE_NATIVE_X11
66 #define GLFW_EXPOSE_NATIVE_GLX
67 #define OVR_OS_LINUX
68#endif
69#include <GLFW/glfw3.h>
70#include <GLFW/glfw3native.h>
71
72#include <OVR_System.h>
73#include <OVR_CAPI_GL.h>
74
75
76#ifdef check
77// Undefine the check macro in AssertMacros.h on OSX or the "cfg.check"
78// call will fail compiling.
79#undef check
80#endif
81
82typedef bool(yarp::os::Value::*valueIsType)(void) const;
85{
86 double resizeW;
87 double resizeH;
88 double x;
89 double y;
90 double z;
91 double alpha;
93 ovrLayerQuad layer;
95};
96//----------------[utilities]
97//WARNING it makes a conversion of the coordinate system
98inline yarp::sig::Vector ovrVec3ToYarp(const ovrVector3f& v)
99{
100 yarp::sig::Vector ret(3);
101
102 ret[0] = -v.z;
103 ret[1] = -v.x;
104 ret[2] = v.y;
105
106 return ret;
107}
108
109//WARNING it makes a conversion of the coordinate system
110inline yarp::sig::Vector ovrRot2YarpRPY(const OVR::Quatf& rot)
111{
112 float yaw, pitch, roll;
114
115 rot.GetEulerAngles<OVR::Axis_Y, OVR::Axis_X, OVR::Axis_Z>(&yaw, &pitch, &roll);
116 v[0] = -roll; v[1] = -pitch; v[2] = yaw;
117
118 return v;
119}
120
121inline yarp::sig::Matrix ovr2matrix(const ovrVector3f& pos, const OVR::Quatf& orientation)
122{
124 ret = yarp::math::rpy2dcm(ovrRot2YarpRPY(orientation));
125 ret.setSubcol(ovrVec3ToYarp(pos), 0, 3);
126
127 return ret;
128}
129
130inline ovrVector3f vecSubtract(const ovrVector3f& a, const ovrVector3f& b)
131{
132 ovrVector3f ret;
133 ret.x = a.x - b.x;
134 ret.y = a.y - b.y;
135 ret.z = a.z - b.z;
136 return ret;
137}
138
139static inline void debugTangent(std::string message, float tangent1, float tangent2)
140{
142 (message + " %10f (%5f[rad] = %5f[deg]) %10f (%5f[rad] = %5f[deg])\n").c_str(),
143 tangent1,
144 atan(tangent1),
145 OVR::RadToDegree(atan(tangent1)),
146 tangent2,
147 atan(tangent2),
148 OVR::RadToDegree(atan(tangent2)));
149}
150static void debugFov(const ovrFovPort fov[2]) {
152 " Left Eye Right Eye\n");
153 debugTangent("LeftTan", fov[0].LeftTan , fov[0].LeftTan);
154 debugTangent("RightTan", fov[0].RightTan, fov[0].RightTan);
155 debugTangent("UpTan", fov[0].UpTan , fov[0].UpTan );
156 debugTangent("DownTan", fov[0].DownTan , fov[0].DownTan );
160}
161
162static int compareLuid(const ovrGraphicsLuid& lhs, const ovrGraphicsLuid& rhs)
163{
164 return memcmp(&lhs, &rhs, sizeof(ovrGraphicsLuid));
165}
166
167static ovrGraphicsLuid GetDefaultAdapterLuid()
168{
169 ovrGraphicsLuid luid = ovrGraphicsLuid();
170
171#if defined(_WIN32)
172 IDXGIFactory* factory = nullptr;
173
174 if (SUCCEEDED(CreateDXGIFactory(IID_PPV_ARGS(&factory))))
175 {
176 IDXGIAdapter* adapter = nullptr;
177
178 if (SUCCEEDED(factory->EnumAdapters(0, &adapter)))
179 {
180 DXGI_ADAPTER_DESC desc;
181
182 adapter->GetDesc(&desc);
183 memcpy(&luid, &desc.AdapterLuid, sizeof(luid));
184 adapter->Release();
185 }
186
187 factory->Release();
188 }
189#endif
190
191 return luid;
192}
193
194inline void writeVec3OnPort(yarp::os::BufferedPort<yarp::os::Bottle>*const & port, const OVR::Vector3f& vec3, yarp::os::Stamp& stamp)
195{
196 if (port || port->getOutputCount() > 0)
197 {
198 yarp::os::Bottle& output = port->prepare();
199 output.clear();
200 output.addFloat64(vec3.x);
201 output.addFloat64(vec3.y);
202 output.addFloat64(vec3.z);
203 port->setEnvelope(stamp);
204 port->write();
205 }
206}
207
208inline OVR::Vector3f radToDeg(const OVR::Vector3f& v)
209{
210 OVR::Vector3f ret;
211
212 ret.x = OVR::RadToDegree(v.x);
213 ret.y = OVR::RadToDegree(v.y);
214 ret.z = OVR::RadToDegree(v.z);
215
216 return ret;
217}
218
219inline void setHeadLockedLayer(ovrLayerQuad& layer, TextureStatic* tex,
220 const float x, const float y, const float z, //position
221 const float rx, const float ry, const float rz, float rw, //rotation
222 const float sizeX, const float sizeY)//scale
223{
224 layer.Header.Type = ovrLayerType_Quad;
225 layer.Header.Flags = ovrLayerFlag_HeadLocked;
226 layer.ColorTexture = tex->textureSwapChain;
227 layer.QuadPoseCenter.Position.x = x;
228 layer.QuadPoseCenter.Position.y = y;
229 layer.QuadPoseCenter.Position.z = z;
230 layer.QuadPoseCenter.Orientation.x = rx;
231 layer.QuadPoseCenter.Orientation.y = ry;
232 layer.QuadPoseCenter.Orientation.z = rz;
233 layer.QuadPoseCenter.Orientation.w = rw;
234 layer.QuadSize.x = sizeX;
235 layer.QuadSize.y = sizeY;
236
237 layer.Viewport = OVR::Recti(0, 0, tex->width, tex->height);
238}
239
240inline void setHeadLockedLayer(ovrLayerQuad& layer, TextureBuffer* tex,
241 const float x, const float y, const float z, //position
242 const float rx, const float ry, const float rz, float rw, //rotation
243 const float sizeX, const float sizeY)//scale
244{
245 layer.Header.Type = ovrLayerType_Quad;
246 layer.Header.Flags = ovrLayerFlag_HeadLocked;
247 layer.ColorTexture = tex->textureSwapChain;
248 layer.QuadPoseCenter.Position.x = x;
249 layer.QuadPoseCenter.Position.y = y;
250 layer.QuadPoseCenter.Position.z = z;
251 layer.QuadPoseCenter.Orientation.x = rx;
252 layer.QuadPoseCenter.Orientation.y = ry;
253 layer.QuadPoseCenter.Orientation.z = rz;
254 layer.QuadPoseCenter.Orientation.w = rw;
255 layer.QuadSize.x = sizeX;
256 layer.QuadSize.y = sizeY;
257
258 layer.Viewport = OVR::Recti(0, 0, tex->width, tex->height);
259}
260
261//----------------end [utilities]
262
264 yarp::dev::DeviceDriver(),
265 yarp::os::PeriodicThread(0.011, yarp::os::ShouldUseSystemClock::Yes) // ~90 fps
266{
268}
273
274void yarp::dev::OVRHeadset::fillAxisStorage()
275{
276 axisIdToValue.push_back(inputState.IndexTrigger);
277 axisIdToValue.push_back(inputState.IndexTrigger + 1);
278 axisIdToValue.push_back(inputState.HandTrigger);
279 axisIdToValue.push_back(inputState.HandTrigger + 1 );
280
281 if (getStickAsAxis)
282 {
283 axisIdToValue.push_back(&inputState.Thumbstick[ovrHand_Left].x);
284 axisIdToValue.push_back(&inputState.Thumbstick[ovrHand_Left].y);
285 axisIdToValue.push_back(&inputState.Thumbstick[ovrHand_Right].x);
286 axisIdToValue.push_back(&inputState.Thumbstick[ovrHand_Right].y);
287 }
288
289}
290
291void yarp::dev::OVRHeadset::fillErrorStorage()
292{
293 error_messages[ovrError_MemoryAllocationFailure ] = "Failure to allocate memory.";
294 error_messages[ovrError_InvalidSession ] = "Invalid ovrSession parameter provided.";
295 error_messages[ovrError_Timeout ] = "The operation timed out.";
296 error_messages[ovrError_NotInitialized ] = "The system or component has not been initialized.";
297 error_messages[ovrError_InvalidParameter ] = "Invalid parameter provided.See error info or log for details.";
298 error_messages[ovrError_ServiceError ] = "Generic service error.See error info or log for details.";
299 error_messages[ovrError_NoHmd ] = "The given HMD doesn't exist.";
300 error_messages[ovrError_Unsupported ] = "Function call is not supported on this hardware / software.";
301 error_messages[ovrError_DeviceUnavailable ] = "Specified device type isn't available.";
302 error_messages[ovrError_InvalidHeadsetOrientation ] = "The headset was in an invalid orientation for the requested operation(e.g.vertically oriented during ovr_RecenterPose).";
303 error_messages[ovrError_ClientSkippedDestroy ] = "The client failed to call ovr_Destroy on an active session before calling ovr_Shutdown.Or the client crashed.";
304 error_messages[ovrError_ClientSkippedShutdown ] = "The client failed to call ovr_Shutdown or the client crashed.";
305 error_messages[ovrError_ServiceDeadlockDetected ] = "The service watchdog discovered a deadlock.";
306 error_messages[ovrError_InvalidOperation ] = "Function call is invalid for object's current state.";
307 error_messages[ovrError_AudioDeviceNotFound ] = "Failure to find the specified audio device.";
308 error_messages[ovrError_AudioComError ] = "Generic COM error.";
309 error_messages[ovrError_Initialize ] = "Generic initialization error.";
310 error_messages[ovrError_LibLoad ] = "Couldn't load LibOVRRT.";
311 error_messages[ovrError_LibVersion ] = "LibOVRRT version incompatibility.";
312 error_messages[ovrError_ServiceConnection ] = "Couldn't connect to the OVR Service.";
313 error_messages[ovrError_ServiceVersion ] = "OVR Service version incompatibility.";
314 error_messages[ovrError_IncompatibleOS ] = "The operating system version is incompatible.";
315 error_messages[ovrError_DisplayInit ] = "Unable to initialize the HMD display.";
316 error_messages[ovrError_ServerStart ] = "Unable to start the server.Is it already running ?";
317 error_messages[ovrError_Reinitialization ] = "Attempting to re - initialize with a different version.";
318 error_messages[ovrError_MismatchedAdapters ] = "Chosen rendering adapters between client and service do not match.";
319 error_messages[ovrError_LeakingResources ] = "Calling application has leaked resources.";
320 error_messages[ovrError_ClientVersion ] = "Client version too old to connect to service.";
321 error_messages[ovrError_OutOfDateOS ] = "The operating system is out of date.";
322 error_messages[ovrError_OutOfDateGfxDriver ] = "The graphics driver is out of date.";
323 error_messages[ovrError_IncompatibleGPU ] = "The graphics hardware is not supported.";
324 error_messages[ovrError_NoValidVRDisplaySystem ] = "No valid VR display system found.";
325 error_messages[ovrError_Obsolete ] = "Feature or API is obsolete and no longer supported.";
326 error_messages[ovrError_DisabledOrDefaultAdapter ] = "No supported VR display system found, but disabled or driverless adapter found.";
327 error_messages[ovrError_HybridGraphicsNotSupported ] = "The system is using hybrid graphics(Optimus, etc...), which is not support.";
328 error_messages[ovrError_DisplayManagerInit ] = "Initialization of the DisplayManager failed.";
329 error_messages[ovrError_TrackerDriverInit ] = "Failed to get the interface for an attached tracker.";
330 error_messages[ovrError_LibSignCheck ] = "LibOVRRT signature check failure.";
331 error_messages[ovrError_LibPath ] = "LibOVRRT path failure.";
332 error_messages[ovrError_LibSymbols ] = "LibOVRRT symbol resolution failure.";
333 error_messages[ovrError_RemoteSession ] = "Failed to connect to the service because remote connections to the service are not allowed.";
334 error_messages[ovrError_DisplayLost ] = "In the event of a system - wide graphics reset or cable unplug this is returned to the app.";
335 error_messages[ovrError_TextureSwapChainFull ] = "ovr_CommitTextureSwapChain was called too many times on a texture swapchain without calling submit to use the chain.";
336 error_messages[ovrError_TextureSwapChainInvalid ] = "The ovrTextureSwapChain is in an incomplete or inconsistent state.Ensure ovr_CommitTextureSwapChain was called at least once first.";
337 error_messages[ovrError_GraphicsDeviceReset ] = "Graphics device has been reset(TDR, etc...)";
338 error_messages[ovrError_DisplayRemoved ] = "HMD removed from the display adapter.";
339 error_messages[ovrError_ContentProtectionNotAvailable ] = "Content protection is not available for the display.";
340 error_messages[ovrError_ApplicationInvisible ] = "Application declared itself as an invisible type and is not allowed to submit frames.";
341 error_messages[ovrError_Disallowed ] = "The given request is disallowed under the current conditions.";
342 error_messages[ovrError_DisplayPluggedIncorrectly ] = "Display portion of HMD is plugged into an incompatible port(ex: IGP)";
343 error_messages[ovrError_RuntimeException ] = "A runtime exception occurred.The application is required to shutdown LibOVR and re - initialize it before this error state will be cleared.";
344 error_messages[ovrError_NoCalibration ] = "Result of a missing calibration block.";
345 error_messages[ovrError_OldVersion ] = "Result of an old calibration block.";
346 error_messages[ovrError_MisformattedBlock ] = "Result of a bad calibration block due to lengths.";
347}
348
349void yarp::dev::OVRHeadset::fillButtonStorage()
350{
351 buttonIdToOvrButton.push_back(ovrButton_A);
352 buttonIdToOvrButton.push_back(ovrButton_B);
353 buttonIdToOvrButton.push_back(ovrButton_RThumb);
354 buttonIdToOvrButton.push_back(ovrButton_RShoulder);
355 buttonIdToOvrButton.push_back(ovrButton_X);
356 buttonIdToOvrButton.push_back(ovrButton_Y);
357 buttonIdToOvrButton.push_back(ovrButton_LThumb);
358 buttonIdToOvrButton.push_back(ovrButton_LShoulder);
359 buttonIdToOvrButton.push_back(ovrButton_Enter);
360 buttonIdToOvrButton.push_back(ovrButton_Back);
361 buttonIdToOvrButton.push_back(ovrButton_VolUp);
362 buttonIdToOvrButton.push_back(ovrButton_VolDown);
363 buttonIdToOvrButton.push_back(ovrButton_Home);
364}
365
366void yarp::dev::OVRHeadset::fillHatStorage()
367{
368 DButtonToHat[0] = YRPJOY_HAT_CENTERED;
369 DButtonToHat[ovrButton_Up] = YRPJOY_HAT_UP;
370 DButtonToHat[ovrButton_Right] = YRPJOY_HAT_RIGHT;
371 DButtonToHat[ovrButton_Down] = YRPJOY_HAT_DOWN;
372 DButtonToHat[ovrButton_Left] = YRPJOY_HAT_LEFT;
373}
374
376{
378
379 typedef std::vector<std::pair<yarp::os::BufferedPort<yarp::os::Bottle>**, std::string> > port_params;
380 typedef std::vector<std::tuple<std::string, std::string, bool*, bool> > optionalParamType;
381
382 yarp::os::Property tfClientCfg;
383 port_params ports;
384 optionalParamType optionalParams;
385 std::string standardPortPrefix;
386
387 standardPortPrefix = "/oculus";
388
389 //checking all the parameter in the configuration file..
390 {
391 constexpr unsigned int STRING = 0;
392 constexpr unsigned int BOOL = 1;
393 constexpr unsigned int INT = 2;
394 constexpr unsigned int DOUBLE = 3;
395
396 std::map<int, std::string> err_msgs;
397 std::map<int, valueIsType> isFunctionMap;
398 std::vector<std::pair<std::string, int> > paramParser;
399
400 err_msgs[STRING] = "a string";
401 err_msgs[BOOL] = "a boolean type";
402 err_msgs[INT] = "an integer type";
403 err_msgs[DOUBLE] = "a real type";
404 isFunctionMap[STRING] = &yarp::os::Value::isString;
405 isFunctionMap[BOOL] = &yarp::os::Value::isBool;
406 isFunctionMap[INT] = &yarp::os::Value::isInt32;
407 isFunctionMap[DOUBLE] = &yarp::os::Value::isFloat64;
408
409 //to add a parameter check, simply add a line below here and let the magic happens
410 paramParser.push_back(std::make_pair("tfDevice", STRING));
411 paramParser.push_back(std::make_pair("tfLocal", STRING));
412 paramParser.push_back(std::make_pair("tfRemote", STRING));
413 paramParser.push_back(std::make_pair("tf_left_hand_frame", STRING));
414 paramParser.push_back(std::make_pair("tf_right_hand_frame", STRING));
415 paramParser.push_back(std::make_pair("tf_root_frame", STRING));
416 paramParser.push_back(std::make_pair("stick_as_axis", BOOL));
417 paramParser.push_back(std::make_pair("gui_elements", INT));
418
419 for (auto& p : paramParser)
420 {
421 if (!cfg.check(p.first) || !(cfg.find(p.first).*isFunctionMap[p.second])())
422 {
423 std::string err_type = err_msgs.find(p.second) == err_msgs.end() ? "[unknown type]" : err_msgs[p.second];
424 yCError(OVRHEADSET) << "ovrHeadset: parameter" << p.first << "not found or not" << err_type << "in configuration file";
425 return false;
426 }
427 }
428 guiCount = cfg.find("gui_elements").asInt32();
429 paramParser.clear();
430 if (guiCount)
431 {
432 paramParser.push_back(std::make_pair("width", DOUBLE));
433 paramParser.push_back(std::make_pair("height", DOUBLE));
434 paramParser.push_back(std::make_pair("x", DOUBLE));
435 paramParser.push_back(std::make_pair("y", DOUBLE));
436 paramParser.push_back(std::make_pair("z", DOUBLE));
437 paramParser.push_back(std::make_pair("alpha", DOUBLE));
438
439 for (unsigned int i = 0; i < guiCount; ++i)
440 {
441 std::string groupName = "GUI_" + std::to_string(i);
442 yarp::os::Bottle& guip = cfg.findGroup(groupName);
443 guiParam hud;
444
445 if (guip.isNull())
446 {
447 yCError(OVRHEADSET) << "group:" << groupName << "not found in configuration file..";
448 return false;
449 }
450
451 for (auto& p : paramParser)
452 {
453 if (!guip.check(p.first) || !(guip.find(p.first).*isFunctionMap[p.second])())
454 {
455 std::string err_type = err_msgs.find(p.second) == err_msgs.end() ? "[unknow type]" : err_msgs[p.second];
456 yCError(OVRHEADSET) << "ovrHeadset: parameter" << p.first << "not found or not" << err_type << "in" << groupName << "group in configuration file";
457 return false;
458 }
459 }
460
461 hud.resizeW = guip.find("width").asFloat64();
462 hud.resizeH = guip.find("height").asFloat64();
463 hud.x = guip.find("x").asFloat64();
464 hud.y = guip.find("y").asFloat64();
465 hud.z = guip.find("z").asFloat64();
466 hud.alpha = guip.find("alpha").asFloat64();
467 hud.port = new FlexImagePort;
468 hud.texture = new TextureBuffer();
469 std::transform(groupName.begin(), groupName.end(), groupName.begin(), ::tolower);
470 hud.port->open(standardPortPrefix + "/" + groupName);
471
472 huds.push_back(hud);
473 }
474 }
475 else
476 {
477 guiEnabled = false;
478 }
479
480 }
481
482 getStickAsAxis = cfg.find("stick_as_axis").asBool();
483 left_frame = cfg.find("tf_left_hand_frame").asString();
484 right_frame = cfg.find("tf_right_hand_frame").asString();
485 root_frame = cfg.find("tf_root_frame").asString();
486 relative = cfg.check("hands_relative", yarp::os::Value(false)).asBool();
487
488 //getting gui information from cfg
489
490 fillAxisStorage();
491 fillButtonStorage();
492 fillErrorStorage();
493 fillHatStorage();
494
495 //opening tf client
496 tfClientCfg.put("device", cfg.find("tfDevice").asString());
497 tfClientCfg.put("local", cfg.find("tfLocal").asString());
498 tfClientCfg.put("remote", cfg.find("tfRemote").asString());
499
500 if (!driver.open(tfClientCfg))
501 {
502 yCError(OVRHEADSET) << "unable to open PolyDriver";
503 return false;
504 }
505
506 if (!driver.view(tfPublisher) || tfPublisher == nullptr)
507 {
508 yCError(OVRHEADSET) << "unable to dynamic cast device to IFrameTransform interface";
509 return false;
510 }
511 yCInfo(OVRHEADSET) << "TransformCLient successfully opened at port: " << cfg.find("tfLocal").asString();
512
513 //opening ports
514 ports =
515 {
516 { &orientationPort, "orientation" },
517 { &positionPort, "position" },
518 { &angularVelocityPort, "angularVelocity" },
519 { &linearVelocityPort, "linearVelocity" },
520 { &angularAccelerationPort, "angularAcceleration" },
521 { &linearAccelerationPort, "linearAcceleration" },
522 { &predictedOrientationPort, "predictedOrientation" },
523 { &predictedPositionPort, "predictedPosition" },
524 { &predictedAngularVelocityPort, "predictedAngularVelocity" },
525 { &predictedLinearVelocityPort, "predictedLinearVelocity" },
526 { &predictedAngularAccelerationPort, "predictedAngularAcceleration" },
527 { &predictedLinearAccelerationPort, "predictedLinearAcceleration" }
528 };
529
530 for (auto port : ports)
531 {
532 std::string name, prefix;
533 bool predicted;
534
536 predicted = port.second.find("predicted") != std::string::npos;
537 prefix = predicted ? standardPortPrefix+"/predicted" : standardPortPrefix;
538 name = prefix + "/headpose/" + port.second + ":o";
539
540 if (!(*port.first)->open(name))
541 {
542 yCError(OVRHEADSET) << "Cannot open" << port.second << "port";
543 this->close();
544 return false;
545 }
546
547 (*port.first)->setWriteOnly();
548 }
549
550 //eyes set-up
551 for (int eye = 0; eye < ovrEye_Count; ++eye) {
552 displayPorts[eye] = new InputCallback(eye);
553 if (!displayPorts[eye]->open(eye == ovrEye_Left ? "/oculus/display/left:i" : "/oculus/display/right:i")) {
554 yCError(OVRHEADSET) << "Cannot open " << (eye == ovrEye_Left ? "left" : "right") << "display port";
555 this->close();
556 return false;
557 }
558 displayPorts[eye]->setReadOnly();
559 }
560
561 texWidth = cfg.check("w", yarp::os::Value(640), "Texture width (usually same as camera width)").asInt32();
562 texHeight = cfg.check("h", yarp::os::Value(480), "Texture height (usually same as camera height)").asInt32();
563
564 // TODO accept different fov for right and left eye?
565 double hfov = cfg.check("hfov", yarp::os::Value(105.), "Camera horizontal field of view").asFloat64();
566 camHFOV[0] = hfov;
567 camHFOV[1] = hfov;
568
569 //optional params
570 optionalParams =
571 {
572 { "flipinput", "[F] Enable input flipping", &flipInputEnabled, true },
573 { "no-imagepose", "[I] Disable image pose", &imagePoseEnabled, false },
574 { "userpose", "[U] Use user pose instead of camera pose", &userPoseEnabled, true },
575 { "no-logo", "[L] Disable logo", &logoEnabled, false },
576 { "no-crosshairs", "[C] Disable crosshairs", &crosshairsEnabled, false },
577 { "no-battery", "[B] Disable battery", &batteryEnabled, false }
578 };
579
580 for (auto p : optionalParams)
581 {
582 if (cfg.check(std::get<0>(p), std::get<1>(p)))
583 {
584 *std::get<2>(p) = std::get<3>(p);
585 }
586 }
587
588 prediction = cfg.check("prediction", yarp::os::Value(0.01), "Prediction [sec]").asFloat64();
589
590 displayPorts[0]->rollOffset = static_cast<float>(cfg.check("left-roll-offset", yarp::os::Value(0.0), "[LEFT_SHIFT+PAGE_UP][LEFT_SHIFT+PAGE_DOWN] Left eye roll offset").asFloat64());
591 displayPorts[0]->pitchOffset = static_cast<float>(cfg.check("left-pitch-offset", yarp::os::Value(0.0), "[LEFT_SHIFT+UP_ARROW][LEFT_SHIFT+DOWN_ARROW] Left eye pitch offset").asFloat64());
592 displayPorts[0]->yawOffset = static_cast<float>(cfg.check("left-yaw-offset", yarp::os::Value(0.0), "[LEFT_SHIFT+LEFT_ARROW][LEFT_SHIFT+RIGHT_ARROW] Left eye yaw offset").asFloat64());
593 displayPorts[1]->rollOffset = static_cast<float>(cfg.check("right-roll-offset", yarp::os::Value(0.0), "[RIGHT_SHIFT+PAGE_UP][RIGHT_SHIFT+PAGE_DOWN] Right eye roll offset").asFloat64());
594 displayPorts[1]->pitchOffset = static_cast<float>(cfg.check("right-pitch-offset", yarp::os::Value(0.0), "[RIGHT_SHIFT+UP_ARROW][RIGHT_SHIFT+DOWN_ARROW] Right eye pitch offset").asFloat64());
595 displayPorts[1]->yawOffset = static_cast<float>(cfg.check("right-yaw-offset", yarp::os::Value(0.0), "[RIGHT_SHIFT+LEFT_ARROW][RIGHT_SHIFT+RIGHT_ARROW] Right eye yaw offset").asFloat64());
596
597 // Start the thread
598 if (!this->start()) {
599 yCError(OVRHEADSET) << "thread start failed, aborting.";
600 this->close();
601 return false;
602 }
603
604 // Enable display port callbacks
605 for (int eye = 0; eye < ovrEye_Count; ++eye) {
606 displayPorts[eye]->useCallback();
607 }
608
609 return true;
610}
611
613{
615 OVR::System::Init();
616
617 // Initializes LibOVR, and the Rift
618 ovrInitParams initParams = { ovrInit_RequestVersion | ovrInit_FocusAware, OVR_MINOR_VERSION, ovrDebugCallback, reinterpret_cast<uintptr_t>(this), 0 };
619 ovrResult r = ovr_Initialize(&initParams);
620// VALIDATE(OVR_SUCCESS(r), "Failed to initialize libOVR.");
621 if (!OVR_SUCCESS(r)) {
622 yCError(OVRHEADSET) << "Failed to initialize libOVR.";
623 }
624
625 // Detect and initialize Oculus Rift
626 ovrGraphicsLuid luid;
627 ovrResult result = ovr_Create(&session, &luid);
628 if (!OVR_SUCCESS(result)) {
629 yCError(OVRHEADSET) << "Oculus Rift not detected.";
630 this->close();
631 return false;
632 }
633
634 if (compareLuid(luid, GetDefaultAdapterLuid())) // If luid that the Rift is on is not the default adapter LUID...
635 {
636 yCError(OVRHEADSET) << "OpenGL supports only the default graphics adapter.";
637 this->close();
638 return false;
639 }
640
641 // FIXME: Which one is better in this case?
642 // ovrTrackingOrigin_FloorLevel will give tracking poses where the floor height is 0
643 // ovrTrackingOrigin_EyeLevel will give tracking poses where the eye height is 0
644 ovr_SetTrackingOriginType(session, ovrTrackingOrigin_EyeLevel);
645
646 hmdDesc = ovr_GetHmdDesc(session);
647 if (hmdDesc.ProductName[0] == '\0') {
648 yCWarning(OVRHEADSET) << "Rift detected, display not enabled.";
649 }
650
651 DebugHmd(hmdDesc);
652
653 // Initialize the GLFW system for creating and positioning windows
654 // GLFW must be initialized after LibOVR
655 // see http://www.glfw.org/docs/latest/rift.html
656 if ( !glfwInit() ) {
657 yCError(OVRHEADSET) << "Failed to initialize GLFW";
658 this->close();
659 return false;
660 }
661 glfwSetErrorCallback(glfwErrorCallback);
662
663 OVR::Sizei windowSize = hmdDesc.Resolution;
664
665 if (!createWindow(windowSize.w, windowSize.h)) {
666 yCError(OVRHEADSET) << "Failed to create window";
667 this->close();
668 return false;
669 }
670
671
672 // Initialize the GLEW OpenGL 3.x bindings
673 // GLEW must be initialized after creating the window
674 glewExperimental = GL_TRUE;
675 GLenum err = glewInit();
676 if (err != GLEW_OK) {
677 yCError(OVRHEADSET) << "glewInit failed, aborting.";
678 this->close();
679 return false;
680 }
681 yCInfo(OVRHEADSET) << "Using GLEW" << (const char*)glewGetString(GLEW_VERSION);
683
684
685 int fbwidth, fbheight;
686 glfwGetFramebufferSize(window, &fbwidth, &fbheight);
687
688 for (int eye = 0; eye < ovrEye_Count; ++eye) {
689 camWidth[eye] = texWidth;
690 camHeight[eye] = texHeight;
691 }
692 reconfigureFOV();
693
694 reconfigureRendering();
695
696 for (int eye = 0; eye < ovrEye_Count; ++eye) {
697 displayPorts[eye]->eyeRenderTexture = new TextureBuffer(texWidth, texHeight, eye, session);
698 }
699
700 textureLogo = new TextureStatic(session, yarp_logo);
701 textureCrosshairs = new TextureStatic(session, crosshairs);
702 textureBattery = new TextureBattery(session, batteryEnabled);
703
704 ovrMirrorTextureDesc desc;
705 memset(&desc, 0, sizeof(desc));
706 desc.Width = windowSize.w;
707 desc.Height = windowSize.h;
708 desc.Format = OVR_FORMAT_R8G8B8A8_UNORM_SRGB;
709
710 // Create mirror texture and an FBO used to copy mirror texture to back buffer
711 result = ovr_CreateMirrorTextureGL(session, &desc, &mirrorTexture);
712 if (!OVR_SUCCESS(result))
713 {
714 yCError(OVRHEADSET) << "Failed to create mirror texture.";
715 this->close();
716 return false;
717 }
718
719 // Configure the mirror read buffer
720 GLuint texId;
721 ovr_GetMirrorTextureBufferGL(session, mirrorTexture, &texId);
722
723 glGenFramebuffers(1, &mirrorFBO);
724 glBindFramebuffer(GL_READ_FRAMEBUFFER, mirrorFBO);
725 glFramebufferTexture2D(GL_READ_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texId, 0);
726 glFramebufferRenderbuffer(GL_READ_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, 0);
727 glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
728
729 // Recenter position
730 ovr_RecenterTrackingOrigin(session);
731
733
734 return true;
735}
736
738{
740
741 // Ensure that threadRelease is not called twice
742 if (closed) {
743 return;
744 }
745 closed = true;
746
747 if (mirrorFBO) {
748 glDeleteFramebuffers(1, &mirrorFBO);
749 }
750
751 if (mirrorTexture) {
752 ovr_DestroyMirrorTexture(session, mirrorTexture);
753 }
754
755 if (textureLogo) {
756 delete textureLogo;
757 textureLogo = nullptr;
758 }
759
760 if (textureCrosshairs) {
761 delete textureCrosshairs;
762 textureCrosshairs = nullptr;
763 }
764
765 if (textureBattery) {
766 delete textureBattery;
767 textureBattery = nullptr;
768 }
769
770 // Shut down GLFW
771 glfwTerminate();
772
773 // Shut down LibOVR
774 if (session) {
775 // Disable Performance Hud Mode before destroying the session,
776 // or it will stay after the device is closed.
777 int PerfHudMode = (int)ovrPerfHud_Off;
778 ovr_SetInt(session, OVR_PERF_HUD_MODE, PerfHudMode);
779
780 ovr_Destroy(session);
781 session = 0;
782 ovr_Shutdown();
783 }
784 std::vector<yarp::os::Contactable*> ports;
785
786 ports.push_back(orientationPort);
787 ports.push_back(positionPort);
788 ports.push_back(angularVelocityPort);
789 ports.push_back(linearVelocityPort);
790 ports.push_back(angularAccelerationPort);
791 ports.push_back(linearAccelerationPort);
792 ports.push_back(predictedOrientationPort);
793 ports.push_back(predictedPositionPort);
794 ports.push_back(predictedAngularVelocityPort);
795 ports.push_back(predictedLinearVelocityPort);
796 ports.push_back(predictedAngularAccelerationPort);
797 ports.push_back(predictedLinearAccelerationPort);
798
799 for (auto& hud : huds)
800 {
801 delete hud.texture;
802 ports.push_back(hud.port);
803 }
804
805 for (auto& p : ports)
806 {
807 if (p != nullptr)
808 {
809 p->interrupt();
810 p->close();
811 delete p;
812 p = nullptr;
813 }
814 }
815
816
817 for (int eye = 0; eye < ovrEye_Count; ++eye) {
818 if (displayPorts[eye]) {
819 displayPorts[eye]->disableCallback();
820 displayPorts[eye]->interrupt();
821 displayPorts[eye]->close();
822 delete displayPorts[eye];
823 displayPorts[eye] = nullptr;
824 }
825 }
826}
827
829{
831 this->askToStop();
832 return true;
833}
834
836{
838 return false;
839}
840
842{
843 if (closed) {
844 return false;
845 }
846
847 constexpr double delay = 60.0;
849 "Thread ran %d times, est period %lf[ms], used %lf[ms]",
850 getIterations(),
851 getEstimatedPeriod()*1000,
852 getEstimatedUsed()*1000);
854 "Display refresh: %3.1f[hz]", getIterations()/delay);
855
856 for (int eye = 0; eye < ovrEye_Count; ++eye) {
858 "%s eye: %3.1f[hz] - %d of %d frames missing, %d of %d frames dropped",
859 (eye == ovrEye_Left ? "Left " : "Right"),
860 (getIterations() - displayPorts[eye]->eyeRenderTexture->missingFrames) / delay,
861 displayPorts[eye]->eyeRenderTexture->missingFrames,
862 getIterations(),
863 displayPorts[eye]->droppedFrames,
864 getIterations());
865 displayPorts[eye]->eyeRenderTexture->missingFrames = 0;
866 getIterations(),
867 displayPorts[eye]->droppedFrames = 0;
868 }
869
870 resetStat();
871
873 return !closed;
874}
875
877{
879 return this->close();
880}
881
882void yarp::dev::OVRHeadset::resetInput()
883{
884 inputStateMutex.lock();
885 inputState.Buttons = 0;
886 inputState.HandTrigger[0] = 0;
887 inputState.HandTrigger[1] = 0;
888 inputState.IndexTrigger[0] = 0;
889 inputState.IndexTrigger[1] = 0;
890 inputState.Thumbstick[0].x = 0;
891 inputState.Thumbstick[0].y = 0;
892 inputState.Thumbstick[1].x = 0;
893 inputState.Thumbstick[1].y = 0;
894 inputStateMutex.unlock();
895}
896
898{
899 ovrResult result = ovrError_InvalidSession;
900 ovrSessionStatus sessionStatus;
901
902 if (glfwWindowShouldClose(window)) {
903 resetInput();
904 close();
905 return;
906 }
907
908 ovr_GetSessionStatus(session, &sessionStatus);
909 if (sessionStatus.ShouldQuit) {
910 resetInput();
911 close();
912 return;
913 }
914 if (sessionStatus.ShouldRecenter) {
915 ovr_RecenterTrackingOrigin(session);
916 }
917
918 // Check window events;
919 glfwPollEvents();
920
921 if (!sessionStatus.IsVisible) {
922 resetInput();
923 return;
924 }
925
926 if (!sessionStatus.HasInputFocus) {
927 // return;
928 }
929
930 // Begin frame
931 ++distortionFrameIndex;
932 double frameTiming = ovr_GetPredictedDisplayTime(session, distortionFrameIndex);
933 YARP_UNUSED(frameTiming);
934
935 // Query the HMD for the current tracking state.
936 ts = ovr_GetTrackingState(session, ovr_GetTimeInSeconds(), false);
937 headpose = ts.HeadPose;
938 yarp::os::Stamp stamp(distortionFrameIndex, ts.HeadPose.TimeInSeconds);
939
940 //Get eye poses, feeding in correct IPD offset
941 ovrPosef ViewPose[2] = {EyeRenderDesc[0].HmdToEyePose,EyeRenderDesc[1].HmdToEyePose};
942 ovrPosef EyeRenderPose[2];
943 ovr_CalcEyePoses(headpose.ThePose, ViewPose, EyeRenderPose);
944
945 // Query the HMD for the predicted state
946 ovrTrackingState predicted_ts = ovr_GetTrackingState(session, ovr_GetTimeInSeconds() + prediction, false);
947 ovrPoseStatef predicted_headpose = predicted_ts.HeadPose;
948 yarp::os::Stamp predicted_stamp(distortionFrameIndex, predicted_ts.HeadPose.TimeInSeconds);
949
950 //send hands frames
951 if (relative)
952 {
953 yarp::sig::Matrix T_Conv(4, 4), T_Head(4, 4), T_LHand(4, 4), T_RHand(4, 4), T_robotHead(4, 4);
954 yarp::sig::Vector rpyHead, rpyRobot;
955
956 tfPublisher->getTransform("head_link", "mobile_base_body_link", T_robotHead);
957 ovrVector3f& leftH = ts.HandPoses[ovrHand_Left].ThePose.Position;
958 ovrVector3f& rightH = ts.HandPoses[ovrHand_Right].ThePose.Position;
959
960 T_RHand = ovr2matrix(vecSubtract(rightH, headpose.ThePose.Position), OVR::Quatf(ts.HandPoses[ovrHand_Right].ThePose.Orientation) * OVR::Quatf(OVR::Vector3f(0, 0, 1), M_PI_2));
961 T_LHand = ovr2matrix(vecSubtract(leftH, headpose.ThePose.Position), OVR::Quatf(ts.HandPoses[ovrHand_Left].ThePose.Orientation) * OVR::Quatf(OVR::Vector3f(0, 0, 1), M_PI_2));
962 T_Head = ovr2matrix(headpose.ThePose.Position, headpose.ThePose.Orientation);
963 rpyHead = yarp::math::dcm2rpy(T_Head);
964 rpyRobot = yarp::math::dcm2rpy(T_robotHead);
965 rpyHead[0] = 0;
966 rpyHead[1] = rpyRobot[1];
967 rpyHead[2] = rpyRobot[2];
968 T_Head = yarp::math::rpy2dcm(rpyHead);
969
970 tfPublisher->setTransform(left_frame, root_frame, operator*(T_Head.transposed(), T_LHand));
971 tfPublisher->setTransform(right_frame, root_frame, operator*(T_Head.transposed(), T_RHand));
972 }
973
974 else
975
976 {
977 OVR::Quatf lRot = OVR::Quatf(ts.HandPoses[ovrHand_Left].ThePose.Orientation) * OVR::Quatf(OVR::Vector3f(0, 0, 1), M_PI_2);
978 OVR::Quatf rRot = OVR::Quatf(ts.HandPoses[ovrHand_Right].ThePose.Orientation) * OVR::Quatf(OVR::Vector3f(0, 0, 1), M_PI_2);
979 tfPublisher->setTransform(left_frame, "mobile_base_body_link", ovr2matrix(ts.HandPoses[ovrHand_Left].ThePose.Position, lRot));
980 tfPublisher->setTransform(right_frame, "mobile_base_body_link", ovr2matrix(ts.HandPoses[ovrHand_Right].ThePose.Position, rRot));
981 }
982
983 //tfPublisher->setTransform(right_frame, root_frame, yarp::math::operator*(T_Head.transposed(), T_RHand));
984
985 // Get Input State
986 inputStateMutex.lock();
987 result = ovr_GetInputState(session, ovrControllerType_Active, &inputState);
988 inputStateMutex.unlock();
989 if (!OVR_SUCCESS(result))
990 {
991 errorManager(result);
992 inputStateError = true;
993 }
994
995 // Read orientation and write it on the port
996 if (ts.StatusFlags & ovrStatus_OrientationTracked) {
997
998 if (orientationPort->getOutputCount() > 0) {
999 OVR::Quatf orientation = headpose.ThePose.Orientation;
1000 float yaw, pitch, roll;
1001 orientation.GetEulerAngles<OVR::Axis_Y, OVR::Axis_X, OVR::Axis_Z>(&yaw, &pitch, &roll);
1002 yarp::os::Bottle& output_orientation = orientationPort->prepare();
1003 output_orientation.clear();
1004 output_orientation.addFloat64(OVR::RadToDegree(pitch));
1005 output_orientation.addFloat64(OVR::RadToDegree(-roll));
1006 output_orientation.addFloat64(OVR::RadToDegree(yaw));
1007 orientationPort->setEnvelope(stamp);
1008 orientationPort->write();
1009 }
1010
1011 writeVec3OnPort(angularVelocityPort, radToDeg(headpose.AngularVelocity), stamp);
1012 writeVec3OnPort(angularAccelerationPort, radToDeg(headpose.AngularAcceleration), stamp);
1013
1014 } else {
1015 // Do not warn more than once every 5 seconds
1016 static double lastOrientWarnTime = 0;
1017 double now = yarp::os::SystemClock::nowSystem();
1018 if (now >= lastOrientWarnTime + 5) {
1019 yCDebug(OVRHEADSET) << "Orientation not tracked";
1020 lastOrientWarnTime = now;
1021 }
1022 }
1023
1024 // Read position and write it on the port
1025 if (ts.StatusFlags & ovrStatus_PositionTracked) {
1026 writeVec3OnPort(positionPort, headpose.ThePose.Position, stamp);
1027 writeVec3OnPort(linearVelocityPort, headpose.LinearVelocity, stamp);
1028 writeVec3OnPort(linearAccelerationPort, headpose.LinearAcceleration, stamp);
1029
1030 } else {
1031 // Do not warn more than once every 5 seconds
1032 static double lastPosWarnTime = 0;
1033 double now = yarp::os::SystemClock::nowSystem();
1034 if (now >= lastPosWarnTime + 5) {
1035 yCDebug(OVRHEADSET) << "Position not tracked";
1036 lastPosWarnTime = now;
1037 }
1038 }
1039
1040 // Read predicted orientation and write it on the port
1041 if (predicted_ts.StatusFlags & ovrStatus_OrientationTracked) {
1042
1043 if (predictedOrientationPort->getOutputCount() > 0) {
1044 OVR::Quatf orientation = predicted_headpose.ThePose.Orientation;
1045 float yaw, pitch, roll;
1046 orientation.GetEulerAngles<OVR::Axis_Y, OVR::Axis_X, OVR::Axis_Z>(&yaw, &pitch, &roll);
1047 yarp::os::Bottle& output_orientation = predictedOrientationPort->prepare();
1048 output_orientation.clear();
1049 output_orientation.addFloat64(OVR::RadToDegree(pitch));
1050 output_orientation.addFloat64(OVR::RadToDegree(-roll));
1051 output_orientation.addFloat64(OVR::RadToDegree(yaw));
1052 predictedOrientationPort->setEnvelope(predicted_stamp);
1053 predictedOrientationPort->write();
1054 }
1055
1056 writeVec3OnPort(predictedAngularVelocityPort, radToDeg(predicted_headpose.AngularVelocity), stamp);
1057 writeVec3OnPort(predictedAngularAccelerationPort, radToDeg(predicted_headpose.AngularAcceleration), stamp);
1058
1059 } else {
1060 // Do not warn more than once every 5 seconds
1061 static double lastPredOrientWarnTime = 0;
1062 double now = yarp::os::SystemClock::nowSystem();
1063 if (now >= lastPredOrientWarnTime + 5) {
1064 yCDebug(OVRHEADSET) << "Predicted orientation not tracked";
1065 lastPredOrientWarnTime = now;
1066 }
1067 }
1068
1069 // Read predicted position and write it on the port
1070 if (predicted_ts.StatusFlags & ovrStatus_PositionTracked) {
1071
1072 writeVec3OnPort(predictedPositionPort, predicted_headpose.ThePose.Position, stamp);
1073 writeVec3OnPort(predictedLinearVelocityPort, predicted_headpose.LinearVelocity, stamp);
1074 writeVec3OnPort(predictedLinearAccelerationPort, predicted_headpose.LinearAcceleration, stamp);
1075
1076 } else {
1077 // Do not warn more than once every 5 seconds
1078 static double lastPredPosWarnTime = 0;
1079 double now = yarp::os::SystemClock::nowSystem();
1080 if (now >= lastPredPosWarnTime + 5) {
1081 yCDebug(OVRHEADSET) << "Position not tracked";
1082 lastPredPosWarnTime = now;
1083 }
1084 }
1085
1086
1087 if (displayPorts[0]->eyeRenderTexture && displayPorts[1]->eyeRenderTexture) {
1088 // Do distortion rendering, Present and flush/sync
1089
1090 // Update the textures
1091 for (int eye = 0; eye < ovrEye_Count; ++eye) {
1092 displayPorts[eye]->eyeRenderTexture->update();
1093 }
1094
1095
1096 for (int eye = 0; eye < ovrEye_Count; ++eye) {
1097 if (imagePoseEnabled) {
1098 if (userPoseEnabled) {
1099 // Use orientation read from the HMD at the beginning of the frame
1100 EyeRenderPose[eye].Orientation = headpose.ThePose.Orientation;
1101 } else {
1102 // Use orientation received from the image
1103 EyeRenderPose[eye].Orientation = displayPorts[eye]->eyeRenderTexture->eyePose.Orientation;
1104 }
1105 } else {
1106 EyeRenderPose[eye].Orientation.w = -1.0f;
1107 EyeRenderPose[eye].Orientation.x = 0.0f;
1108 EyeRenderPose[eye].Orientation.y = 0.0f;
1109 EyeRenderPose[eye].Orientation.z = 0.0f;
1110 }
1111 }
1112
1113 // If the image size is different from the texture size,
1114 bool needReconfigureFOV = false;
1115 for (int eye = 0; eye < ovrEye_Count; ++eye) {
1116 if ((displayPorts[eye]->eyeRenderTexture->imageWidth != 0 && displayPorts[eye]->eyeRenderTexture->imageWidth != camWidth[eye]) ||
1117 (displayPorts[eye]->eyeRenderTexture->imageHeight != 0 && displayPorts[eye]->eyeRenderTexture->imageHeight != camHeight[eye])) {
1118
1119 camWidth[eye] = displayPorts[eye]->eyeRenderTexture->imageWidth;
1120 camHeight[eye] = displayPorts[eye]->eyeRenderTexture->imageHeight;
1121 needReconfigureFOV = true;
1122 }
1123 }
1124 if (needReconfigureFOV) {
1125 reconfigureFOV();
1126 reconfigureRendering();
1127 }
1128
1129 std::list<ovrLayerHeader*> layerList;
1130
1131 ovrLayerEyeFov eyeLayer;
1132 eyeLayer.Header.Type = ovrLayerType_EyeFov;
1133 eyeLayer.Header.Flags = ovrLayerFlag_HighQuality;
1134 if (flipInputEnabled) {
1135 eyeLayer.Header.Flags |= ovrLayerFlag_TextureOriginAtBottomLeft;
1136 }
1137 for (int eye = 0; eye < 2; ++eye) {
1138 eyeLayer.ColorTexture[eye] = displayPorts[eye]->eyeRenderTexture->textureSwapChain;
1139 eyeLayer.Viewport[eye] = OVR::Recti(0, 0, displayPorts[eye]->eyeRenderTexture->width, displayPorts[eye]->eyeRenderTexture->height);
1140 eyeLayer.Fov[eye] = fov[eye];
1141 eyeLayer.RenderPose[eye] = EyeRenderPose[eye];
1142 }
1143 layerList.push_back(&eyeLayer.Header);
1144
1145 if (logoEnabled) {
1146 setHeadLockedLayer(logoLayer, textureLogo, 0.2f, -0.2f, -0.5f, 0.0f, 0.0f, 0.0f, 1.0f, 0.05f, 0.05f);
1147 layerList.push_back(&logoLayer.Header);
1148 }
1149
1150 if (crosshairsEnabled) {
1151 setHeadLockedLayer(crosshairsLayer, textureCrosshairs, 0.0f, 0.0f, -5.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.08f, 0.08f);
1152 layerList.push_back(&crosshairsLayer.Header);
1153 }
1154
1155 if (batteryEnabled) {
1156 setHeadLockedLayer(batteryLayer, textureBattery->currentTexture, 0.25f, 0.25f, -0.50f, 0.0f, 0.0f, 0.0f, 1.0f, 0.05f, 0.05f);
1157 layerList.push_back(&batteryLayer.Header);
1158 }
1159
1160 //setting up dynamic hud
1161 if (guiEnabled)
1162 {
1163 for (auto& hud : huds)
1164 {
1165 if (!hud.port->getInputCount())
1166 {
1167 continue;
1168 }
1169
1170 yarp::sig::FlexImage* image = hud.port->read(false);
1171
1172 if (!image)
1173 {
1174 layerList.push_back(&hud.layer.Header);
1175 continue;
1176 }
1177
1178 hud.texture->fromImage(session, *image, hud.alpha);
1179 setHeadLockedLayer(hud.layer, hud.texture, hud.x, hud.y, hud.z, 0.0f, 0.0f, 0.0f, 1.0f, hud.resizeW, hud.resizeH);
1180 layerList.push_back(&hud.layer.Header);
1181 }
1182 }
1183
1184 ovrLayerHeader** layers = new ovrLayerHeader*[layerList.size()];
1185 std::copy(layerList.begin(), layerList.end(), layers);
1186
1187 ovr_WaitToBeginFrame(session, distortionFrameIndex);
1188 ovr_BeginFrame(session, distortionFrameIndex);
1189 ovr_EndFrame(session, distortionFrameIndex, nullptr, layers, layerList.size());
1190 delete[] layers;
1191
1192 // Blit mirror texture to back buffer
1193 glBindFramebuffer(GL_READ_FRAMEBUFFER, mirrorFBO);
1194
1195 glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
1196 GLint bw = hmdDesc.Resolution.w;
1197 GLint bh = hmdDesc.Resolution.h;
1198 GLint ww, wh;
1199 glfwGetWindowSize(window, &ww, &wh);
1200 glBlitFramebuffer(0, bh, bw, 0, 0, 0, ww, wh, GL_COLOR_BUFFER_BIT, GL_NEAREST);
1201 glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
1202
1204
1205 glfwSwapBuffers(window);
1206
1207 } else {
1208 // Do not warn more than once every 5 seconds
1209 static double lastImgWarnTime = 0;
1210 double now = yarp::os::SystemClock::nowSystem();
1211 if (now >= lastImgWarnTime + 5) {
1212 yCDebug(OVRHEADSET) << "No image received";
1213 lastImgWarnTime = now;
1214 }
1215 }
1216}
1217
1218bool yarp::dev::OVRHeadset::createWindow(int w, int h)
1219{
1221 glfwWindowHint(GLFW_DEPTH_BITS, 16);
1222 window = glfwCreateWindow(w/2, h/2, "YARP Oculus", nullptr, nullptr);
1223 if (!window) {
1224 yCError(OVRHEADSET) << "Could not create window";
1225 return false;
1226 }
1227
1228 glfwSetWindowUserPointer(window, this);
1229 glfwSetKeyCallback(window, glfwKeyCallback);
1230 glfwMakeContextCurrent(window);
1231
1232 return true;
1233}
1234
1235void yarp::dev::OVRHeadset::onKey(int key, int scancode, int action, int mods)
1236{
1238
1239 if (GLFW_PRESS != action) {
1240 return;
1241 }
1242
1243 bool leftShiftPressed = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS);
1244 bool rightShiftPressed = (glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS);
1245 bool leftCtrlPressed = (glfwGetKey(window, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS);
1246 bool rightCtrlPressed = (glfwGetKey(window, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS);
1247 bool leftAltPressed = (glfwGetKey(window, GLFW_KEY_LEFT_ALT) == GLFW_PRESS);
1248 bool rightAltPressed = (glfwGetKey(window, GLFW_KEY_RIGHT_ALT) == GLFW_PRESS);
1249 bool shiftPressed = leftShiftPressed || rightShiftPressed;
1250 bool ctrlPressed = leftCtrlPressed || rightCtrlPressed;
1251 bool altPressed = leftAltPressed || rightAltPressed;
1252
1253 switch (key) {
1254 case GLFW_KEY_R:
1255
1256 if (!leftShiftPressed && !rightShiftPressed) {
1257 yCDebug(OVRHEADSET) << "Recentering pose";
1258 ovr_RecenterTrackingOrigin(session);
1259 } else {
1260 yCDebug(OVRHEADSET) << "Resetting yaw offset to current position";
1261 for (int eye = 0; eye < ovrEye_Count; ++eye) {
1262 float iyaw, ipitch, iroll;
1263 if (imagePoseEnabled) {
1264 OVR::Quatf imageOrientation = displayPorts[eye]->eyeRenderTexture->eyePose.Orientation;
1265 imageOrientation.GetEulerAngles<OVR::Axis_Y, OVR::Axis_X, OVR::Axis_Z>(&iyaw, &ipitch, &iroll);
1266 } else {
1267 iyaw = 0.0f;
1268 ipitch = 0.0f;
1269 iyaw = 0.0f;
1270 }
1271
1272 iyaw -= displayPorts[eye]->yawOffset;
1273 displayPorts[eye]->yawOffset = - iyaw;
1274 yCDebug(OVRHEADSET) << (eye == ovrEye_Left? "Left" : "Right") << "eye yaw offset =" << displayPorts[eye]->yawOffset;
1275 }
1276 }
1277 break;
1278 case GLFW_KEY_F:
1279 flipInputEnabled = !flipInputEnabled;
1280 yCDebug(OVRHEADSET) << "Flip input" << (flipInputEnabled ? "ON" : "OFF");
1281 reconfigureRendering();
1282 break;
1283 case GLFW_KEY_I:
1284 imagePoseEnabled = !imagePoseEnabled;
1285 yCDebug(OVRHEADSET) << "Image pose" << (imagePoseEnabled ? "ON" : "OFF");
1286 yCDebug(OVRHEADSET) << "User pose" << (userPoseEnabled ? "ON" : "OFF");
1287 break;
1288 case GLFW_KEY_U:
1289 userPoseEnabled = !userPoseEnabled;
1290 yCDebug(OVRHEADSET) << "Image pose" << (imagePoseEnabled ? "ON" : "OFF");
1291 yCDebug(OVRHEADSET) << "User pose" << (userPoseEnabled ? "ON" : "OFF");
1292 break;
1293 case GLFW_KEY_L:
1294 logoEnabled = !logoEnabled;
1295 yCDebug(OVRHEADSET) << "Overlays:" <<
1296 "Logo" << (logoEnabled ? "ON" : "OFF") <<
1297 "Crosshairs" << (crosshairsEnabled ? "ON" : "OFF") <<
1298 "Battery" << (batteryEnabled ? "ON" : "OFF") <<
1299 "Gui" << ((guiCount != 0) ? (guiEnabled ? "ON" : "OFF") : "DISABLED");
1300 break;
1301 case GLFW_KEY_C:
1302 crosshairsEnabled = !crosshairsEnabled;
1303 yCDebug(OVRHEADSET) << "Overlays:" <<
1304 "Logo" << (logoEnabled ? "ON" : "OFF") <<
1305 "Crosshairs" << (crosshairsEnabled ? "ON" : "OFF") <<
1306 "Battery" << (batteryEnabled ? "ON" : "OFF") <<
1307 "Gui" << ((guiCount != 0) ? (guiEnabled ? "ON" : "OFF") : "DISABLED");
1308 break;
1309 case GLFW_KEY_B:
1310 batteryEnabled = !batteryEnabled;
1311 if (batteryEnabled) {
1312 textureBattery->resume();
1313 } else {
1314 textureBattery->suspend();
1315 }
1316 yCDebug(OVRHEADSET) << "Overlays:" <<
1317 "Logo" << (logoEnabled ? "ON" : "OFF") <<
1318 "Crosshairs" << (crosshairsEnabled ? "ON" : "OFF") <<
1319 "Battery" << (batteryEnabled ? "ON" : "OFF") <<
1320 "Gui" << ((guiCount != 0) ? (guiEnabled ? "ON" : "OFF") : "DISABLED");
1321 break;
1322 case GLFW_KEY_G:
1323 if (guiCount != 0) {
1324 guiEnabled = !guiEnabled;
1325 }
1326 yCDebug(OVRHEADSET) << "Overlays:" <<
1327 "Logo" << (logoEnabled ? "ON" : "OFF") <<
1328 "Crosshairs" << (crosshairsEnabled ? "ON" : "OFF") <<
1329 "Battery" << (batteryEnabled ? "ON" : "OFF") <<
1330 "Gui" << ((guiCount != 0) ? (guiEnabled ? "ON" : "OFF") : "DISABLED") ;
1331 break;
1332 case GLFW_KEY_ESCAPE:
1333 this->close();
1334 break;
1335 case GLFW_KEY_Z:
1336 if (!rightShiftPressed) {
1337 --camHFOV[0];
1338 yCDebug(OVRHEADSET) << "Left eye HFOV =" << camHFOV[0];
1339 }
1340 if (!leftShiftPressed) {
1341 --camHFOV[1];
1342 yCDebug(OVRHEADSET) << "Right eye HFOV =" << camHFOV[1];
1343 }
1344 reconfigureFOV();
1345 reconfigureRendering();
1346 break;
1347 case GLFW_KEY_X:
1348 if (!rightShiftPressed) {
1349 ++camHFOV[0];
1350 yCDebug(OVRHEADSET) << "Left eye HFOV =" << camHFOV[0];
1351 }
1352 if (!leftShiftPressed) {
1353 ++camHFOV[1];
1354 yCDebug(OVRHEADSET) << "Right eye HFOV =" << camHFOV[1];
1355 }
1356 reconfigureFOV();
1357 reconfigureRendering();
1358 break;
1359 case GLFW_KEY_UP:
1360 if (!rightShiftPressed) {
1361 displayPorts[0]->pitchOffset += ctrlPressed ? 0.05f : 0.0025f;
1362 yCDebug(OVRHEADSET) << "Left eye pitch offset =" << displayPorts[0]->pitchOffset;
1363 }
1364 if (!leftShiftPressed) {
1365 displayPorts[1]->pitchOffset += ctrlPressed ? 0.05f : 0.0025f;
1366 yCDebug(OVRHEADSET) << "Right eye pitch offset =" << displayPorts[1]->pitchOffset;
1367 }
1368 break;
1369 case GLFW_KEY_DOWN:
1370 if (!rightShiftPressed) {
1371 displayPorts[0]->pitchOffset -= ctrlPressed ? 0.05f : 0.0025f;
1372 yCDebug(OVRHEADSET) << "Left eye pitch offset =" << displayPorts[0]->pitchOffset;
1373 }
1374 if (!leftShiftPressed) {
1375 displayPorts[1]->pitchOffset -= ctrlPressed ? 0.05f : 0.0025f;
1376 yCDebug(OVRHEADSET) << "Right eye pitch offset =" << displayPorts[1]->pitchOffset;
1377 }
1378 break;
1379 case GLFW_KEY_LEFT:
1380 if (!rightShiftPressed) {
1381 displayPorts[0]->yawOffset += ctrlPressed ? 0.05f : 0.0025f;
1382 yCDebug(OVRHEADSET) << "Left eye yaw offset =" << displayPorts[0]->yawOffset;
1383 }
1384 if (!leftShiftPressed) {
1385 displayPorts[1]->yawOffset += ctrlPressed ? 0.05f : 0.0025f;
1386 yCDebug(OVRHEADSET) << "Right eye yaw offset =" << displayPorts[1]->yawOffset;
1387 }
1388 break;
1389 case GLFW_KEY_RIGHT:
1390 if (!rightShiftPressed) {
1391 displayPorts[0]->yawOffset -= ctrlPressed ? 0.05f : 0.0025f;
1392 yCDebug(OVRHEADSET) << "Left eye yaw offset =" << displayPorts[0]->yawOffset;
1393 }
1394 if (!leftShiftPressed) {
1395 displayPorts[1]->yawOffset -= ctrlPressed ? 0.05f : 0.0025f;
1396 yCDebug(OVRHEADSET) << "Right eye yaw offset =" << displayPorts[1]->yawOffset;
1397 }
1398 break;
1399 case GLFW_KEY_PAGE_UP:
1400 if (!rightShiftPressed) {
1401 displayPorts[0]->rollOffset += ctrlPressed ? 0.05f : 0.0025f;
1402 yCDebug(OVRHEADSET) << "Left eye roll offset =" << displayPorts[0]->rollOffset;
1403 }
1404 if (!leftShiftPressed) {
1405 displayPorts[1]->rollOffset += ctrlPressed ? 0.05f : 0.0025f;
1406 yCDebug(OVRHEADSET) << "Right eye roll offset =" << displayPorts[1]->rollOffset;
1407 }
1408 break;
1409 case GLFW_KEY_PAGE_DOWN:
1410 if (!rightShiftPressed) {
1411 displayPorts[0]->rollOffset -= ctrlPressed ? 0.05f : 0.0025f;
1412 yCDebug(OVRHEADSET) << "Left eye roll offset =" << displayPorts[0]->rollOffset;
1413 }
1414 if (!leftShiftPressed) {
1415 displayPorts[1]->rollOffset -= ctrlPressed ? 0.05f : 0.0025f;
1416 yCDebug(OVRHEADSET) << "Right eye roll offset =" << displayPorts[1]->rollOffset;
1417 }
1418 break;
1419 case GLFW_KEY_SLASH:
1420 {
1421 int PerfHudMode = ovr_GetInt(session, OVR_PERF_HUD_MODE, 0);
1422 PerfHudMode = (PerfHudMode + 1) % 8;
1423 ovr_SetInt(session, OVR_PERF_HUD_MODE, PerfHudMode);
1424 }
1425 break;
1426 case GLFW_KEY_P:
1427 yCDebug(OVRHEADSET) << "--------------------------------------------";
1428 yCDebug(OVRHEADSET) << "Current settings:";
1429 yCDebug(OVRHEADSET) << " Flip input" << (flipInputEnabled ? "ON" : "OFF");
1430 yCDebug(OVRHEADSET) << " Image pose" << (imagePoseEnabled ? "ON" : "OFF");
1431 yCDebug(OVRHEADSET) << " User pose" << (userPoseEnabled ? "ON" : "OFF");
1432 yCDebug(OVRHEADSET) << " Overlays:";
1433 yCDebug(OVRHEADSET) << " Logo" << (logoEnabled ? "ON" : "OFF");
1434 yCDebug(OVRHEADSET) << " Crosshairs" << (crosshairsEnabled ? "ON" : "OFF");
1435 yCDebug(OVRHEADSET) << " Battery" << (batteryEnabled ? "ON" : "OFF");
1436 yCDebug(OVRHEADSET) << " Gui" << ((guiCount != 0) ? (guiEnabled ? "ON" : "OFF") : "DISABLED");
1437 yCDebug(OVRHEADSET) << " Left eye:";
1438 yCDebug(OVRHEADSET) << " HFOV = " << camHFOV[0];
1439 yCDebug(OVRHEADSET) << " pitch offset =" << displayPorts[0]->pitchOffset;
1440 yCDebug(OVRHEADSET) << " yaw offset =" << displayPorts[0]->yawOffset;
1441 yCDebug(OVRHEADSET) << " roll offset =" << displayPorts[0]->rollOffset;
1442 yCDebug(OVRHEADSET) << " Right eye:";
1443 yCDebug(OVRHEADSET) << " HFOV =" << camHFOV[1];
1444 yCDebug(OVRHEADSET) << " pitch offset =" << displayPorts[1]->pitchOffset;
1445 yCDebug(OVRHEADSET) << " yaw offset =" << displayPorts[1]->yawOffset;
1446 yCDebug(OVRHEADSET) << " roll offset =" << displayPorts[1]->rollOffset;
1447 yCDebug(OVRHEADSET) << "--------------------------------------------";
1448 break;
1449 default:
1450 break;
1451 }
1452}
1453
1454void yarp::dev::OVRHeadset::reconfigureRendering()
1455{
1456 for (int eye = 0; eye < ovrEye_Count; ++eye) {
1457 ovr_GetRenderDesc(session, (ovrEyeType)eye, fov[eye]);
1458 }
1459}
1460
1461void yarp::dev::OVRHeadset::reconfigureFOV()
1462{
1463 for (int eye = 0; eye < ovrEye_Count; ++eye) {
1464 double camHFOV_rad = OVR::DegreeToRad(camHFOV[eye]);
1465 double texCamRatio = static_cast<double>(texWidth)/camWidth[eye];
1466 double texHFOV_rad = 2 * (atan(texCamRatio * tan(camHFOV_rad/2)));
1467
1468 double aspectRatio = static_cast<double>(texWidth)/texHeight;
1469 fov[eye].UpTan = static_cast<float>(fabs(tan(texHFOV_rad/2)/aspectRatio));
1470 fov[eye].DownTan = static_cast<float>(fabs(tan(texHFOV_rad/2)/aspectRatio));
1471 fov[eye].LeftTan = static_cast<float>(fabs(tan(texHFOV_rad/2)));
1472 fov[eye].RightTan = static_cast<float>(fabs(tan(texHFOV_rad/2)));
1473 }
1474 debugFov(fov);
1475}
1476
1477void yarp::dev::OVRHeadset::glfwKeyCallback(GLFWwindow* window, int key, int scancode, int action, int mods)
1478{
1479 OVRHeadset* instance = (OVRHeadset*)glfwGetWindowUserPointer(window);
1480 instance->onKey(key, scancode, action, mods);
1481}
1482
1483void yarp::dev::OVRHeadset::glfwErrorCallback(int error, const char* description)
1484{
1485 yCError(OVRHEADSET) << error << description;
1486}
1487
1488void yarp::dev::OVRHeadset::ovrDebugCallback(uintptr_t userData, int level, const char* message)
1489{
1490 yarp::dev::OVRHeadset* ovr = reinterpret_cast<yarp::dev::OVRHeadset*>(userData);
1491 YARP_UNUSED(ovr);
1492
1493 if (!message)
1494 {
1495 return;
1496 }
1497
1498 switch (level) {
1499 case ovrLogLevel_Debug:
1500 yCDebug(OVRHEADSET) << "ovrDebugCallback" << message;
1501 break;
1502 case ovrLogLevel_Info:
1503 yCInfo(OVRHEADSET) << "ovrDebugCallback" << message;
1504 break;
1505 case ovrLogLevel_Error:
1506 yCError(OVRHEADSET) << "ovrDebugCallback" << message;
1507 break;
1508 default:
1509 yCWarning(OVRHEADSET) << "ovrDebugCallback" << message;
1510 break;
1511 }
1512}
1513
1514void yarp::dev::OVRHeadset::DebugHmd(ovrHmdDesc hmdDesc)
1515{
1516 yCDebug(OVRHEADSET, " * ProductName: %s", hmdDesc.ProductName);
1517 yCDebug(OVRHEADSET, " * Manufacturer: %s", hmdDesc.Manufacturer);
1518 yCDebug(OVRHEADSET, " * VendorId:ProductId: %04X:%04X", hmdDesc.VendorId, hmdDesc.ProductId);
1519 yCDebug(OVRHEADSET, " * SerialNumber: %s", hmdDesc.SerialNumber);
1520 yCDebug(OVRHEADSET, " * Firmware Version: %d.%d", hmdDesc.FirmwareMajor, hmdDesc.FirmwareMinor);
1521 yCDebug(OVRHEADSET, " * Resolution: %dx%d", hmdDesc.Resolution.w, hmdDesc.Resolution.h);
1522}
1523
1524void yarp::dev::OVRHeadset::errorManager(ovrResult error)
1525{
1526 if (error_messages.find(error) != error_messages.end())
1527 {
1528 yCError(OVRHEADSET) << error_messages[error].c_str();
1529 }
1530}
1531
1532
1533//IJoypadController method
1534bool yarp::dev::OVRHeadset::getAxisCount(unsigned int& axis_count)
1535{
1536 if (inputStateError) return false;
1537 axis_count = axisIdToValue.size();
1538 return true;
1539}
1540
1541bool yarp::dev::OVRHeadset::getButtonCount(unsigned int& button_count)
1542{
1543 if (inputStateError) return false;
1544 button_count = BUTTON_COUNT;
1545 return true;
1546}
1547
1548bool yarp::dev::OVRHeadset::getTrackballCount(unsigned int& Trackball_count)
1549{
1550 if (inputStateError) return false;
1551 Trackball_count = 0;
1552 return true;
1553}
1554
1555bool yarp::dev::OVRHeadset::getHatCount(unsigned int& Hat_count)
1556{
1557 if (inputStateError) return false;
1558 Hat_count = 1;
1559 return true;
1560}
1561
1562bool yarp::dev::OVRHeadset::getTouchSurfaceCount(unsigned int& touch_count)
1563{
1564 if (inputStateError) return false;
1565 touch_count = 0;
1566 return true;
1567}
1568
1569bool yarp::dev::OVRHeadset::getStickCount(unsigned int& stick_count)
1570{
1571 if (inputStateError) return false;
1572 stick_count = getStickAsAxis ? 0 : STICK_COUNT;
1573 return true;
1574}
1575
1576bool yarp::dev::OVRHeadset::getStickDoF(unsigned int stick_id, unsigned int& DoF)
1577{
1578 DoF = 2;
1579 return true;
1580}
1581
1582bool yarp::dev::OVRHeadset::getButton(unsigned int button_id, float& value)
1583{
1584 if (inputStateError) return false;
1585 std::lock_guard<std::mutex> lock(inputStateMutex);
1586 if (button_id > buttonIdToOvrButton.size() - 1)
1587 {
1588 yCError(OVRHEADSET) << "OVRHeadset: button id out of bound";
1589 return false;
1590 }
1591 value = inputState.Buttons & buttonIdToOvrButton[button_id] ? 1.0f : 0.0f;
1592 return true;
1593}
1594
1595bool yarp::dev::OVRHeadset::getTrackball(unsigned int trackball_id, yarp::sig::Vector& value)
1596{
1597 return false;
1598}
1599
1600bool yarp::dev::OVRHeadset::getHat(unsigned int hat_id, unsigned char& value)
1601{
1602 if (inputStateError) return false;
1603 std::lock_guard<std::mutex> lock(inputStateMutex);
1604 if (hat_id > 0)
1605 {
1606 yCError(OVRHEADSET) << "OVRHeadset: hat id out of bound";
1607 return false;
1608 }
1609 value = DButtonToHat[inputState.Buttons & ovrButton_Up] |
1610 DButtonToHat[inputState.Buttons & ovrButton_Down] |
1611 DButtonToHat[inputState.Buttons & ovrButton_Right] |
1612 DButtonToHat[inputState.Buttons & ovrButton_Left];
1613 return true;
1614}
1615
1616bool yarp::dev::OVRHeadset::getAxis(unsigned int axis_id, double& value)
1617{
1618 std::lock_guard<std::mutex> lock(inputStateMutex);
1619 if (axis_id > axisIdToValue.size())
1620 {
1621 yCError(OVRHEADSET) << "OVRHeadset: axis id out of bound";
1622 return false;
1623 }
1624
1625 value = *axisIdToValue[axis_id];
1626 return true;
1627}
1628
1629bool yarp::dev::OVRHeadset::getStick(unsigned int stick_id, yarp::sig::Vector& value, JoypadCtrl_coordinateMode coordinate_mode)
1630{
1631 if (inputStateError) return false;
1632 std::lock_guard<std::mutex> lock(inputStateMutex);
1633 if (getStickAsAxis)
1634 {
1635 return false;
1636 }
1637
1638 if (stick_id > STICK_COUNT - 1)
1639 {
1640 yCError(OVRHEADSET) << "stick id out of bound";
1641 return false;
1642 }
1643 value.clear();
1644 if (coordinate_mode == JoypadCtrl_coordinateMode::JypCtrlcoord_POLAR)
1645 {
1646 value.push_back(sqrt(inputState.Thumbstick[stick_id].y * inputState.Thumbstick[stick_id].y +
1647 inputState.Thumbstick[stick_id].x * inputState.Thumbstick[stick_id].x));
1648
1649 value.push_back(atan2(inputState.Thumbstick[stick_id].y, inputState.Thumbstick[stick_id].x));
1650 }
1651 value.push_back(inputState.Thumbstick[stick_id].x);
1652 value.push_back(inputState.Thumbstick[stick_id].y);
1653 return true;
1654}
1655
1656bool yarp::dev::OVRHeadset::getTouch(unsigned int touch_id, yarp::sig::Vector& value)
1657{
1658 return false;
1659}
#define checkGlErrorMacro
Definition GLDebug.h:32
#define YRPJOY_HAT_DOWN
#define YRPJOY_HAT_CENTERED
#define YRPJOY_HAT_LEFT
#define YRPJOY_HAT_RIGHT
#define YRPJOY_HAT_UP
const yarp::os::LogComponent & OVRHEADSET()
yarp::os::BufferedPort< yarp::sig::FlexImage > FlexImagePort
ovrVector3f vecSubtract(const ovrVector3f &a, const ovrVector3f &b)
OVR::Vector3f radToDeg(const OVR::Vector3f &v)
static void debugFov(const ovrFovPort fov[2])
constexpr unsigned int STICK_COUNT
yarp::sig::Vector ovrVec3ToYarp(const ovrVector3f &v)
static void debugTangent(std::string message, float tangent1, float tangent2)
constexpr unsigned int AXIS_COUNT
static int compareLuid(const ovrGraphicsLuid &lhs, const ovrGraphicsLuid &rhs)
constexpr unsigned int BUTTON_COUNT
yarp::sig::Matrix ovr2matrix(const ovrVector3f &pos, const OVR::Quatf &orientation)
void writeVec3OnPort(yarp::os::BufferedPort< yarp::os::Bottle > *const &port, const OVR::Vector3f &vec3, yarp::os::Stamp &stamp)
void setHeadLockedLayer(ovrLayerQuad &layer, TextureStatic *tex, const float x, const float y, const float z, const float rx, const float ry, const float rz, float rw, const float sizeX, const float sizeY)
bool(yarp::os::Value::* valueIsType)(void) const
static ovrGraphicsLuid GetDefaultAdapterLuid()
yarp::sig::Vector ovrRot2YarpRPY(const OVR::Quatf &rot)
This class is a callback class that receives the video frame from the YARP backend.
Definition ImagePort.h:30
ovrTextureSwapChain textureSwapChain
ovrTextureSwapChain textureSwapChain
unsigned int width
unsigned int height
Interface implemented by all device drivers.
Device that manages the Oculus Rift Headset.
Definition OVRHeadset.h:83
bool getStickDoF(unsigned int stick_id, unsigned int &DoF) override
virtual void threadRelease()
Release method.
virtual bool startService()
Initiate the service, whatever it is.
virtual bool close()
Close the DeviceDriver.
bool getButtonCount(unsigned int &button_count) override
bool getHat(unsigned int hat_id, unsigned char &value) override
virtual bool stopService()
Shut down the service, whatever it is.
bool getHatCount(unsigned int &Hat_count) override
bool getButton(unsigned int button_id, float &value) override
bool getStick(unsigned int stick_id, yarp::sig::Vector &value, JoypadCtrl_coordinateMode coordinate_mode) override
bool getTouchSurfaceCount(unsigned int &touch_count) override
bool getTrackballCount(unsigned int &Trackball_count) override
virtual bool threadInit()
Initialization method.
virtual bool open(yarp::os::Searchable &cfg)
Open the DeviceDriver.
bool getStickCount(unsigned int &stick_count) override
bool getTouch(unsigned int touch_id, yarp::sig::Vector &value) override
virtual bool updateService()
Give the service the chance to run for a while.
bool getAxis(unsigned int axis_id, double &value) override
virtual void run()
Loop function.
bool getTrackball(unsigned int trackball_id, yarp::sig::Vector &value) override
bool getAxisCount(unsigned int &axis_count) override
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition Bottle.cpp:158
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition Bottle.cpp:278
void clear()
Empties the bottle of any objects it contains.
Definition Bottle.cpp:121
bool isNull() const override
Checks if the object is invalid.
Definition Bottle.cpp:344
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition Bottle.cpp:288
A mini-server for performing network communication in the background.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
int getOutputCount() override
Determine how many output connections this port has.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
A class for storing options and configuration information.
Definition Property.h:32
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition Property.cpp:980
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
static double nowSystem()
static void delaySystem(double seconds)
A single value (typically within a Bottle).
Definition Value.h:44
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:222
virtual bool isString() const
Checks if value is a string.
Definition Value.cpp:156
virtual bool isBool() const
Checks if value is a boolean.
Definition Value.cpp:114
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
Definition Value.cpp:150
virtual bool isInt32() const
Checks if value is a 32-bit integer.
Definition Value.cpp:132
Image class with user control of representation details.
Definition Image.h:361
bool read(yarp::os::ConnectionReader &connection) override
Read image from a connection.
Definition Image.cpp:480
A class for a Matrix.
Definition Matrix.h:40
bool setSubcol(const Vector &v, size_t r, size_t c)
Set a portion of a column of this matrix with the values of the specified vector v.
Definition Matrix.cpp:532
void push_back(const Scalar &elem)
Push a new element in the vector: size is changed.
Definition VectorBase.h:225
void clear()
Clear (removes all elements) the vector.
Definition VectorBase.h:216
DumpObj * factory(Bottle &obj)
Definition main.cpp:81
#define YARP_CONSTEXPR
Expands to constexpr if constant expressions are supported by the current c++ compiler and build flag...
Definition compiler.h:2900
const TextureStatic::Image crosshairs
const TextureStatic::Image yarp_logo
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
yarp::sig::Matrix eye(int r, int c)
Build an identity matrix (defined in Math.h).
Definition math.cpp:586
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition math.cpp:808
The main, catch-all namespace for YARP.
Definition dirs.h:16
double x
double resizeH
double alpha
FlexImagePort * port
double resizeW
ovrLayerQuad layer
double y
double z
TextureBuffer * texture
#define YARP_UNUSED(var)
Definition api.h:162