Add data channel and tests to frame

components/common/cpp/include/ftl/utility/vectorbuffer.hpp

+#ifndef _FTL_UTILITY_VECTORBUFFER_HPP_
+#define _FTL_UTILITY_VECTORBUFFER_HPP_
+
+#include <vector>
+
+namespace ftl {
+namespace util {
+class FTLVectorBuffer {
+	public:
+	inline explicit FTLVectorBuffer(std::vector<unsigned char> &v) : vector_(v) {}
+
+	inline void write(const char *data, std::size_t size) {
+		vector_.insert(vector_.end(), (const unsigned char*)data, (const unsigned char*)data+size);
+	}
+
+	private:
+	std::vector<unsigned char> &vector_;
+};
+}
+}
+
+#endif  // _FTL_UTILITY_VECTORBUFFER_HPP_

components/rgbd-sources/include/ftl/rgbd/frame.hpp

@@ -13,6 +13,7 @@
 #include <ftl/rgbd/camera.hpp>
 #include <ftl/codecs/codecs.hpp>
 #include <ftl/codecs/packet.hpp>
+#include <ftl/utility/vectorbuffer.hpp>
 
 #include <ftl/cuda_common.hpp>
 
@@ -196,6 +197,12 @@ public:
 	 */
 	template <typename T> T &create(ftl::codecs::Channel c);
 
+	/**
+	 * Set the value of a channel. Some channels should not be modified via the
+	 * non-const get method, for example the data channels.
+	 */
+	template <typename T> void create(ftl::codecs::Channel channel, const T &value);
+
 	/**
 	 * Create a CUDA texture object for a channel. This version takes a format
 	 * argument to also create (or recreate) the associated GpuMat.
@@ -268,6 +275,7 @@ public:
 	inline bool hasChannel(ftl::codecs::Channel channel) const {
 		int c = static_cast<int>(channel);
 		if (c >= 64 && c <= 68) return true;
+		else if (c >= 2048) return data_channels_.has(channel);
 		else if (c >= 32) return false;
 		else return channels_.has(channel);
 	}
@@ -313,6 +321,8 @@ public:
 	 */
 	template <typename T> const T& get(ftl::codecs::Channel channel) const;
 
