From 2541ac256f210f98f391627610a802126ab68730 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Tue, 17 Dec 2019 13:00:57 +0200
Subject: [PATCH] Use msgpack for pose
---
components/rgbd-sources/src/source.cpp | 7 +++++--
.../src/sources/ftlfile/file_source.cpp | 5 +++++
components/rgbd-sources/src/sources/net/net.cpp | 15 ++++++++++++++-
components/rgbd-sources/src/streamer.cpp | 2 +-
4 files changed, 25 insertions(+), 4 deletions(-)
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 4a7873515..f4f217af1 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 25033f9af..554a558b3 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 aeb1b954a..c29ad5491 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 3ca2bade8..a672c1bd1 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());
//}
--
GitLab