Check the software usage to get the pose of the trackers without the headsets

[HosameldinMohamed]

Following #10, we are able use openVR to obtain the trackers poses without the headset. However, the current version of the software (https://github.com/ami-iit/yarp-openvr-trackers/releases/tag/v0.0.1), doesn't support it yet.

In this issue, we highlight the difference in behavior when eliminating the headset.

In detail, following #10, then launching

yarpdev --device transformServer --ROS::enable_ros_publisher 0 --ROS::enable_ros_subscriber 0

then

yarp-openvr-trackers

Streams these error messages

[ERROR] The state of the output of device LHR-87B324E3 is not ok

Which seems to be coming from

yarp-openvr-trackers/src/OpenVRTrackersDriver.cpp

Lines 317 to 322 in cd153db

	if (pose.eTrackingResult
	!= vr::ETrackingResult::TrackingResult_Running_OK) {
	yError() << "The state of the output of device" << serialNumber
	<< "is not ok";
	return std::nullopt;
	}

[HosameldinMohamed]

Some observations

• When we launch yarp-openvr-trackers it detects a device called /hmd/Null Serial Number, which seems to be the dummy driver we enable to bypass the headset. It also detects the trackers when they are connected.
• The identified tracker is considered of the unsupported type, seeing this message

[INFO] The device xxxxxx has unsupported type

Which seems to be coming from

yarp-openvr-trackers/src/OpenVRTrackersDriver.cpp

Lines 203 to 206 in cd153db

	if (!Impl::DeviceTypeIsSupported(type)) {
	yInfo() << "The device" << serialNumber << "has unsupported type";
	return true;
	}

But it seems only a warning.

• Commenting out the checks in

  yarp-openvr-trackers/src/OpenVRTrackersDriver.cpp

  Lines 317 to 322 in cd153db

  	if (pose.eTrackingResult
  	!= vr::ETrackingResult::TrackingResult_Running_OK) {
  	yError() << "The state of the output of device" << serialNumber
  	<< "is not ok";
  	return std::nullopt;
  	}

  and in

  yarp-openvr-trackers/src/OpenVRTrackersDriver.cpp

  Lines 324 to 328 in cd153db

  	// Check pose validity
  	if (!pose.bPoseIsValid) {
  	yWarning() << "The pose of device" << serialNumber << "is not valid";
  	return std::nullopt;
  	}

  doesn't solve the issue. The streamed pose of the tracker seems frozen, probably it took the first value and then refused to take other values.
• Actually, looking at the documentation of the method GetControllerStateWithPose, it seems that the pose is updated when the last button event occurred, so it was probably waiting for a button to be pressed (Maybe it considers the tracker as a controller).
• It seems that the correct method is GetDeviceToAbsoluteTrackingPose, (I didn't see then the issue Use GetDeviceToAbsoluteTrackingPose instead of GetControllerStateWithPose #8 😄).

[HosameldinMohamed]

• It seems that the correct method is GetDeviceToAbsoluteTrackingPose, (I didn't see then the issue Use GetDeviceToAbsoluteTrackingPose instead of GetControllerStateWithPose #8 smile).

Replacing

yarp-openvr-trackers/src/OpenVRTrackersDriver.cpp

Lines 309 to 314 in cd153db

	pImpl->vr->GetControllerStateWithPose(
	vr::ETrackingUniverseOrigin(pImpl->origin),
	pImpl->devices[serialNumber].index,
	&state,
	sizeof(state),
	&pose);

With

vr::TrackedDevicePose_t poses[pImpl->devices.size()];
pImpl->vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin(pImpl->origin), 0, &poses[0], pImpl->devices.size());
pose = poses[pImpl->devices[serialNumber].index];

Seems to do the trick! The behavior is as expected. The trackers' poses are streamed via the transform server, and The checks are not throwing warnings.

However the method is a bit different from the previous one because it expects an array of vr::TrackedDevicePose_t.

We need to review it because it throws a segfault when a tracker is switched off for example. There should be a better way to access it!

[HosameldinMohamed]

After the modification in #12, the device can work without the headset, but I had to change the line

yarp-openvr-trackers/src/OpenVRTrackersDriver.h

Lines 59 to 60 in e6694ad

	DevicesManager(
	const TrackingUniverseOrigin origin = TrackingUniverseOrigin::Seated);

From Seated to Standing. Not sure if updating it upstream will affect other use cases.