Pack UDP send in tracking thread

This commit is contained in:
Patrick Nicolas committed 2024-10-02 18:39:40 +02:00
1 parent 0d21fb6980
commit 00a4562625
5 files changed
+156 -43

No files matched your search

+34 -38
View File
@@ -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<from_headset::tracking> tracking;
std::vector<from_headset::hand_tracking> hands;
std::vector<from_headset::fb_face2> 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<XrDuration>(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<serialization_packet> 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)
+7 -2
View File
@@ -27,8 +27,13 @@ using namespace xrt::drivers::wivrn;
class wivrn_session
{
typed_socket<TCP, to_headset::packets, from_headset::packets> control;
typed_socket<UDP, to_headset::packets, from_headset::packets> stream;
public:
using control_socket_t = typed_socket<TCP, to_headset::packets, from_headset::packets>;
using stream_socket_t = typed_socket<UDP, to_headset::packets, from_headset::packets>;
private:
control_socket_t control;
stream_socket_t stream;
template <typename T>
void handshake(T address, bool tcp_only);
+4
View File
@@ -138,6 +138,10 @@ public:
serialization_traits<T>::serialize(value, *this);
}
operator const std::vector<std::span<uint8_t>> *()
{
return &this->operator const std::vector<std::span<uint8_t>> &();
}
operator const std::vector<std::span<uint8_t>> &()
{
exp_spans.clear();
+90
View File
@@ -359,6 +359,40 @@ void xrt::drivers::wivrn::UDP::send_raw(const std::vector<std::span<uint8_t>> &
throw std::system_error{errno, std::generic_category()};
}
void xrt::drivers::wivrn::UDP::send_many_raw(std::span<const std::vector<std::span<uint8_t>> *> data)
{
thread_local std::vector<iovec> iovecs;
thread_local std::vector<mmsghdr> 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<std::span<uint8_t>> &
hdr.msg_iov[0].iov_len -= sent;
}
}
void xrt::drivers::wivrn::TCP::send_many_raw(std::span<const std::vector<std::span<uint8_t>> *> data)
{
thread_local std::vector<iovec> iovecs;
thread_local std::vector<uint16_t> 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;
}
}
+21 -3
View File
@@ -112,6 +112,7 @@ public:
std::pair<xrt::drivers::wivrn::deserialization_packet, sockaddr_in6> receive_from_raw();
void send_raw(const std::vector<uint8_t> & data);
void send_raw(const std::vector<std::span<uint8_t>> & data);
void send_many_raw(std::span<const std::vector<std::span<uint8_t>> *> 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<std::span<uint8_t>> & data);
void send_many_raw(std::span<const std::vector<std::span<uint8_t>> *> data);
};
using UnixDatagram = UDP;
@@ -212,16 +214,32 @@ public:
return packet.deserialize<ReceivedType>();
}
// WARNING: serialization packet keeps references to data
template <typename T>
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::decay_t<T>, std::tuple<VariantTypes...>>::value;
p.serialize(index);
p.serialize(std::forward<T>(data));
p.serialize(data);
}
template <typename T>
void send(const T & data)
{
thread_local serialization_packet p;
serialize(p, data);
this->send_raw(p);
}
void send(const std::span<serialization_packet> & packets)
{
thread_local std::vector<const std::vector<std::span<uint8_t>> *> data;
data.clear();
for (auto & packet: packets)
data.emplace_back(packet);
this->send_many_raw(data);
}
};
} // namespace xrt::drivers::wivrn