Files
WiVRn/server/driver/wivrn_controller.cpp
T
2024-12-03 19:38:53 +01:00

425 lines
17 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 "wivrn_session.h"
#include "util/u_logging.h"
#include <stdio.h>
#include "xrt/xrt_defines.h"
namespace wivrn
{
enum wivrn_controller_input_index
{
WIVRN_CONTROLLER_AIM_POSE,
WIVRN_CONTROLLER_GRIP_POSE,
WIVRN_CONTROLLER_HAND_TRACKER,
WIVRN_CONTROLLER_PALM_POSE,
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 xrt_result_t wivrn_controller_get_tracked_pose(xrt_device * xdev,
xrt_input_name name,
int64_t at_timestamp_ns,
xrt_space_relation * out_relation)
{
*out_relation = static_cast<wivrn_controller *>(xdev)->get_tracked_pose(name, at_timestamp_ns);
return XRT_SUCCESS;
}
static void wivrn_controller_get_hand_tracking(xrt_device * xdev,
xrt_input_name name,
int64_t desired_timestamp_ns,
xrt_hand_joint_set * out_value,
int64_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 xrt_result_t wivrn_controller_update_inputs(xrt_device * xdev)
{
static_cast<wivrn_controller *>(xdev)->update_inputs();
return XRT_SUCCESS;
}
wivrn_controller::wivrn_controller(int hand_id,
xrt_device * hmd,
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),
palm(hand_id == 0 ? device_id::LEFT_PALM : device_id::RIGHT_PALM),
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 = cnx->get_info().hand_tracking;
base->position_tracking_supported = true;
base->tracking_origin = hmd->tracking_origin;
inputs_array.resize(WIVRN_CONTROLLER_INPUT_COUNT);
inputs = inputs_array.data();
input_count = inputs_array.size();
// 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 = hand_tracking_supported;
inputs[WIVRN_CONTROLLER_PALM_POSE].name = XRT_INPUT_GENERIC_PALM_POSE;
inputs[WIVRN_CONTROLLER_PALM_POSE].active = cnx->get_info().palm_pose;
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, int64_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, int64_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);
cnx->set_enabled(aim.device, true);
break;
case XRT_INPUT_TOUCH_GRIP_POSE:
std::tie(extrapolation_time, res) = grip.get_at(at_timestamp_ns);
cnx->set_enabled(grip.device, true);
break;
case XRT_INPUT_GENERIC_PALM_POSE:
std::tie(extrapolation_time, res) = palm.get_at(at_timestamp_ns);
cnx->set_enabled(palm.device, true);
break;
default:
U_LOG_W("Unknown input name requested");
return {};
}
cnx->add_predict_offset(extrapolation_time);
return res;
}
std::pair<xrt_hand_joint_set, int64_t> wivrn_controller::get_hand_tracking(xrt_input_name name, int64_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);
cnx->set_enabled(joints.hand_id == 0 ? to_headset::tracking_control::id::left_hand : to_headset::tracking_control::id::right_hand, true);
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)
{
if (not aim.update_tracking(tracking, offset))
cnx->set_enabled(aim.device, false);
if (not grip.update_tracking(tracking, offset))
cnx->set_enabled(grip.device, false);
if (not palm.update_tracking(tracking, offset))
cnx->set_enabled(palm.device, false);
}
void wivrn_controller::update_hand_tracking(const from_headset::hand_tracking & tracking, const clock_offset & offset)
{
if (not joints.update_tracking(tracking, offset))
cnx->set_enabled(joints.hand_id == 0 ? to_headset::tracking_control::id::left_hand : to_headset::tracking_control::id::right_hand, false);
}
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)
{
static_cast<wivrn_controller *>(xdev)->unregister();
}
static void wivrn_controller_get_hand_tracking(xrt_device * xdev,
xrt_input_name name,
int64_t desired_timestamp_ns,
xrt_hand_joint_set * out_value,
int64_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);
}
} // namespace wivrn