+	template <typename T> void get(ftl::codecs::Channel channel, T &params) const;
+
 	/**
 	 * Method to get reference to the channel content.
 	 * @param	Channel type
@@ -409,9 +419,11 @@ private:
 	};
 
 	std::array<ChannelData, ftl::codecs::Channels<0>::kMax> data_;
+	std::unordered_map<int, std::vector<unsigned char>> data_data_;
 
 	ftl::codecs::Channels<0> channels_;	// Does it have a channel
 	ftl::codecs::Channels<0> gpu_;		// Is the channel on a GPU
+	ftl::codecs::Channels<2048> data_channels_;
 
 	// Persistent state
 	FrameState state_;
@@ -432,11 +444,34 @@ template<> cv::cuda::GpuMat& Frame::get(ftl::codecs::Channel channel);
 //template<> const Eigen::Matrix4d &Frame::get(ftl::codecs::Channel channel) const;
 template<> const ftl::rgbd::Camera &Frame::get(ftl::codecs::Channel channel) const;
 
+// Default data channel implementation
+template <typename T>
+void Frame::get(ftl::codecs::Channel channel, T &params) const {
+	if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel");
+	if (!hasChannel(channel)) throw ftl::exception("Data channel does not exist");
+
+	const auto &i = data_data_.find(static_cast<int>(channel));
+	if (i == data_data_.end()) throw ftl::exception("Data channel does not exist");
+
+	auto unpacked = msgpack::unpack((const char*)(*i).second.data(), (*i).second.size());
+	unpacked.get().convert(params);
+}
+
 template <> cv::Mat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &);
 template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &);
 template <> cv::Mat &Frame::create(ftl::codecs::Channel c);
 template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c);
 
+template <typename T>
+void Frame::create(ftl::codecs::Channel channel, const T &value) {
+	if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel");
+
+	data_channels_ += channel;
+	data_data_.insert({static_cast<int>(channel),{}});
+	ftl::util::FTLVectorBuffer buf(data_data_[static_cast<int>(channel)]);
+	msgpack::pack(buf, value);
+}
+
 template <typename T>
 ftl::cuda::TextureObject<T> &Frame::getTexture(ftl::codecs::Channel c) {
 	if (!channels_.has(c)) throw ftl::exception(ftl::Formatter() << "Texture channel does not exist: " << (int)c);

components/rgbd-sources/src/frame.cpp

@@ -76,6 +76,7 @@ void Frame::reset() {
 	origin_ = nullptr;
 	channels_.clear();
 	gpu_.clear();
+	data_channels_.clear();
 	for (size_t i=0u; i<Channels<0>::kMax; ++i) {
 		data_[i].encoded.clear();
 	}
@@ -190,6 +191,10 @@ void Frame::swapTo(ftl::codecs::Channels<0> channels, Frame &f) {
 			}
 		}
 	}
+
+	f.data_data_ = std::move(data_data_);
+	f.data_channels_ = data_channels_;
+	data_channels_.clear();
 }
 
 void Frame::swapChannels(ftl::codecs::Channel a, ftl::codecs::Channel b) {
@@ -225,6 +230,9 @@ void Frame::copyTo(ftl::codecs::Channels<0> channels, Frame &f) {
 			m2.encoded = m1.encoded; //std::move(m1.encoded);  // TODO: Copy?
 		}
 	}
+
+	f.data_data_ = data_data_;
+	f.data_channels_ = data_channels_;
 }
 
 template<> cv::Mat& Frame::get(ftl::codecs::Channel channel) {

components/rgbd-sources/test/frame_unit.cpp

@@ -377,3 +377,125 @@ TEST_CASE("Frame::get() Pose", "") {
 		REQUIRE( pose1 == pose2 );
 	}
 }
+
+TEST_CASE("Frame::get() Data channel", "") {
+	SECTION("Get valid data") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		f.create(Channel::Data, val_in);
+		f.get(Channel::Data, val_out);
+
+		REQUIRE( std::get<0>(val_in) == std::get<0>(val_out) );
+		REQUIRE( std::get<1>(val_in) == std::get<1>(val_out) );
+	}
+
+	SECTION("Read from non existing channel") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		//f.create(Channel::Data, val_in);
+
+		bool except = false;
+		try {
+			f.get(Channel::Data, val_out);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Read from non data channel") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		//f.create(Channel::Data, val_in);
+
+		bool except = false;
+		try {
+			f.get(Channel::Colour, val_out);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Use non data channel") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		bool except = false;
+		try {
+			f.create(Channel::Colour, val_in);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Mix types") {
+		Frame f;
+		
+		std::string val_in = "Hello World";
+		std::tuple<int,float> val_out;
+
+		f.create(Channel::Data, val_in);
+
+		bool except = false;
+		try {
+			f.get(Channel::Data, val_out);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Has channel after create") {
+		Frame f;
+		
+		std::string val_in = "Hello World";
+
+		REQUIRE( !f.hasChannel(Channel::Data) );
+		f.create(Channel::Data, val_in);
+		REQUIRE( f.hasChannel(Channel::Data) );
+	}
+}
+
+TEST_CASE("Frame::swapTo() Data channel", "") {
+	SECTION("Swap valid data") {
+		Frame f1;
+		Frame f2;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		auto val_in2 = std::make_tuple(52,7.0f);
+		decltype(val_in) val_out;
+
+		f1.create(Channel::Data, val_in);
+
+		REQUIRE( f1.hasChannel(Channel::Data) );
+		REQUIRE( !f2.hasChannel(Channel::Data) );
+
+		f1.swapTo(Channels<0>::All(), f2);
+
+		REQUIRE( !f1.hasChannel(Channel::Data) );
+		REQUIRE( f2.hasChannel(Channel::Data) );
+
+		f1.create(Channel::Data, val_in2);
+		f2.get(Channel::Data, val_out);
+
+		REQUIRE( std::get<0>(val_in) == std::get<0>(val_out) );
+		REQUIRE( std::get<1>(val_in) == std::get<1>(val_out) );
+	}
+}

components/streams/src/injectors.cpp

 #include "injectors.hpp"
+#include <ftl/utility/vectorbuffer.hpp>
 
 using ftl::codecs::Channel;
-
-class VectorBuffer2 {
-	public:
-	inline explicit VectorBuffer2(std::vector<unsigned char> &v) : vector_(v) {}
-
-	inline void write(const char *data, std::size_t size) {
-		vector_.insert(vector_.end(), (const unsigned char*)data, (const unsigned char*)data+size);
-	}
-
-	private:
-	std::vector<unsigned char> &vector_;
-};
+using ftl::util::FTLVectorBuffer;
 
 void ftl::stream::injectCalibration(ftl::stream::Stream *stream, const ftl::rgbd::FrameSet &fs, int ix, bool right) {
 	ftl::stream::injectCalibration(stream, fs.frames[ix], fs.timestamp, ix, right);
@@ -38,7 +28,7 @@ void ftl::stream::injectConfig(ftl::stream::Stream *stream, const ftl::rgbd::Fra
 	pkt.frame_count = 1;
 	pkt.flags = 0;
 
-	VectorBuffer2 buf(pkt.data);
+	FTLVectorBuffer buf(pkt.data);
 	msgpack::pack(buf, fs.frames[ix].getConfigString());
 
 	stream->post(spkt, pkt);
@@ -62,7 +52,7 @@ void ftl::stream::injectPose(ftl::stream::Stream *stream, const ftl::rgbd::Frame
 
 	auto &pose = f.getPose();
 	std::vector<double> data(pose.data(), pose.data() + 4*4);
-	VectorBuffer2 buf(pkt.data);
+	FTLVectorBuffer buf(pkt.data);
 	msgpack::pack(buf, data);
 
 	stream->post(spkt, pkt);
@@ -88,7 +78,7 @@ void ftl::stream::injectCalibration(ftl::stream::Stream *stream, const ftl::rgbd
 	pkt.frame_count = 1;
 	pkt.flags = 0;
 
-	VectorBuffer2 buf(pkt.data);
+	FTLVectorBuffer buf(pkt.data);
 	msgpack::pack(buf, data);
 	stream->post(spkt, pkt);
 }