Files
WiVRn/server/driver/wivrn_controller.cpp
T
Patrick Nicolas 7658f427e6 Request tracking extrapolation on client
Add the timestamp where data was requested, use this to compute how far
in the future the application is requesting samples.
With this information, client requests 2 tracking samples: one at the
current time and one in the future.
Server now keeps track of actual samples + one predicted sample, and is
most of the time in the interpolation use case.
2024-04-04 13:41:36 +02:00

414 lines
16 KiB
C++

/*
* WiVRn VR streaming
* Copyright (C) 2022 Guillaume Meunier <[email protected]>
* Copyright (C) 2022 Patrick Nicolas <[email protected]>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "wivrn_controller.h"
#include "util/u_logging.h"
#include <stdio.h>
#include "xrt/xrt_defines.h"
/*
*
* Defines & structs.
*
*/
enum wivrn_controller_input_index
{
WIVRN_CONTROLLER_AIM_POSE,
WIVRN_CONTROLLER_GRIP_POSE,
WIVRN_CONTROLLER_HAND_TRACKER,
WIVRN_CONTROLLER_MENU_CLICK, // /user/hand/left/input/menu/click
WIVRN_CONTROLLER_A_CLICK, // /user/hand/right/input/a/click
WIVRN_CONTROLLER_A_TOUCH, // /user/hand/right/input/a/touch
WIVRN_CONTROLLER_B_CLICK, // /user/hand/right/input/b/click
WIVRN_CONTROLLER_B_TOUCH, // /user/hand/right/input/b/touch
WIVRN_CONTROLLER_X_CLICK = WIVRN_CONTROLLER_A_CLICK, // /user/hand/left/input/x/click
WIVRN_CONTROLLER_X_TOUCH = WIVRN_CONTROLLER_A_TOUCH, // /user/hand/left/input/x/touch
WIVRN_CONTROLLER_Y_CLICK = WIVRN_CONTROLLER_B_CLICK, // /user/hand/left/input/y/click
WIVRN_CONTROLLER_Y_TOUCH = WIVRN_CONTROLLER_B_TOUCH, // /user/hand/left/input/y/touch
WIVRN_CONTROLLER_SQUEEZE_CLICK, // /user/hand/XXXX/input/squeeze/click
WIVRN_CONTROLLER_SQUEEZE_VALUE, // /user/hand/XXXX/input/squeeze/value
WIVRN_CONTROLLER_TRIGGER_CLICK, // /user/hand/XXXX/input/trigger/click
WIVRN_CONTROLLER_TRIGGER_VALUE, // /user/hand/XXXX/input/trigger/value
WIVRN_CONTROLLER_TRIGGER_TOUCH, // /user/hand/XXXX/input/trigger/touch
WIVRN_CONTROLLER_THUMBSTICK, // /user/hand/XXXX/input/thumbstick/{x,y}
WIVRN_CONTROLLER_THUMBSTICK_CLICK, // /user/hand/XXXX/input/thumbstick/click
WIVRN_CONTROLLER_THUMBSTICK_TOUCH, // /user/hand/XXXX/input/thumbstick/touch
WIVRN_CONTROLLER_THUMBREST_TOUCH, // /user/hand/XXXX/input/thumbrest/touch
WIVRN_CONTROLLER_INPUT_COUNT
};
enum class wivrn_input_type
{
BOOL,
FLOAT,
VEC2_X,
VEC2_Y
};
struct wivrn_to_wivrn_controller_input
{
wivrn_controller_input_index input_id;
device_id wivrn_id;
wivrn_input_type input_type;
};
// clang-format off
static const wivrn_to_wivrn_controller_input left_hand_bindings[] = {
{WIVRN_CONTROLLER_MENU_CLICK, device_id::MENU_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/menu/click
{WIVRN_CONTROLLER_X_CLICK, device_id::X_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/x/click
{WIVRN_CONTROLLER_X_TOUCH, device_id::X_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/x/touch
{WIVRN_CONTROLLER_Y_CLICK, device_id::Y_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/y/click
{WIVRN_CONTROLLER_Y_TOUCH, device_id::Y_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/y/touch
{WIVRN_CONTROLLER_MENU_CLICK, device_id::MENU_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/menu/click
{WIVRN_CONTROLLER_SQUEEZE_VALUE, device_id::LEFT_SQUEEZE_VALUE, wivrn_input_type::FLOAT}, // /user/hand/left/input/squeeze/value
{WIVRN_CONTROLLER_TRIGGER_VALUE, device_id::LEFT_TRIGGER_VALUE, wivrn_input_type::FLOAT}, // /user/hand/left/input/trigger/value
{WIVRN_CONTROLLER_TRIGGER_TOUCH, device_id::LEFT_TRIGGER_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/trigger/touch
{WIVRN_CONTROLLER_THUMBSTICK, device_id::LEFT_THUMBSTICK_X, wivrn_input_type::VEC2_X}, // /user/hand/left/input/thumbstick/x
{WIVRN_CONTROLLER_THUMBSTICK, device_id::LEFT_THUMBSTICK_Y, wivrn_input_type::VEC2_Y}, // /user/hand/left/input/thumbstick/y
{WIVRN_CONTROLLER_THUMBSTICK_CLICK, device_id::LEFT_THUMBSTICK_CLICK, wivrn_input_type::BOOL},// /user/hand/left/input/thumbstick/click
{WIVRN_CONTROLLER_THUMBSTICK_TOUCH, device_id::LEFT_THUMBSTICK_TOUCH, wivrn_input_type::BOOL},// /user/hand/left/input/thumbstick/touch
{WIVRN_CONTROLLER_THUMBREST_TOUCH, device_id::LEFT_THUMBREST_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/thumbrest/touch
};
static const wivrn_to_wivrn_controller_input right_hand_bindings[] = {
{WIVRN_CONTROLLER_A_CLICK, device_id::A_CLICK, wivrn_input_type::BOOL}, // /user/hand/right/input/a/click
{WIVRN_CONTROLLER_A_TOUCH, device_id::A_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/a/touch
{WIVRN_CONTROLLER_B_CLICK, device_id::B_CLICK, wivrn_input_type::BOOL}, // /user/hand/right/input/b/click
{WIVRN_CONTROLLER_B_TOUCH, device_id::B_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/b/touch
{WIVRN_CONTROLLER_SQUEEZE_VALUE, device_id::RIGHT_SQUEEZE_VALUE, wivrn_input_type::FLOAT}, // /user/hand/right/input/squeeze/value
{WIVRN_CONTROLLER_TRIGGER_VALUE, device_id::RIGHT_TRIGGER_VALUE, wivrn_input_type::FLOAT}, // /user/hand/right/input/trigger/value
{WIVRN_CONTROLLER_TRIGGER_TOUCH, device_id::RIGHT_TRIGGER_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/trigger/touch
{WIVRN_CONTROLLER_THUMBSTICK, device_id::RIGHT_THUMBSTICK_X, wivrn_input_type::VEC2_X}, // /user/hand/right/input/thumbstick/x
{WIVRN_CONTROLLER_THUMBSTICK, device_id::RIGHT_THUMBSTICK_Y, wivrn_input_type::VEC2_Y}, // /user/hand/right/input/thumbstick/y
{WIVRN_CONTROLLER_THUMBSTICK_CLICK, device_id::RIGHT_THUMBSTICK_CLICK, wivrn_input_type::BOOL},// /user/hand/right/input/thumbstick/click
{WIVRN_CONTROLLER_THUMBSTICK_TOUCH, device_id::RIGHT_THUMBSTICK_TOUCH, wivrn_input_type::BOOL},// /user/hand/right/input/thumbstick/touch
{WIVRN_CONTROLLER_THUMBREST_TOUCH, device_id::RIGHT_THUMBREST_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/thumbrest/touch
};
// clang-format on
static const size_t left_hand_bindings_count = ARRAY_SIZE(left_hand_bindings);
static const size_t right_hand_bindings_count = ARRAY_SIZE(right_hand_bindings);
static xrt_binding_input_pair simple_input_binding[] = {
{XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_TOUCH_TRIGGER_VALUE},
{XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_TOUCH_MENU_CLICK},
{XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_TOUCH_GRIP_POSE},
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_TOUCH_AIM_POSE},
};
static xrt_binding_output_pair simple_output_binding[] = {
{XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_TOUCH_HAPTIC},
};
static xrt_binding_profile wivrn_binding_profiles[] = {
{
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
.inputs = simple_input_binding,
.input_count = std::size(simple_input_binding),
.outputs = simple_output_binding,
.output_count = std::size(simple_output_binding),
},
};
static void wivrn_controller_destroy(xrt_device * xdev);
static void wivrn_controller_get_tracked_pose(xrt_device * xdev,
xrt_input_name name,
uint64_t at_timestamp_ns,
xrt_space_relation * out_relation);
static void wivrn_controller_get_hand_tracking(xrt_device * xdev,
xrt_input_name name,
uint64_t desired_timestamp_ns,
xrt_hand_joint_set * out_value,
uint64_t * out_timestamp_ns);
static void wivrn_controller_set_output(struct xrt_device * xdev, enum xrt_output_name name, const union xrt_output_value * value);
static void wivrn_controller_update_inputs(xrt_device * xdev);
wivrn_controller::wivrn_controller(int hand_id,
xrt_device * hmd,
std::shared_ptr<xrt::drivers::wivrn::wivrn_session> cnx) :
xrt_device{},
grip(hand_id == 0 ? device_id::LEFT_GRIP : device_id::RIGHT_GRIP),
aim(hand_id == 0 ? device_id::LEFT_AIM : device_id::RIGHT_AIM),
joints(hand_id),
cnx(cnx)
{
xrt_device * base = this;
base->destroy = wivrn_controller_destroy;
base->get_tracked_pose = wivrn_controller_get_tracked_pose;
base->get_hand_tracking = wivrn_controller_get_hand_tracking;
base->set_output = wivrn_controller_set_output;
base->update_inputs = wivrn_controller_update_inputs;
base->name = XRT_DEVICE_TOUCH_CONTROLLER;
base->orientation_tracking_supported = true;
base->hand_tracking_supported = true;
base->position_tracking_supported = true;
base->tracking_origin = hmd->tracking_origin;
inputs_array.resize(WIVRN_CONTROLLER_INPUT_COUNT);
inputs = inputs_array.data();
input_count = WIVRN_CONTROLLER_INPUT_COUNT;
// Setup input.
#define SET_INPUT(NAME) \
do \
{ \
inputs[WIVRN_CONTROLLER_##NAME].name = XRT_INPUT_TOUCH_##NAME; \
inputs[WIVRN_CONTROLLER_##NAME].active = true; \
} while (0)
SET_INPUT(AIM_POSE);
SET_INPUT(GRIP_POSE);
if (hand_id == 0)
{
SET_INPUT(X_CLICK);
SET_INPUT(Y_CLICK);
SET_INPUT(X_TOUCH);
SET_INPUT(Y_TOUCH);
SET_INPUT(MENU_CLICK);
}
else
{
SET_INPUT(A_CLICK);
SET_INPUT(B_CLICK);
SET_INPUT(A_TOUCH);
SET_INPUT(B_TOUCH);
}
SET_INPUT(SQUEEZE_VALUE);
SET_INPUT(TRIGGER_VALUE);
SET_INPUT(TRIGGER_TOUCH);
SET_INPUT(THUMBSTICK);
SET_INPUT(THUMBSTICK_CLICK);
SET_INPUT(THUMBSTICK_TOUCH);
SET_INPUT(THUMBREST_TOUCH);
inputs[WIVRN_CONTROLLER_HAND_TRACKER].name = hand_id == 0 ? XRT_INPUT_GENERIC_HAND_TRACKING_LEFT : XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT;
inputs[WIVRN_CONTROLLER_HAND_TRACKER].active = true;
output_count = 1;
outputs = &haptic_output;
haptic_output.name = XRT_OUTPUT_NAME_TOUCH_HAPTIC;
inputs_staging = inputs_array;
switch (hand_id)
{
case 0:
device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
// Print name.
strcpy(str, "WiVRn HMD left hand controller");
strcpy(serial, "WiVRn HMD left hand controller");
break;
case 1:
device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
// Print name.
strcpy(str, "WiVRn HMD right hand controller");
strcpy(serial, "WiVRn HMD right hand controller");
break;
default:
throw std::runtime_error("Invalid hand ID");
}
binding_profile_count = std::size(wivrn_binding_profiles);
binding_profiles = wivrn_binding_profiles;
}
void wivrn_controller::update_inputs()
{
std::lock_guard _{mutex};
inputs_array = inputs_staging;
}
void wivrn_controller::set_inputs(const from_headset::inputs & inputs, const clock_offset & clock_offset)
{
std::lock_guard lock{mutex};
for (const auto & input: inputs.values)
{
set_inputs(input.id, input.value, input.last_change_time ? clock_offset.from_headset(input.last_change_time) : 0);
}
}
void wivrn_controller::set_inputs(device_id input_id, float value, uint64_t last_change_time)
{
const struct wivrn_to_wivrn_controller_input * bindings;
size_t bindings_count;
if (device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER)
{
bindings = left_hand_bindings;
bindings_count = left_hand_bindings_count;
}
else
{
bindings = right_hand_bindings;
bindings_count = right_hand_bindings_count;
}
const struct wivrn_to_wivrn_controller_input * binding = NULL;
for (size_t i = 0; i < bindings_count; i++)
{
if (bindings[i].wivrn_id == input_id)
{
binding = &bindings[i];
break;
}
}
if (binding)
{
inputs_staging[binding->input_id].timestamp = last_change_time;
switch (binding->input_type)
{
case wivrn_input_type::BOOL:
inputs_staging[binding->input_id].value.boolean = (value != 0);
break;
case wivrn_input_type::FLOAT:
inputs_staging[binding->input_id].value.vec1.x = value;
break;
case wivrn_input_type::VEC2_X:
inputs_staging[binding->input_id].value.vec2.x = value;
break;
case wivrn_input_type::VEC2_Y:
inputs_staging[binding->input_id].value.vec2.y = value;
break;
}
}
}
xrt_space_relation wivrn_controller::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns)
{
std::chrono::nanoseconds extrapolation_time;
xrt_space_relation res;
switch (name)
{
case XRT_INPUT_TOUCH_AIM_POSE:
std::tie(extrapolation_time, res) = aim.get_at(at_timestamp_ns);
break;
case XRT_INPUT_TOUCH_GRIP_POSE:
std::tie(extrapolation_time, res) = grip.get_at(at_timestamp_ns);
break;
default:
U_LOG_W("Unknown input name requested");
return {};
}
cnx->add_predict_offset(extrapolation_time);
return res;
}
std::pair<xrt_hand_joint_set, uint64_t> wivrn_controller::get_hand_tracking(xrt_input_name name, uint64_t desired_timestamp_ns)
{
switch (name)
{
case XRT_INPUT_GENERIC_HAND_TRACKING_LEFT:
case XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT: {
auto [extrapolation_time, data] = joints.get_at(desired_timestamp_ns);
cnx->add_predict_offset(extrapolation_time);
return {data, desired_timestamp_ns};
}
default:
U_LOG_W("Unknown input name requested");
return {};
}
}
void wivrn_controller::update_tracking(const from_headset::tracking & tracking, const clock_offset & offset)
{
aim.update_tracking(tracking, offset);
grip.update_tracking(tracking, offset);
}
void wivrn_controller::update_hand_tracking(const from_headset::hand_tracking & tracking, const clock_offset & offset)
{
joints.update_tracking(tracking, offset);
}
void wivrn_controller::set_output(xrt_output_name name, const xrt_output_value * value)
{
auto id = device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER ? device_id::LEFT_CONTROLLER_HAPTIC
: device_id::RIGHT_CONTROLLER_HAPTIC;
try
{
cnx->send_stream(to_headset::haptics{
.id = id,
.duration = std::chrono::nanoseconds(value->vibration.duration_ns),
.frequency = value->vibration.frequency,
.amplitude = value->vibration.amplitude});
}
catch (...)
{
// Ignore errors
}
}
/*
*
* Functions
*
*/
static void wivrn_controller_destroy(xrt_device * xdev)
{
delete (wivrn_controller *)xdev;
}
static void wivrn_controller_update_inputs(xrt_device * xdev)
{
static_cast<wivrn_controller *>(xdev)->update_inputs();
}
static void wivrn_controller_get_tracked_pose(xrt_device * xdev,
xrt_input_name name,
uint64_t at_timestamp_ns,
xrt_space_relation * out_relation)
{
*out_relation = static_cast<wivrn_controller *>(xdev)->get_tracked_pose(name, at_timestamp_ns);
}
static void wivrn_controller_get_hand_tracking(xrt_device * xdev,
xrt_input_name name,
uint64_t desired_timestamp_ns,
xrt_hand_joint_set * out_value,
uint64_t * out_timestamp_ns)
{
std::tie(*out_value, *out_timestamp_ns) = static_cast<wivrn_controller *>(xdev)->get_hand_tracking(name, desired_timestamp_ns);
}
static void wivrn_controller_set_output(struct xrt_device * xdev, enum xrt_output_name name, const union xrt_output_value * value)
{
static_cast<wivrn_controller *>(xdev)->set_output(name, value);
}