File oxr_relay.cpp
File List > openxr_interface > oxr_relay.cpp
Go to the documentation of this file
#include "oxr_relay.hpp"
const int log_interval = 300; // pose logging interval in frames
#include (locate_result),
// static_cast(space_loc.locationFlags));
}
}
}
if (current_hand_interactions_.is_valid()) {
// Throttle logging to once every 300 frames
static uint64_t hi_log_counter = 0;
if ((++hi_log_counter % 300) == 1) {
// spdlog::get("illixr")->debug("update_hand_interaction: logging");
const pose::side hand_enums[] = {pose::LEFT, pose::RIGHT};
const pose::interaction_pose_type type_enums[] = {pose::AIM, pose::GRIP, pose::PINCH, pose::POKE};
for (auto hand : hand_enums) {
const auto& hip = current_hand_interactions_.hands.at(hand);
for (auto type_enum : type_enums) {
const auto& ip = hip.at(type_enum);
if (!ip.valid()) {
// spdlog::get("illixr")->debug("[hand_interaction] {} {} not valid", hand_names[h], type_names[t]);
continue;
}
// if (type_enums[t] == pose::POKE) {
// spdlog::get("illixr")->debug(
// "[hand_interactionX] {} {} | pos=({:.3f},{:.3f},{:.3f})",
// hand_names[h], type_names[t],
// ip.position.x(), ip.position.y(), ip.position.z());
//} else {
// spdlog::get("illixr")->debug(
// "[hand_interactionX] {} {} | pos=({:.3f},{:.3f},{:.3f}) | value={:.3f} ready={}",
// hand_names[h], type_names[t],
// ip.position.x(), ip.position.y(), ip.position.z(),
// ip.value, ip.ready ? "true" : "false");
//}
}
}
}
}
// Palm poses via XR_EXT_palm_pose
// Located here (after xrSyncActions) so the action-system state is current.
// The palm pose action was bound against multiple interaction profiles in
// init_hand_interaction(), so the runtime resolves it from whichever input
// source is active (bare hands or controllers).
current_palm_poses_.sensor_time = clock_->now();
for (int h = 0; h < 2; h++) {
if (palm_pose_spaces_[h] == XR_NULL_HANDLE)
continue;
XrActionStateGetInfo get_info = {XR_TYPE_ACTION_STATE_GET_INFO};
get_info.action = palm_pose_action_;
get_info.subactionPath = hand_subaction_paths_[h];
XrActionStatePose pose_state = {XR_TYPE_ACTION_STATE_POSE};
if (XR_FAILED(xrGetActionStatePose(session_, &get_info, &pose_state)) || !pose_state.isActive) {
current_palm_poses_.hands[(h == 0) ? pose::LEFT : pose::RIGHT].relation_flags = 0;
continue;
}
XrSpaceVelocity velocity = {XR_TYPE_SPACE_VELOCITY};
XrSpaceLocation location = {XR_TYPE_SPACE_LOCATION};
location.next = &velocity;
XrResult r = xrLocateSpace(palm_pose_spaces_[h], local_space_, predicted_time, &location);
const bool pos_valid = (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0u;
const bool ori_valid = (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0u;
if (XR_SUCCEEDED(r) && pos_valid && ori_valid) {
pose::side palm_hand = (h == 0) ? pose::LEFT : pose::RIGHT;
current_palm_poses_.hands[palm_hand].update(location, velocity);
} else {
current_palm_poses_.hands[(h == 0) ? pose::LEFT : pose::RIGHT].relation_flags = 0;
}
}
if (current_palm_poses_.is_valid()) {
// Throttle logging to once every 300 frames
static uint64_t pp_log_counter = 0;
if ((++pp_log_counter % log_interval) == 1) {
const pose::side hand_enums[] = {pose::LEFT, pose::RIGHT};
for (int h = 0; h < 2; h++) {
const auto& pd = current_palm_poses_.hands.at(hand_enums[h]);
if (!pd.is_valid()) {
spdlog::get("illixr")->debug("[palm_pose] {} not valid", hand_names[h]);
continue;
}
}
}
}
}
// Shared by hand interaction updates and Quest controller sampling. Both callers
// hold actions_mutex_ so they cannot sync/read the same action set concurrently.
bool oxr_relay::sync_actions() {
if (hand_interaction_action_set_ == XR_NULL_HANDLE || session_ == XR_NULL_HANDLE) {
return false;
}
XrActiveActionSet active_set{};
active_set.actionSet = hand_interaction_action_set_;
active_set.subactionPath = XR_NULL_PATH;
XrActionsSyncInfo sync_info{XR_TYPE_ACTIONS_SYNC_INFO};
sync_info.countActiveActionSets = 1;
sync_info.activeActionSets = &active_set;
const auto start = std::chrono::steady_clock::now();
const XrResult result = xrSyncActions(session_, &sync_info);
const auto elapsed =
std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - start).count() / 1000.0;
if (elapsed > TIME_CUTOFF) {
spdlog::get("illixr")->warn("[oxr_timing] xrSyncActions took {:.2f}ms", elapsed);
}
if (result == XR_SESSION_NOT_FOCUSED) {
return false;
}
if (XR_FAILED(result)) {
spdlog::get("illixr")->warn("xrSyncActions failed: {}", static_cast<int>(result));
return false;
}
return true;
}
#ifdef ILLIXR_ENABLE_QUEST_CONTROLLERS
void oxr_relay::refresh_controller_profiles() {
for (std::size_t hand = 0; hand < controller_profiles_.size(); ++hand) {
XrInteractionProfileState state{XR_TYPE_INTERACTION_PROFILE_STATE};
const XrResult result = xrGetCurrentInteractionProfile(session_, hand_subaction_paths_[hand], &state);
if (XR_FAILED(result)) {
continue;
}
std::string path;
if (state.interactionProfile != XR_NULL_PATH) {
std::uint32_t size = 0;
if (XR_SUCCEEDED(xrPathToString(instance_, state.interactionProfile, 0, &size, nullptr)) && size > 0) {
std::vector<char> buffer(size);
if (XR_SUCCEEDED(xrPathToString(instance_, state.interactionProfile, size, &size, buffer.data()))) {
path.assign(buffer.data());
}
}
}
const auto profile = controller_profile_from_path(path);
if (controller_profiles_[hand] != profile) {
controller_profiles_[hand] = profile;
spdlog::get("illixr")->info("{} controller profile -> {}", hand == 0 ? "left" : "right",
path.empty() ? "none" : path);
}
}
}
bool oxr_relay::query_controller_pose(XrAction action, XrSpace space, XrPath hand_path, XrTime sample_time,
quest_controller_pose* pose_out) {
*pose_out = quest_controller_pose{};
XrActionStateGetInfo get_info{XR_TYPE_ACTION_STATE_GET_INFO};
get_info.action = action;
get_info.subactionPath = hand_path;
XrActionStatePose state{XR_TYPE_ACTION_STATE_POSE};
XrResult result = xrGetActionStatePose(session_, &get_info, &state);
if (XR_FAILED(result)) {
spdlog::get("illixr")->warn("xrGetActionStatePose(controller) failed: {}", static_cast<int>(result));
return false;
}
pose_out->active = state.isActive == XR_TRUE;
if (!pose_out->active) {
return true;
}
XrSpaceLocation location{XR_TYPE_SPACE_LOCATION};
result = xrLocateSpace(space, local_space_, sample_time, &location);
if (XR_FAILED(result)) {
spdlog::get("illixr")->warn("xrLocateSpace(controller) failed: {}", static_cast<int>(result));
return false;
}
pose_out->position_valid = (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0;
pose_out->orientation_valid = (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0;
pose_out->position_tracked = (location.locationFlags & XR_SPACE_LOCATION_POSITION_TRACKED_BIT) != 0;
pose_out->orientation_tracked = (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT) != 0;
if (pose_out->position_valid) {
pose_out->position = location.pose.position;
}
if (pose_out->orientation_valid) {
pose_out->orientation = location.pose.orientation;
}
return true;
}
bool oxr_relay::query_controller_boolean(XrAction action, XrPath hand_path, quest_controller_button* button) {
*button = quest_controller_button{};
XrActionStateGetInfo get_info{XR_TYPE_ACTION_STATE_GET_INFO};
get_info.action = action;
get_info.subactionPath = hand_path;
XrActionStateBoolean state{XR_TYPE_ACTION_STATE_BOOLEAN};
const XrResult result = xrGetActionStateBoolean(session_, &get_info, &state);
if (XR_FAILED(result)) {
return false;
}
button->active = state.isActive == XR_TRUE;
button->pressed = button->active && state.currentState == XR_TRUE;
button->changed_since_last_sync = state.changedSinceLastSync == XR_TRUE;
button->value = button->pressed ? 1.0F : 0.0F;
button->last_change_time = static_cast<std::int64_t>(state.lastChangeTime);
return true;
}
bool oxr_relay::query_controller_float(XrAction action, XrPath hand_path, float threshold, quest_controller_button* button) {
*button = quest_controller_button{};
XrActionStateGetInfo get_info{XR_TYPE_ACTION_STATE_GET_INFO};
get_info.action = action;
get_info.subactionPath = hand_path;
XrActionStateFloat state{XR_TYPE_ACTION_STATE_FLOAT};
const XrResult result = xrGetActionStateFloat(session_, &get_info, &state);
if (XR_FAILED(result)) {
return false;
}
button->active = state.isActive == XR_TRUE;
button->value = button->active ? state.currentState : 0.0F;
button->pressed = button->active && state.currentState >= threshold;
button->changed_since_last_sync = state.changedSinceLastSync == XR_TRUE;
button->last_change_time = static_cast<std::int64_t>(state.lastChangeTime);
return true;
}
bool oxr_relay::query_controller_axis(XrAction action, XrPath hand_path, quest_controller_axis2d* axis) {
*axis = quest_controller_axis2d{};
XrActionStateGetInfo get_info{XR_TYPE_ACTION_STATE_GET_INFO};
get_info.action = action;
get_info.subactionPath = hand_path;
XrActionStateVector2f state{XR_TYPE_ACTION_STATE_VECTOR2F};
const XrResult result = xrGetActionStateVector2f(session_, &get_info, &state);
if (XR_FAILED(result)) {
return false;
}
axis->active = state.isActive == XR_TRUE;
axis->changed_since_last_sync = state.changedSinceLastSync == XR_TRUE;
axis->last_change_time = static_cast<std::int64_t>(state.lastChangeTime);
if (axis->active) {
axis->value = {state.currentState.x, state.currentState.y};
}
return true;
}
void oxr_relay::merge_controller_button(quest_controller_button* destination, const quest_controller_button& source) {
destination->active = destination->active || source.active;
destination->pressed = destination->pressed || source.pressed;
destination->changed_since_last_sync = destination->changed_since_last_sync || source.changed_since_last_sync;
destination->value = std::max(destination->value, source.value);
destination->last_change_time = std::max(destination->last_change_time, source.last_change_time);
}
bool oxr_relay::query_controller_hand(std::size_t hand_index, XrTime sample_time, quest_hand_controller* hand) {
*hand = quest_hand_controller{};
hand->interaction_profile = controller_profiles_[hand_index];
if (!query_controller_pose(interaction_pose_actions_[pose::GRIP], interaction_pose_spaces_[hand_index][pose::GRIP],
hand_subaction_paths_[hand_index], sample_time, &hand->grip_pose) ||
!query_controller_pose(interaction_pose_actions_[pose::AIM], interaction_pose_spaces_[hand_index][pose::AIM],
hand_subaction_paths_[hand_index], sample_time, &hand->aim_pose)) {
return false;
}
quest_controller_button trigger_click;
quest_controller_button trigger_value;
if (!query_controller_boolean(controller_trigger_click_action_, hand_subaction_paths_[hand_index], &trigger_click) ||
!query_controller_float(controller_trigger_value_action_, hand_subaction_paths_[hand_index],
kControllerTriggerThreshold, &trigger_value) ||
!query_controller_float(controller_squeeze_value_action_, hand_subaction_paths_[hand_index],
kControllerSqueezeThreshold, &hand->squeeze) ||
!query_controller_boolean(controller_primary_click_action_, hand_subaction_paths_[hand_index], &hand->primary) ||
!query_controller_boolean(controller_secondary_click_action_, hand_subaction_paths_[hand_index], &hand->secondary) ||
!query_controller_boolean(controller_thumbstick_click_action_, hand_subaction_paths_[hand_index],
&hand->thumbstick_click) ||
!query_controller_axis(controller_thumbstick_axis_action_, hand_subaction_paths_[hand_index], &hand->thumbstick)) {
return false;
}
hand->trigger = trigger_click;
merge_controller_button(&hand->trigger, trigger_value);
hand->available = hand->grip_pose.active || hand->aim_pose.active || hand->trigger.active || hand->squeeze.active ||
hand->primary.active || hand->secondary.active || hand->thumbstick_click.active || hand->thumbstick.active;
return true;
}
#endif
#ifdef ILLIXR_ENABLE_BOBA
void oxr_relay::publish_boba_input(XrTime predicted_time, XrDuration predicted_period, XrBool32 should_render,
XrViewStateFlags view_flags, const XrView views[2],
const XrViewConfigurationView view_configs[2]) {
// One sequence and host timestamp bind the separately transported events
// into a coherent predicted-display-time snapshot on the desktop.
const std::uint64_t sequence = boba_input_sequence_.fetch_add(1, std::memory_order_relaxed);
const time_point now = clock_->now();
quest_controller_input controller;
controller.sequence = sequence;
controller.sample_time = now;
controller.xr_sample_time = static_cast<std::int64_t>(predicted_time);
if (controller_actions_initialized_) {
std::lock_guard<std::mutex> actions_lock{actions_mutex_};
if (sync_actions()) {
refresh_controller_profiles();
if (!query_controller_hand(0, predicted_time, &controller.left) ||
!query_controller_hand(1, predicted_time, &controller.right)) {
spdlog::get("illixr")->warn("Could not sample all Quest controller actions");
}
}
}
openxr_view_frame frame;
frame.sequence = sequence;
frame.sample_time = now;
frame.xr_sample_time = static_cast<std::int64_t>(predicted_time);
frame.xr_predicted_display_period = static_cast<std::int64_t>(predicted_period);
frame.should_render = should_render == XR_TRUE;
const bool pose_valid =
(view_flags & XR_VIEW_STATE_POSITION_VALID_BIT) != 0 && (view_flags & XR_VIEW_STATE_ORIENTATION_VALID_BIT) != 0;
const bool pose_tracked =
(view_flags & XR_VIEW_STATE_POSITION_TRACKED_BIT) != 0 && (view_flags & XR_VIEW_STATE_ORIENTATION_TRACKED_BIT) != 0;
const auto copy_view = [pose_valid, pose_tracked](const XrView& source, const XrViewConfigurationView& config,
openxr_eye_view* destination) {
destination->valid = pose_valid;
destination->pose_tracked = pose_tracked;
destination->recommended_width = config.recommendedImageRectWidth;
destination->recommended_height = config.recommendedImageRectHeight;
destination->angle_left = source.fov.angleLeft;
destination->angle_right = source.fov.angleRight;
destination->angle_up = source.fov.angleUp;
destination->angle_down = source.fov.angleDown;
if (pose_valid) {
destination->update(source.pose);
}
};
copy_view(views[0], view_configs[0], &frame.left);
copy_view(views[1], view_configs[1], &frame.right);
// Publish only after both objects are complete; consumers reject a sample
// until the matching sequence has arrived on both UDP topics.
quest_controller_writer_.put(std::make_shared<quest_controller_input>(std::move(controller)));
openxr_view_writer_.put(std::make_shared<openxr_view_frame>(std::move(frame)));
}
#endif
void oxr_relay::calibrate_time_offsets() {
// On Quest/Android, XrTime is nanoseconds since device boot,
// equivalent to CLOCK_BOOTTIME. During an active XR session the
// device does not suspend, so CLOCK_BOOTTIME == CLOCK_MONOTONIC.
// We therefore approximate xr_to_monotonic_offset as zero and
// compute monotonic_to_system_offset directly.
//
// Verification: sample XrTime from predicted_time_ alongside
// CLOCK_MONOTONIC to confirm they are in the same timebase.
xr_to_monotonic_offset_ns_ = 0;
// Compute monotonic_to_system_offset_ns_:
// system_clock_ns = CLOCK_MONOTONIC_ns + monotonic_to_system_offset_ns_
constexpr int NUM_SAMPLES = 20;
int64_t best_offset = 0;
int64_t best_rtt = INT64_MAX;
for (int i = 0; i < NUM_SAMPLES; i++) {
auto before_sys = std::chrono::system_clock::now();
struct timespec ts_mono;
clock_gettime(CLOCK_MONOTONIC, &ts_mono);
auto after_sys = std::chrono::system_clock::now();
int64_t before_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(before_sys.time_since_epoch()).count();
int64_t after_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(after_sys.time_since_epoch()).count();
int64_t rtt = after_ns - before_ns;
int64_t mid_ns = before_ns + rtt / 2;
int64_t mono_ns = ts_mono.tv_sec * 1'000'000'000LL + ts_mono.tv_nsec;
int64_t offset = mid_ns - mono_ns;
if (rtt < best_rtt) {
best_rtt = rtt;
best_offset = offset;
}
}
monotonic_to_system_offset_ns_ = best_offset;
time_offsets_calibrated_ = true;
/*spdlog::get("illixr")->info(
"oxr_relay: calibrate_time_offsets: "
"xr_to_monotonic={:.6f}s (assumed 0, Quest has no conversion extension) "
"monotonic_to_system={:.6f}s",
xr_to_monotonic_offset_ns_ / 1'000'000'000.0,
monotonic_to_system_offset_ns_ / 1'000'000'000.0);
*/
}
bool oxr_relay::get_pose_history(uint64_t id, pose_history_entry& out_entry) const {
std::lock_guard<std::mutex> lock(pose_history_mutex_);
auto it = pose_history_.find(id);
if (it == pose_history_.end()) {
return false;
}
out_entry = it->second;
return true;
}