Use msgpack for pose

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);
 }

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);
 	}
 }
 

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) {

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());
 	//}