diff --git a/client/scenes/stream_tracking.cpp b/client/scenes/stream_tracking.cpp index 00157368..038bcafc 100644 --- a/client/scenes/stream_tracking.cpp +++ b/client/scenes/stream_tracking.cpp @@ -82,18 +82,9 @@ class timer public: timer(xr::instance & instance) : instance(instance) {} - void pause() - { - duration += instance.now() - start; - } - void resume() - { - start = instance.now(); - } XrDuration count() { - pause(); - return duration; + return instance.now() - start; } }; } // namespace @@ -200,13 +191,19 @@ void scenes::stream::tracking() const XrDuration dt = 100'000; // Wake up 0.1ms before measuring the position XrTime t0 = instance.now(); - from_headset::tracking packet{}; + std::vector tracking; + std::vector hands; + std::vector face; int skip_samples = 0; while (not exiting) { try { + tracking.clear(); + hands.clear(); + face.clear(); + XrTime now = instance.now(); if (now < t0) std::this_thread::sleep_for(std::chrono::nanoseconds(t0 - now - dt)); @@ -217,12 +214,6 @@ void scenes::stream::tracking() timer t(instance); int samples = 0; - auto send_stream = [&, this](auto && data) { - t.pause(); - network_session->send_stream(data); - t.resume(); - }; - to_headset::tracking_control control; { std::lock_guard lock(tracking_control_mutex); @@ -233,16 +224,9 @@ void scenes::stream::tracking() auto period = std::max(display_time_period.load(), 1'000'000); for (XrDuration Δt = 0; Δt <= prediction + period / 2; Δt += period, ++samples) { - from_headset::hand_tracking hands{}; - from_headset::fb_face2 fb_face2{}; - + auto & packet = tracking.emplace_back(); packet.production_timestamp = t0; - hands.production_timestamp = t0; - fb_face2.production_timestamp = t0; - packet.timestamp = t0 + Δt; - hands.timestamp = t0 + Δt; - fb_face2.timestamp = t0 + Δt; try { @@ -268,29 +252,30 @@ void scenes::stream::tracking() packet.device_poses.push_back(locate_space(device, space, world_space, t0 + Δt)); } - send_stream(packet); - if (application::get_hand_tracking_supported()) { if (control.enabled[size_t(tid::left_hand)]) - { - hands.hand = xrt::drivers::wivrn::from_headset::hand_tracking::left; - hands.joints = locate_hands(application::get_left_hand(), world_space, hands.timestamp); - send_stream(hands); - } + hands.emplace_back( + t0, + t0 + Δt, + from_headset::hand_tracking::left, + locate_hands(application::get_left_hand(), world_space, t0 + Δt)); if (control.enabled[size_t(tid::right_hand)]) - { - hands.hand = xrt::drivers::wivrn::from_headset::hand_tracking::right; - hands.joints = locate_hands(application::get_right_hand(), world_space, hands.timestamp); - send_stream(hands); - } + hands.emplace_back( + t0, + t0 + Δt, + from_headset::hand_tracking::right, + locate_hands(application::get_right_hand(), world_space, t0 + Δt)); } if (application::get_fb_face_tracking2_supported() and control.enabled[size_t(tid::face)]) { + auto & fb_face2 = face.emplace_back(); + fb_face2.production_timestamp = t0; + fb_face2.timestamp = t0 + Δt; + application::get_fb_face_tracker2().get_weights(t0 + Δt, &fb_face2); - send_stream(fb_face2); } } catch (const std::system_error & e) @@ -346,6 +331,17 @@ void scenes::stream::tracking() } #endif + std::vector packets; + packets.reserve(tracking.size() + hands.size() + face.size()); + for (const auto & i: tracking) + wivrn_session::stream_socket_t::serialize(packets.emplace_back(), i); + for (const auto & i: hands) + wivrn_session::stream_socket_t::serialize(packets.emplace_back(), i); + for (const auto & i: face) + wivrn_session::stream_socket_t::serialize(packets.emplace_back(), i); + + network_session->send_stream(std::span(packets)); + t0 += tracking_period; } catch (std::exception & e) diff --git a/client/wivrn_client.h b/client/wivrn_client.h index ff8b9ca1..eb85241e 100644 --- a/client/wivrn_client.h +++ b/client/wivrn_client.h @@ -27,8 +27,13 @@ using namespace xrt::drivers::wivrn; class wivrn_session { - typed_socket control; - typed_socket stream; +public: + using control_socket_t = typed_socket; + using stream_socket_t = typed_socket; + +private: + control_socket_t control; + stream_socket_t stream; template void handshake(T address, bool tcp_only); diff --git a/common/wivrn_serialization.h b/common/wivrn_serialization.h index 51d95667..2b772de0 100644 --- a/common/wivrn_serialization.h +++ b/common/wivrn_serialization.h @@ -138,6 +138,10 @@ public: serialization_traits::serialize(value, *this); } + operator const std::vector> *() + { + return &this->operator const std::vector> &(); + } operator const std::vector> &() { exp_spans.clear(); diff --git a/common/wivrn_sockets.cpp b/common/wivrn_sockets.cpp index 4db9da8a..42392e74 100644 --- a/common/wivrn_sockets.cpp +++ b/common/wivrn_sockets.cpp @@ -359,6 +359,40 @@ void xrt::drivers::wivrn::UDP::send_raw(const std::vector> & throw std::system_error{errno, std::generic_category()}; } +void xrt::drivers::wivrn::UDP::send_many_raw(std::span> *> data) +{ + thread_local std::vector iovecs; + thread_local std::vector mmsgs; + iovecs.clear(); + mmsgs.clear(); + for (const auto & message: data) + { + for (const auto & span: *message) + { + iovecs.push_back( + { + .iov_base = span.data(), + .iov_len = span.size_bytes(), + }); + } + } + size_t i = 0; + for (const auto & message: data) + { + mmsgs.push_back( + { + .msg_hdr = { + .msg_iov = &iovecs[i], + .msg_iovlen = message->size(), + }, + }); + i += message->size(); + } + // sendmmsg may not send all messages, just consider them as lost for UDP + if (sendmmsg(fd, mmsgs.data(), mmsgs.size(), 0) < 0) + throw std::system_error{errno, std::generic_category()}; +} + xrt::drivers::wivrn::deserialization_packet xrt::drivers::wivrn::TCP::receive_raw() { ssize_t expected_size; @@ -482,3 +516,59 @@ void xrt::drivers::wivrn::TCP::send_raw(const std::vector> & hdr.msg_iov[0].iov_len -= sent; } } + +void xrt::drivers::wivrn::TCP::send_many_raw(std::span> *> data) +{ + thread_local std::vector iovecs; + thread_local std::vector sizes; + iovecs.clear(); + sizes.clear(); + + sizes.reserve(data.size()); + for (const auto & spans: data) + { + auto & size = sizes.emplace_back(0); + iovecs.emplace_back(&size, sizeof(size)); + for (const auto & span: *spans) + { + size += span.size_bytes(); + iovecs.emplace_back(span.data(), span.size_bytes()); + } + } + + msghdr hdr{ + .msg_name = nullptr, + .msg_namelen = 0, + .msg_iov = iovecs.data(), + .msg_iovlen = iovecs.size(), + .msg_control = nullptr, + .msg_controllen = 0, + .msg_flags = 0, + }; + + std::lock_guard lock(*mutex); + while (true) + { + ssize_t sent = ::sendmsg(fd, &hdr, MSG_NOSIGNAL); + + if (sent == 0) + throw socket_shutdown{}; + + if (sent < 0) + throw std::system_error{errno, std::generic_category()}; + + bytes_sent_ += sent; + + // iov fully consumed + while (hdr.msg_iovlen > 0 and sent >= hdr.msg_iov[0].iov_len) + { + sent -= hdr.msg_iov[0].iov_len; + ++hdr.msg_iov; + --hdr.msg_iovlen; + } + if (hdr.msg_iovlen == 0) + return; + hdr.msg_iov[0].iov_base = (void *)((uintptr_t)hdr.msg_iov[0].iov_base + sent); + hdr.msg_iov[0].iov_len -= sent; + } +} diff --git a/common/wivrn_sockets.h b/common/wivrn_sockets.h index 05b0767c..cb00cf9e 100644 --- a/common/wivrn_sockets.h +++ b/common/wivrn_sockets.h @@ -112,6 +112,7 @@ public: std::pair receive_from_raw(); void send_raw(const std::vector & data); void send_raw(const std::vector> & data); + void send_many_raw(std::span> *> data); void connect(in6_addr address, int port); void connect(in_addr address, int port); @@ -140,6 +141,7 @@ public: deserialization_packet receive_raw(); deserialization_packet receive_pending(); void send_raw(const std::vector> & data); + void send_many_raw(std::span> *> data); }; using UnixDatagram = UDP; @@ -212,16 +214,32 @@ public: return packet.deserialize(); } + // WARNING: serialization packet keeps references to data template - void send(T && data) + static void serialize(serialization_packet & p, const T & data) { - thread_local serialization_packet p; p.clear(); uint8_t index = details::Index, std::tuple>::value; p.serialize(index); - p.serialize(std::forward(data)); + p.serialize(data); + } + + template + void send(const T & data) + { + thread_local serialization_packet p; + serialize(p, data); this->send_raw(p); } + + void send(const std::span & packets) + { + thread_local std::vector> *> data; + data.clear(); + for (auto & packet: packets) + data.emplace_back(packet); + this->send_many_raw(data); + } }; } // namespace xrt::drivers::wivrn