diff --git a/client/src/main.cpp b/client/src/main.cpp index 1e70ed36..3d768d41 100644 --- a/client/src/main.cpp +++ b/client/src/main.cpp @@ -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(state.width), static_cast(state.height)}; EmRemoteExperience *remote_experience = em_remote_experience_new(state.connection, stream_client, state.instance, state.session, &eye_extents); @@ -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 // diff --git a/proto/electricmaple.proto b/proto/electricmaple.proto index 3b18cf23..199de989 100644 --- a/proto/electricmaple.proto +++ b/proto/electricmaple.proto @@ -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 diff --git a/proto/generated/electricmaple.pb.h b/proto/generated/electricmaple.pb.h index 667fc0f2..28cdf818 100644 --- a/proto/generated/electricmaple.pb.h +++ b/proto/generated/electricmaple.pb.h @@ -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 { @@ -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} @@ -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} @@ -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 @@ -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 @@ -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) \ @@ -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 diff --git a/server/src/ems/ems_hmd.cpp b/server/src/ems/ems_hmd.cpp index deb74727..0107cf07 100644 --- a/server/src/ems/ems_hmd.cpp +++ b/server/src/ems/ems_hmd.cpp @@ -15,7 +15,6 @@ #include "ems_callbacks.h" #include "xrt/xrt_defines.h" -#include #undef CLAMP #include "xrt/xrt_device.h" @@ -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" @@ -40,12 +40,12 @@ #include "ems_server_internal.h" +#include #include +#include #include #include -#include - /* @@ -60,6 +60,7 @@ * @implements xrt_device */ +static constexpr uint64_t kFixedAssumedLatencyUs = 50 * U_TIME_1MS_IN_NS; /// Casting helper function @@ -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); @@ -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 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 @@ -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) { @@ -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 lock(eh->received->mutex); - eh->received->pose = pose; + eh->received->rel = rel; + eh->received->timestamp = now - kFixedAssumedLatencyUs; eh->received->updated = true; } } @@ -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. @@ -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; diff --git a/server/src/ems/ems_server_internal.h b/server/src/ems/ems_server_internal.h index 0c3cc317..4d36ff01 100644 --- a/server/src/ems/ems_server_internal.h +++ b/server/src/ems/ems_server_internal.h @@ -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" @@ -19,18 +20,18 @@ #include "util/u_pacing.h" #include "util/u_logging.h" -#include -#include -#include - #include #include #include #include -#include #include +#include #include +#include +#include +#include +#include struct ems_callbacks; @@ -41,7 +42,8 @@ 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 @@ -49,12 +51,11 @@ 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 received; enum u_logging_level log_level; };