Skip to content

Pose prediction/history #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
rpavlik wants to merge 3 commits into
base: main
Choose a base branch
from
Draft

Pose prediction/history #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d4d001a
Add velocities in protocol
rpavlik Sep 29, 2023
b76d1a0
Flip init order back again
rpavlik Sep 29, 2023
f25f53b
Handle velocity and prediction on server BROKEN
rpavlik Sep 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions client/src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -353,6 +353,9 @@ android_main(struct android_app *app)
ALOGI("%s: starting connection", __FUNCTION__);
em_connection_connect(state.connection);

ALOGI("%s: starting stream client mainloop thread", __FUNCTION__);
em_stream_client_spawn_thread(stream_client, state.connection);

XrExtent2Di eye_extents{static_cast<int32_t>(state.width), static_cast<int32_t>(state.height)};
EmRemoteExperience *remote_experience =
em_remote_experience_new(state.connection, stream_client, state.instance, state.session, &eye_extents);
Expand All @@ -361,9 +364,6 @@ android_main(struct android_app *app)
return;
}

ALOGI("%s: starting stream client mainloop thread", __FUNCTION__);
em_stream_client_spawn_thread(stream_client, state.connection);

//
// End of remote-rendering-specific setup, into main loop
//
Expand Down
3 changes: 3 additions & 0 deletions proto/electricmaple.proto
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,9 @@ enum TrackedStatus {

message TrackingMessage {
Pose P_localSpace_viewSpace = 1;
Vec3 V_localSpace_viewSpace_linear = 10; // linear velocity
Vec3 V_localSpace_viewSpace_angular = 11; // angular velocity

Pose P_viewSpace_view0 = 2; // Left view
Pose P_viewSpace_view1 = 3; // Right view

Expand Down
20 changes: 15 additions & 5 deletions proto/generated/electricmaple.pb.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,10 @@ typedef struct _em_proto_TrackingMessage {
em_proto_Pose controller_aim_right;
int64_t timestamp;
int64_t sequence_idx;
bool has_V_localSpace_viewSpace_linear;
em_proto_Vec3 V_localSpace_viewSpace_linear; /* linear velocity */
bool has_V_localSpace_viewSpace_angular;
em_proto_Vec3 V_localSpace_viewSpace_angular; /* angular velocity */
} em_proto_TrackingMessage;

typedef struct _em_proto_InputThumbstick {
Expand Down Expand Up @@ -168,7 +172,7 @@ extern "C" {
#define em_proto_Vec3_init_default {0, 0, 0}
#define em_proto_Vec2_init_default {0, 0}
#define em_proto_Pose_init_default {false, em_proto_Vec3_init_default, false, em_proto_Quaternion_init_default}
#define em_proto_TrackingMessage_init_default {false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, 0, 0}
#define em_proto_TrackingMessage_init_default {false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, 0, 0, false, em_proto_Vec3_init_default, false, em_proto_Vec3_init_default}
#define em_proto_InputThumbstick_init_default {false, em_proto_Vec2_init_default, 0, 0}
#define em_proto_InputValueTouch_init_default {0, 0}
#define em_proto_InputClickTouch_init_default {0, 0}
Expand All @@ -183,7 +187,7 @@ extern "C" {
#define em_proto_Vec3_init_zero {0, 0, 0}
#define em_proto_Vec2_init_zero {0, 0}
#define em_proto_Pose_init_zero {false, em_proto_Vec3_init_zero, false, em_proto_Quaternion_init_zero}
#define em_proto_TrackingMessage_init_zero {false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, 0, 0}
#define em_proto_TrackingMessage_init_zero {false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, 0, 0, false, em_proto_Vec3_init_zero, false, em_proto_Vec3_init_zero}
#define em_proto_InputThumbstick_init_zero {false, em_proto_Vec2_init_zero, 0, 0}
#define em_proto_InputValueTouch_init_zero {0, 0}
#define em_proto_InputClickTouch_init_zero {0, 0}
Expand Down Expand Up @@ -216,6 +220,8 @@ extern "C" {
#define em_proto_TrackingMessage_controller_aim_right_tag 7
#define em_proto_TrackingMessage_timestamp_tag 8
#define em_proto_TrackingMessage_sequence_idx_tag 9
#define em_proto_TrackingMessage_V_localSpace_viewSpace_linear_tag 10
#define em_proto_TrackingMessage_V_localSpace_viewSpace_angular_tag 11
#define em_proto_InputThumbstick_xy_tag 1
#define em_proto_InputThumbstick_click_tag 2
#define em_proto_InputThumbstick_touch_tag 3
Expand Down Expand Up @@ -286,7 +292,9 @@ X(a, STATIC, OPTIONAL, MESSAGE, controller_aim_left, 5) \
X(a, STATIC, OPTIONAL, MESSAGE, controller_grip_right, 6) \
X(a, STATIC, OPTIONAL, MESSAGE, controller_aim_right, 7) \
X(a, STATIC, SINGULAR, INT64, timestamp, 8) \
X(a, STATIC, SINGULAR, INT64, sequence_idx, 9)
X(a, STATIC, SINGULAR, INT64, sequence_idx, 9) \
X(a, STATIC, OPTIONAL, MESSAGE, V_localSpace_viewSpace_linear, 10) \
X(a, STATIC, OPTIONAL, MESSAGE, V_localSpace_viewSpace_angular, 11)
#define em_proto_TrackingMessage_CALLBACK NULL
#define em_proto_TrackingMessage_DEFAULT NULL
#define em_proto_TrackingMessage_P_localSpace_viewSpace_MSGTYPE em_proto_Pose
Expand All @@ -296,6 +304,8 @@ X(a, STATIC, SINGULAR, INT64, sequence_idx, 9)
#define em_proto_TrackingMessage_controller_aim_left_MSGTYPE em_proto_Pose
#define em_proto_TrackingMessage_controller_grip_right_MSGTYPE em_proto_Pose
#define em_proto_TrackingMessage_controller_aim_right_MSGTYPE em_proto_Pose
#define em_proto_TrackingMessage_V_localSpace_viewSpace_linear_MSGTYPE em_proto_Vec3
#define em_proto_TrackingMessage_V_localSpace_viewSpace_angular_MSGTYPE em_proto_Vec3

#define em_proto_InputThumbstick_FIELDLIST(X, a) \
X(a, STATIC, OPTIONAL, MESSAGE, xy, 1) \
Expand Down Expand Up @@ -427,9 +437,9 @@ extern const pb_msgdesc_t em_proto_DownMessage_msg;
#define em_proto_TouchControllerCommon_size 38
#define em_proto_TouchControllerLeft_size 58
#define em_proto_TouchControllerRight_size 58
#define em_proto_TrackingMessage_size 309
#define em_proto_TrackingMessage_size 343
#define em_proto_UpFrameMessage_size 44
#define em_proto_UpMessage_size 369
#define em_proto_UpMessage_size 403
#define em_proto_Vec2_size 10
#define em_proto_Vec3_size 15

Expand Down
61 changes: 44 additions & 17 deletions server/src/ems/ems_hmd.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@

#include "ems_callbacks.h"
#include "xrt/xrt_defines.h"
#include <memory>
#undef CLAMP

#include "xrt/xrt_device.h"
Expand All @@ -24,6 +23,7 @@

#include "math/m_api.h"
#include "math/m_mathinclude.h"
#include "math/m_relation_history.h"

#include "util/u_var.h"
#include "util/u_misc.h"
Expand All @@ -40,12 +40,12 @@

#include "ems_server_internal.h"

#include <cstdint>
#include <glib.h>
#include <memory>
#include <mutex>
#include <stdio.h>

#include <thread>



/*
Expand All @@ -60,6 +60,7 @@
* @implements xrt_device
*/

static constexpr uint64_t kFixedAssumedLatencyUs = 50 * U_TIME_1MS_IN_NS;


/// Casting helper function
Expand All @@ -82,6 +83,8 @@ ems_hmd_destroy(struct xrt_device *xdev)

eh->received = nullptr;

m_relation_history_destroy(&eh->pose_history);

// Remove the variable tracking.
u_var_remove_root(eh);

Expand All @@ -108,16 +111,20 @@ ems_hmd_get_tracked_pose(struct xrt_device *xdev,
}

if (eh->received->updated) {
xrt_space_relation rel;
uint64_t timestamp;
std::lock_guard<std::mutex> lock(eh->received->mutex);
eh->pose = eh->received->pose;
math_quat_normalize(&eh->pose.orientation);
rel = eh->received->rel;
timestamp = eh->received->timestamp;
eh->received->rel.relation_flags = XRT_SPACE_RELATION_BITMASK_NONE;
eh->received->updated = false;
if (0 == (rel.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT)) {
// guess our velocities if not reported.
m_relation_history_estimate_motion(eh->pose_history, &rel, timestamp, &rel);
}
m_relation_history_push(eh->pose_history, &rel, timestamp);
}
// TODO Estimate pose at timestamp at_timestamp_ns!
out_relation->pose = eh->pose;
out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
m_relation_history_get(eh->pose_history, at_timestamp_ns, out_relation);
}

static void
Expand All @@ -133,6 +140,12 @@ ems_hmd_get_view_poses(struct xrt_device *xdev,
out_poses);
}

xrt_vec3
to_xrt_vec3(const em_proto_Vec3 &v)
{
return {v.x, v.y, v.z};
}

static void
ems_hmd_handle_data(enum ems_callbacks_event event, const em_proto_UpMessage *message, void *userdata)
{
Expand All @@ -142,20 +155,35 @@ ems_hmd_handle_data(enum ems_callbacks_event event, const em_proto_UpMessage *me
return;
}
struct xrt_pose pose = {};
pose.position = {message->tracking.P_localSpace_viewSpace.position.x,
message->tracking.P_localSpace_viewSpace.position.y,
message->tracking.P_localSpace_viewSpace.position.z};
pose.position = to_xrt_vec3(message->tracking.P_localSpace_viewSpace.position);

pose.orientation.w = message->tracking.P_localSpace_viewSpace.orientation.w;
pose.orientation.x = message->tracking.P_localSpace_viewSpace.orientation.x;
pose.orientation.y = message->tracking.P_localSpace_viewSpace.orientation.y;
pose.orientation.z = message->tracking.P_localSpace_viewSpace.orientation.z;

uint64_t now = os_monotonic_get_ns();
// TODO handle timestamp, etc

xrt_space_relation rel = XRT_SPACE_RELATION_ZERO;
rel.pose = pose;
math_quat_normalize(&rel.pose.orientation);
rel.relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
if (message->tracking.has_V_localSpace_viewSpace_linear) {
rel.linear_velocity = to_xrt_vec3(message->tracking.V_localSpace_viewSpace_linear);
rel.relation_flags =
(enum xrt_space_relation_flags)(rel.relation_flags | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
}
if (message->tracking.has_V_localSpace_viewSpace_angular) {
rel.angular_velocity = to_xrt_vec3(message->tracking.V_localSpace_viewSpace_angular);
rel.relation_flags =
(enum xrt_space_relation_flags)(rel.relation_flags | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
}
{
std::lock_guard<std::mutex> lock(eh->received->mutex);
eh->received->pose = pose;
eh->received->rel = rel;
eh->received->timestamp = now - kFixedAssumedLatencyUs;
eh->received->updated = true;
}
}
Expand Down Expand Up @@ -185,7 +213,7 @@ ems_hmd_create(ems_instance &emsi)

// Private data.
eh->instance = &emsi;
eh->pose = (struct xrt_pose){XRT_QUAT_IDENTITY, {0.0f, 1.6f, 0.0f}};
// eh->pose = (struct xrt_pose){XRT_QUAT_IDENTITY, {0.0f, 1.6f, 0.0f}};
eh->log_level = debug_get_log_option_sample_log();

// Print name.
Expand Down Expand Up @@ -249,7 +277,6 @@ ems_hmd_create(ems_instance &emsi)
// TODO: Are we going to have any actual useful info to show here?
// Setup variable tracker: Optional but useful for debugging
u_var_add_root(eh, "Electric Maple Server HMD", true);
u_var_add_pose(eh, &eh->pose, "pose");
u_var_add_log_level(eh, &eh->log_level, "log_level");

return eh;
Expand Down
17 changes: 9 additions & 8 deletions server/src/ems/ems_server_internal.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#pragma once

#include "os/os_threading.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_system.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_instance.h"
Expand All @@ -19,18 +20,18 @@
#include "util/u_pacing.h"
#include "util/u_logging.h"

#include <memory>
#include <mutex>
#include <stdio.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>

#include <thread>
#include <atomic>
#include <memory>
#include <mutex>
#include <mutex>
#include <stdint.h>
#include <stdio.h>
#include <thread>


struct ems_callbacks;
Expand All @@ -41,20 +42,20 @@ struct ems_hmd_recvbuf
{
std::atomic_bool updated;
std::mutex mutex;
struct xrt_pose pose;
uint64_t timestamp;
xrt_space_relation rel = XRT_SPACE_RELATION_ZERO;
};

struct ems_hmd
{
//! Has to come first.
struct xrt_device base;

struct xrt_pose pose;
struct m_relation_history *pose_history;

// Should outlive us
struct ems_instance *instance;

// struct os_mutex mutex;
std::unique_ptr<ems_hmd_recvbuf> received;
enum u_logging_level log_level;
};
Expand Down