diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp index 4a7873515a2a77faf44965e94208ec005085caf8..f4f217af128b559c45c0d109de3a670c29393159 100644 --- a/components/rgbd-sources/src/source.cpp +++ b/components/rgbd-sources/src/source.cpp @@ -314,12 +314,15 @@ void Source::inject(const Eigen::Matrix4d &pose) { spkt.channel_count = 0; spkt.channel = Channel::Pose; spkt.streamID = 0; - pkt.codec = ftl::codecs::codec_t::POSE; + pkt.codec = ftl::codecs::codec_t::MSGPACK; pkt.definition = ftl::codecs::definition_t::Any; pkt.block_number = 0; pkt.block_total = 1; pkt.flags = 0; - pkt.data = std::move(std::vector<uint8_t>((uint8_t*)pose.data(), (uint8_t*)pose.data() + 4*4*sizeof(double))); + + std::vector<double> data(pose.data(), pose.data() + 4*4*sizeof(double)); + VectorBuffer buf(pkt.data); + msgpack::pack(buf, data); notifyRaw(spkt, pkt); } diff --git a/components/rgbd-sources/src/sources/ftlfile/file_source.cpp b/components/rgbd-sources/src/sources/ftlfile/file_source.cpp index 25033f9af59172f652919ad8868f343bbbab00e8..554a558b3b5cecffc94f59ae8f3f98a65051d72e 100644 --- a/components/rgbd-sources/src/sources/ftlfile/file_source.cpp +++ b/components/rgbd-sources/src/sources/ftlfile/file_source.cpp @@ -93,7 +93,12 @@ void FileSource::_processPose(ftl::codecs::Packet &pkt) { Eigen::Matrix4d p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data()); host_->setPose(p); } else if (pkt.codec == codec_t::MSGPACK) { + auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size()); + std::vector<double> posevec; + unpacked.get().convert(posevec); + Eigen::Matrix4d p(posevec.data()); + host_->setPose(p); } } diff --git a/components/rgbd-sources/src/sources/net/net.cpp b/components/rgbd-sources/src/sources/net/net.cpp index aeb1b954a0c4228304d97518ea007e7bffda52fd..c29ad5491b0b0a15a3d8549936bfe14b18762330 100644 --- a/components/rgbd-sources/src/sources/net/net.cpp +++ b/components/rgbd-sources/src/sources/net/net.cpp @@ -8,6 +8,7 @@ #include "colour.hpp" #include <ftl/rgbd/streamer.hpp> +#include <ftl/codecs/bitrates.hpp> using ftl::rgbd::detail::NetFrame; using ftl::rgbd::detail::NetFrameQueue; @@ -21,6 +22,7 @@ using std::this_thread::sleep_for; using std::chrono::milliseconds; using std::tuple; using ftl::codecs::Channel; +using ftl::codecs::codec_t; // ===== NetFrameQueue ========================================================= @@ -250,7 +252,18 @@ void NetSource::_processCalibration(const ftl::codecs::Packet &pkt) { } void NetSource::_processPose(const ftl::codecs::Packet &pkt) { - LOG(INFO) << "Got POSE channel"; + if (pkt.codec == ftl::codecs::codec_t::POSE) { + Eigen::Matrix4d p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data()); + //host_->setPose(p); + } else if (pkt.codec == ftl::codecs::codec_t::MSGPACK) { + auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size()); + std::vector<double> posevec; + unpacked.get().convert(posevec); + + Eigen::Matrix4d p(posevec.data()); + //host_->setPose(p); + // TODO: What to do with pose? + } } void NetSource::_checkDataRate(size_t tx_size, int64_t tx_latency) { diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp index 3ca2bade8ddb4355682a0002beb74749d62a3ffb..a672c1bd136e03ed52f99dbd38237b52efb67472 100644 --- a/components/rgbd-sources/src/streamer.cpp +++ b/components/rgbd-sources/src/streamer.cpp @@ -343,7 +343,7 @@ void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID // Finally, inject calibration and config data s->src->inject(Channel::Calibration, s->src->parameters(Channel::Left), Channel::Left, s->src->getCapabilities()); s->src->inject(Channel::Calibration, s->src->parameters(Channel::Right), Channel::Right, s->src->getCapabilities()); - //s->src->inject(s->src->getPose()); + s->src->inject(s->src->getPose()); //if (!(*s->src->get<nlohmann::json>("meta")).is_null()) { s->src->inject(Channel::Configuration, "/original", s->src->getConfig().dump()); //}