YARP
Yet Another Robot Platform
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"
27 #include "OVRHeadsetLogComponent.h"
28 
29 #include "img-yarp-robot-64.h"
30 #include "img-crosshairs.h"
31 
32 #include <yarp/os/BufferedPort.h>
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
52 YARP_CONSTEXPR unsigned int AXIS_COUNT = 8;
53 YARP_CONSTEXPR unsigned int STICK_COUNT = 2;
54 YARP_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 
82 typedef bool(yarp::os::Value::*valueIsType)(void) const;
83 typedef yarp::os::BufferedPort<yarp::sig::FlexImage> FlexImagePort;
84 struct guiParam
85 {
86  double resizeW;
87  double resizeH;
88  double x;
89  double y;
90  double z;
91  double alpha;
92  FlexImagePort* port;
93  ovrLayerQuad layer;
94  TextureBuffer* texture;
95 };
96 //----------------[utilities]
97 //WARNING it makes a conversion of the coordinate system
98 inline 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
110 inline yarp::sig::Vector ovrRot2YarpRPY(const OVR::Quatf& rot)
111 {
112  float yaw, pitch, roll;
113  yarp::sig::Vector v(3);
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 
121 inline yarp::sig::Matrix ovr2matrix(const ovrVector3f& pos, const OVR::Quatf& orientation)
122 {
123  yarp::sig::Matrix ret;
124  ret = yarp::math::rpy2dcm(ovrRot2YarpRPY(orientation));
125  ret.setSubcol(ovrVec3ToYarp(pos), 0, 3);
126 
127  return ret;
128 }
129 
130 inline 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 
139 static inline void debugTangent(std::string message, float tangent1, float tangent2)
140 {
141  yCDebug(OVRHEADSET,
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 }
150 static void debugFov(const ovrFovPort fov[2]) {
151  yCDebug(OVRHEADSET,
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 );
157  yCDebug(OVRHEADSET);
158  yCDebug(OVRHEADSET);
159  yCDebug(OVRHEADSET);
160 }
161 
162 static int compareLuid(const ovrGraphicsLuid& lhs, const ovrGraphicsLuid& rhs)
163 {
164  return memcmp(&lhs, &rhs, sizeof(ovrGraphicsLuid));
165 }
166 
167 static 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 
194 inline 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 
208 inline 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 
219 inline 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 
240 inline 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 
263 yarp::dev::OVRHeadset::OVRHeadset() :
264  yarp::dev::DeviceDriver(),
265  yarp::os::PeriodicThread(0.011, yarp::os::ShouldUseSystemClock::Yes) // ~90 fps
266 {
267  yCTrace(OVRHEADSET);
268 }
269 yarp::dev::OVRHeadset::~OVRHeadset()
270 {
271  yCTrace(OVRHEADSET);
272 }
273 
274 void 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 
291 void 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 
349 void 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 
366 void 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 
375 bool yarp::dev::OVRHeadset::open(yarp::os::Searchable& cfg)
376 {
377  yCTrace(OVRHEADSET);
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 
535  *port.first = new yarp::os::BufferedPort<yarp::os::Bottle>;
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 
612 bool yarp::dev::OVRHeadset::threadInit()
613 {
614  yCTrace(OVRHEADSET);
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);
682  checkGlErrorMacro;
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 
732  checkGlErrorMacro;
733 
734  return true;
735 }
736 
737 void yarp::dev::OVRHeadset::threadRelease()
738 {
739  yCTrace(OVRHEADSET);
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 
828 bool yarp::dev::OVRHeadset::close()
829 {
830  yCTrace(OVRHEADSET);
831  this->askToStop();
832  return true;
833 }
834 
835 bool yarp::dev::OVRHeadset::startService()
836 {
837  yCTrace(OVRHEADSET);
838  return false;
839 }
840 
841 bool yarp::dev::OVRHeadset::updateService()
842 {
843  if (closed) {
844  return false;
845  }
846 
847  constexpr double delay = 60.0;
848  yCDebug(OVRHEADSET,
849  "Thread ran %d times, est period %lf[ms], used %lf[ms]",
850  getIterations(),
851  getEstimatedPeriod()*1000,
852  getEstimatedUsed()*1000);
853  yCDebug(OVRHEADSET,
854  "Display refresh: %3.1f[hz]", getIterations()/delay);
855 
856  for (int eye = 0; eye < ovrEye_Count; ++eye) {
857  yCDebug(OVRHEADSET,
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 
872  yarp::os::SystemClock::delaySystem(delay);
873  return !closed;
874 }
875 
876 bool yarp::dev::OVRHeadset::stopService()
877 {
878  yCTrace(OVRHEADSET);
879  return this->close();
880 }
881 
882 void 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 
897 void yarp::dev::OVRHeadset::run()
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 
1203  checkGlErrorMacro;
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 
1218 bool yarp::dev::OVRHeadset::createWindow(int w, int h)
1219 {
1220  yCTrace(OVRHEADSET);
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 
1235 void yarp::dev::OVRHeadset::onKey(int key, int scancode, int action, int mods)
1236 {
1237  yCTrace(OVRHEADSET);
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 
1454 void 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 
1461 void 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 
1477 void 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 
1483 void yarp::dev::OVRHeadset::glfwErrorCallback(int error, const char* description)
1484 {
1485  yCError(OVRHEADSET) << error << description;
1486 }
1487 
1488 void 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 
1514 void 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 
1524 void 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
1534 bool yarp::dev::OVRHeadset::getAxisCount(unsigned int& axis_count)
1535 {
1536  if (inputStateError) return false;
1537  axis_count = axisIdToValue.size();
1538  return true;
1539 }
1540 
1541 bool yarp::dev::OVRHeadset::getButtonCount(unsigned int& button_count)
1542 {
1543  if (inputStateError) return false;
1544  button_count = BUTTON_COUNT;
1545  return true;
1546 }
1547 
1548 bool yarp::dev::OVRHeadset::getTrackballCount(unsigned int& Trackball_count)
1549 {
1550  if (inputStateError) return false;
1551  Trackball_count = 0;
1552  return true;
1553 }
1554 
1555 bool yarp::dev::OVRHeadset::getHatCount(unsigned int& Hat_count)
1556 {
1557  if (inputStateError) return false;
1558  Hat_count = 1;
1559  return true;
1560 }
1561 
1562 bool yarp::dev::OVRHeadset::getTouchSurfaceCount(unsigned int& touch_count)
1563 {
1564  if (inputStateError) return false;
1565  touch_count = 0;
1566  return true;
1567 }
1568 
1569 bool 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 
1576 bool yarp::dev::OVRHeadset::getStickDoF(unsigned int stick_id, unsigned int& DoF)
1577 {
1578  DoF = 2;
1579  return true;
1580 }
1581 
1582 bool 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 
1595 bool yarp::dev::OVRHeadset::getTrackball(unsigned int trackball_id, yarp::sig::Vector& value)
1596 {
1597  return false;
1598 }
1599 
1600 bool 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 
1616 bool 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 
1629 bool 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 
1656 bool yarp::dev::OVRHeadset::getTouch(unsigned int touch_id, yarp::sig::Vector& value)
1657 {
1658  return false;
1659 }
LogStream.h
yarp::dev::IJoypadController::JoypadCtrl_coordinateMode
JoypadCtrl_coordinateMode
Definition: IJoypadController.h:37
yarp::os::Bottle
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
yarp::dev::OVRHeadset::getTouch
bool getTouch(unsigned int touch_id, yarp::sig::Vector &value) override
Get the value of a touch if present, return false otherwise.
Definition: OVRHeadset.cpp:1656
yarp::os::Property::put
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:998
SystemClock.h
debugFov
static void debugFov(const ovrFovPort fov[2])
Definition: OVRHeadset.cpp:150
yarp::dev::OVRHeadset::getAxis
bool getAxis(unsigned int axis_id, double &value) override
Get the value of an axis if present, return false otherwise.
Definition: OVRHeadset.cpp:1616
yarp::os::Bottle::clear
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:124
Network.h
yarp::dev::OVRHeadset::startService
virtual bool startService()
Initiate the service, whatever it is.
Definition: OVRHeadset.cpp:835
yarp::os::Searchable
A base class for nested structures that can be searched.
Definition: Searchable.h:69
guiParam::resizeW
double resizeW
Definition: OVRHeadset.cpp:86
OVRHeadsetLogComponent.h
yarp::dev::OVRHeadset::~OVRHeadset
virtual ~OVRHeadset()
Definition: OVRHeadset.cpp:269
yarp::dev::OVRHeadset::getAxisCount
bool getAxisCount(unsigned int &axis_count) override
Get number of Axes.
Definition: OVRHeadset.cpp:1534
guiParam::layer
ovrLayerQuad layer
Definition: OVRHeadset.cpp:93
yarp::os::Searchable::findGroup
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
yCWarning
#define yCWarning(component,...)
Definition: LogComponent.h:146
InputCallback
Definition: InputCallback.h:31
yarp::math::dcm2rpy
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:780
yarp::dev::OVRHeadset::getHat
bool getHat(unsigned int hat_id, unsigned char &value) override
Get the value of an Hat.
Definition: OVRHeadset.cpp:1600
writeVec3OnPort
void writeVec3OnPort(yarp::os::BufferedPort< yarp::os::Bottle > *const &port, const OVR::Vector3f &vec3, yarp::os::Stamp &stamp)
Definition: OVRHeadset.cpp:194
yarp::dev::DeviceDriver
Interface implemented by all device drivers.
Definition: DeviceDriver.h:38
yarp::dev::OVRHeadset::stopService
virtual bool stopService()
Shut down the service, whatever it is.
Definition: OVRHeadset.cpp:876
FlexImagePort
yarp::os::BufferedPort< yarp::sig::FlexImage > FlexImagePort
Definition: OVRHeadset.cpp:83
yarp::math::eye
yarp::sig::Matrix eye(int r, int c)
Build an identity matrix (defined in Math.h).
Definition: math.cpp:562
OVRHEADSET
const yarp::os::LogComponent & OVRHEADSET()
Definition: OVRHeadsetLogComponent.cpp:11
YARP_UNUSED
#define YARP_UNUSED(var)
Definition: api.h:159
yarp::os::BufferedPort::setEnvelope
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
Definition: BufferedPort-inl.h:232
YARP_CONSTEXPR
#define YARP_CONSTEXPR
Expands to constexpr if constant expressions are supported by the current c++ compiler and build flag...
Definition: compiler.h:2898
ret
bool ret
Definition: ImplementAxisInfo.cpp:72
yarp::sig::VectorOf::clear
void clear()
Definition: Vector.h:521
yarp::dev::OVRHeadset::getButton
bool getButton(unsigned int button_id, float &value) override
Get the value of a button.
Definition: OVRHeadset.cpp:1582
guiParam::port
FlexImagePort * port
Definition: OVRHeadset.cpp:92
yarp::os::Bottle::addFloat64
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:161
yarp::sig::Image::read
bool read(yarp::os::ConnectionReader &connection) override
Read image from a connection.
Definition: Image.cpp:665
TextureStatic::width
unsigned int width
Definition: TextureStatic.h:42
yarp::os::Bottle::find
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition: Bottle.cpp:290
yarp::os::Time::now
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:124
TextureBuffer::textureSwapChain
ovrTextureSwapChain textureSwapChain
Definition: TextureBuffer.h:49
YRPJOY_HAT_LEFT
#define YRPJOY_HAT_LEFT
Definition: IJoypadController.h:281
YRPJOY_HAT_UP
#define YRPJOY_HAT_UP
Definition: IJoypadController.h:278
yarp::os::SystemClock::nowSystem
static double nowSystem()
Definition: SystemClock.cpp:37
yarp::math::rpy2dcm
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:818
guiParam::alpha
double alpha
Definition: OVRHeadset.cpp:91
checkGlErrorMacro
#define checkGlErrorMacro
Definition: GLDebug.h:32
yarp::os::BufferedPort::prepare
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
Definition: BufferedPort-inl.h:114
yarp::sig::VectorOf
Provides:
Definition: Vector.h:122
yarp::os::BufferedPort< yarp::sig::FlexImage >
yarp::os::Bottle::check
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition: Bottle.cpp:280
TextureStatic.h
yarp::os::BufferedPort::getOutputCount
int getOutputCount() override
Determine how many output connections this port has.
Definition: BufferedPort-inl.h:251
yarp::os::Value::isString
virtual bool isString() const
Checks if value is a string.
Definition: Value.cpp:159
guiParam::y
double y
Definition: OVRHeadset.cpp:89
compareLuid
static int compareLuid(const ovrGraphicsLuid &lhs, const ovrGraphicsLuid &rhs)
Definition: OVRHeadset.cpp:162
debugTangent
static void debugTangent(std::string message, float tangent1, float tangent2)
Definition: OVRHeadset.cpp:139
yarp::dev::OVRHeadset::getButtonCount
bool getButtonCount(unsigned int &button_count) override
Get number of Buttons.
Definition: OVRHeadset.cpp:1541
Property.h
Math.h
yarp::os::Value::isBool
virtual bool isBool() const
Checks if value is a boolean.
Definition: Value.cpp:117
yarp::os::Value::isFloat64
virtual bool isFloat64() const
Checks if value is a 64-bit floating point number.
Definition: Value.cpp:153
yarp::os::SystemClock::delaySystem
static void delaySystem(double seconds)
Definition: SystemClock.cpp:32
yarp::sig::FlexImage
Image class with user control of representation details.
Definition: Image.h:403
TextureStatic::textureSwapChain
ovrTextureSwapChain textureSwapChain
Definition: TextureStatic.h:40
TextureBuffer.h
img-yarp-robot-64.h
GetDefaultAdapterLuid
static ovrGraphicsLuid GetDefaultAdapterLuid()
Definition: OVRHeadset.cpp:167
Stamp.h
yarp::dev::OVRHeadset::open
virtual bool open(yarp::os::Searchable &cfg)
Open the DeviceDriver.
Definition: OVRHeadset.cpp:375
yarp::os::Value::asBool
virtual bool asBool() const
Get boolean value.
Definition: Value.cpp:189
yarp::os::Value::asString
virtual std::string asString() const
Get string value.
Definition: Value.cpp:237
guiParam::resizeH
double resizeH
Definition: OVRHeadset.cpp:87
yarp::dev::OVRHeadset::getTrackballCount
bool getTrackballCount(unsigned int &Trackball_count) override
Get number of trackballs.
Definition: OVRHeadset.cpp:1548
yarp::os::BufferedPort::open
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition: BufferedPort-inl.h:41
yarp::dev::OVRHeadset::updateService
virtual bool updateService()
Give the service the chance to run for a while.
Definition: OVRHeadset.cpp:841
valueIsType
bool(yarp::os::Value::* valueIsType)(void) const
Definition: OVRHeadset.cpp:82
yarp::os::Searchable::check
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
OVRHeadset.h
crosshairs
const TextureStatic::Image crosshairs
Definition: img-crosshairs.h:25
yarp::os::Searchable::find
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
AXIS_COUNT
constexpr unsigned int AXIS_COUNT
Definition: OVRHeadset.cpp:52
vecSubtract
ovrVector3f vecSubtract(const ovrVector3f &a, const ovrVector3f &b)
Definition: OVRHeadset.cpp:130
yarp::dev::OVRHeadset::getStickDoF
bool getStickDoF(unsigned int stick_id, unsigned int &DoF) override
Get the Degree Of Freedom count for desired stick.
Definition: OVRHeadset.cpp:1576
TextureBattery.h
yarp::dev::OVRHeadset::getStick
bool getStick(unsigned int stick_id, yarp::sig::Vector &value, JoypadCtrl_coordinateMode coordinate_mode) override
Get the value of a stick if present, return false otherwise.
Definition: OVRHeadset.cpp:1629
guiParam::x
double x
Definition: OVRHeadset.cpp:88
BufferedPort.h
radToDeg
OVR::Vector3f radToDeg(const OVR::Vector3f &v)
Definition: OVRHeadset.cpp:208
YRPJOY_HAT_CENTERED
#define YRPJOY_HAT_CENTERED
Definition: IJoypadController.h:277
img-crosshairs.h
TextureBuffer::height
size_t height
Definition: TextureBuffer.h:53
yarp::dev::OVRHeadset::getTouchSurfaceCount
bool getTouchSurfaceCount(unsigned int &touch_count) override
get the number of touch surface.
Definition: OVRHeadset.cpp:1562
yarp::os::Bottle::isNull
bool isNull() const override
Checks if the object is invalid.
Definition: Bottle.cpp:373
yarp::dev::OVRHeadset::getTrackball
bool getTrackball(unsigned int trackball_id, yarp::sig::Vector &value) override
Get the axes change of a Trackball.
Definition: OVRHeadset.cpp:1595
Image.h
yarp::dev::OVRHeadset::run
virtual void run()
Loop function.
Definition: OVRHeadset.cpp:897
yarp::dev::OVRHeadset::threadRelease
virtual void threadRelease()
Release method.
Definition: OVRHeadset.cpp:737
yarp::os::Stamp
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:25
yCError
#define yCError(component,...)
Definition: LogComponent.h:157
yarp::os::Value::asInt32
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:207
guiParam::texture
TextureBuffer * texture
Definition: OVRHeadset.cpp:94
TextureBuffer::width
size_t width
Definition: TextureBuffer.h:52
yarp::os::BufferedPort::write
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
Definition: BufferedPort-inl.h:126
yarp::dev::OVRHeadset
ovrheadset: Device that manages the Oculus Rift Headset.
Definition: OVRHeadset.h:84
yCInfo
#define yCInfo(component,...)
Definition: LogComponent.h:135
yCDebug
#define yCDebug(component,...)
Definition: LogComponent.h:112
TextureStatic
Definition: TextureStatic.h:27
yarp::dev::OVRHeadset::getHatCount
bool getHatCount(unsigned int &Hat_count) override
Get number of Hats.
Definition: OVRHeadset.cpp:1555
yarp
The main, catch-all namespace for YARP.
Definition: environment.h:18
yarp::os::ShouldUseSystemClock
ShouldUseSystemClock
Definition: Time.h:23
yarp::dev::OVRHeadset::threadInit
virtual bool threadInit()
Initialization method.
Definition: OVRHeadset.cpp:612
YRPJOY_HAT_DOWN
#define YRPJOY_HAT_DOWN
Definition: IJoypadController.h:280
yarp::dev::OVRHeadset::getStickCount
bool getStickCount(unsigned int &stick_count) override
get the number of the sticks
Definition: OVRHeadset.cpp:1569
ovrVec3ToYarp
yarp::sig::Vector ovrVec3ToYarp(const ovrVector3f &v)
Definition: OVRHeadset.cpp:98
STICK_COUNT
constexpr unsigned int STICK_COUNT
Definition: OVRHeadset.cpp:53
yarp::dev::OVRHeadset::OVRHeadset
OVRHeadset()
Definition: OVRHeadset.cpp:263
InputCallback.h
Time.h
guiParam
Definition: OVRHeadset.cpp:85
ovrRot2YarpRPY
yarp::sig::Vector ovrRot2YarpRPY(const OVR::Quatf &rot)
Definition: OVRHeadset.cpp:110
yCTrace
#define yCTrace(component,...)
Definition: LogComponent.h:88
YRPJOY_HAT_RIGHT
#define YRPJOY_HAT_RIGHT
Definition: IJoypadController.h:279
GLDebug.h
yarp::os::Value
A single value (typically within a Bottle).
Definition: Value.h:47
BUTTON_COUNT
constexpr unsigned int BUTTON_COUNT
Definition: OVRHeadset.cpp:54
yarp::os::Value::asFloat64
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:225
setHeadLockedLayer
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)
Definition: OVRHeadset.cpp:219
yarp::sig::VectorOf::push_back
void push_back(const T &elem)
Push a new element in the vector: size is changed.
Definition: Vector.h:282
yarp::os::Time::delay
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:114
yarp_logo
const TextureStatic::Image yarp_logo
Definition: img-yarp-robot-64.h:25
TextureStatic::height
unsigned int height
Definition: TextureStatic.h:43
TextureBuffer
Definition: TextureBuffer.h:29
yarp::dev::OVRHeadset::close
virtual bool close()
Close the DeviceDriver.
Definition: OVRHeadset.cpp:828
TextureBattery
Definition: TextureBattery.h:34
ovr2matrix
yarp::sig::Matrix ovr2matrix(const ovrVector3f &pos, const OVR::Quatf &orientation)
Definition: OVRHeadset.cpp:121
yarp::os::Property
A class for storing options and configuration information.
Definition: Property.h:37
yarp::sig::Matrix
A class for a Matrix.
Definition: Matrix.h:46
guiParam::z
double z
Definition: OVRHeadset.cpp:90
yarp::os::Value::isInt32
virtual bool isInt32() const
Checks if value is a 32-bit integer.
Definition: Value.cpp:135