diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 8059e5b2712a70379a89183250ab8cbc0758873a..536daee44ad85087ba852abac94d119526ec77fa 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -66,34 +66,6 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
return rz * rx * ry;
}
-static void writeSourceProperties(ftl::codecs::Writer &writer, int id, ftl::rgbd::Source *src) {
- ftl::codecs::StreamPacket spkt;
- ftl::codecs::Packet pkt;
-
- spkt.timestamp = 0;
- spkt.streamID = id;
- spkt.channel = Channel::Calibration;
- spkt.channel_count = 1;
- pkt.codec = ftl::codecs::codec_t::CALIBRATION;
- 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*)&src->parameters(), (uint8_t*)&src->parameters() + sizeof(ftl::rgbd::Camera)));
-
- writer.write(spkt, pkt);
-
- spkt.channel = Channel::Pose;
- pkt.codec = ftl::codecs::codec_t::POSE;
- 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*)src->getPose().data(), (uint8_t*)src->getPose().data() + 4*4*sizeof(double)));
-
- writer.write(spkt, pkt);
-}
-
static void run(ftl::Configurable *root) {
Universe *net = ftl::create<Universe>(root, "net");
ftl::ctrl::Slave slave(net, root);
@@ -128,7 +100,7 @@ static void run(ftl::Configurable *root) {
LOG(INFO) << "Found " << (max_stream+1) << " sources in " << path;
// For each stream found, add a source object
- for (int i=0; i<max_stream; ++i) {
+ for (int i=0; i<=max_stream; ++i) {
root->getConfig()["sources"].push_back(nlohmann::json{{"uri",std::string("file://") + path + std::string("#") + std::to_string(i)}});
}
}
@@ -246,14 +218,15 @@ static void run(ftl::Configurable *root) {
fileout.open(std::string(timestamp) + ".ftl");
writer.begin();
+ group->addRawCallback(std::function(recorder));
// TODO: Write pose+calibration+config packets
auto sources = group->sources();
for (int i=0; i<sources.size(); ++i) {
- writeSourceProperties(writer, i, sources[i]);
+ //writeSourceProperties(writer, i, sources[i]);
+ sources[i]->inject(Channel::Calibration, sources[i]->parameters(), Channel::Left, sources[i]->getCapabilities());
+ sources[i]->inject(sources[i]->getPose());
}
-
- group->addRawCallback(std::function(recorder));
} else {
group->removeRawCallback(recorder);
writer.end();
diff --git a/applications/recorder/src/main.cpp b/applications/recorder/src/main.cpp
index a6f8926a5afa5d02112ca943c6a48af89a17c5fa..c00daa9755a931dfc894e191746c8bfe52558cc3 100644
--- a/applications/recorder/src/main.cpp
+++ b/applications/recorder/src/main.cpp
@@ -62,34 +62,6 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
return rz * rx * ry;
}
-static void writeSourceProperties(ftl::codecs::Writer &writer, int id, ftl::rgbd::Source *src) {
- ftl::codecs::StreamPacket spkt;
- ftl::codecs::Packet pkt;
-
- spkt.timestamp = 0;
- spkt.streamID = id;
- spkt.channel = Channel::Calibration;
- spkt.channel_count = 1;
- pkt.codec = ftl::codecs::codec_t::CALIBRATION;
- 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*)&src->parameters(), (uint8_t*)&src->parameters() + sizeof(ftl::rgbd::Camera)));
-
- writer.write(spkt, pkt);
-
- spkt.channel = Channel::Pose;
- pkt.codec = ftl::codecs::codec_t::POSE;
- 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*)src->getPose().data(), (uint8_t*)src->getPose().data() + 4*4*sizeof(double)));
-
- writer.write(spkt, pkt);
-}
-
static void run(ftl::Configurable *root) {
Universe *net = ftl::create<Universe>(root, "net");
ftl::ctrl::Slave slave(net, root);
@@ -199,7 +171,9 @@ static void run(ftl::Configurable *root) {
// TODO: Write pose+calibration+config packets
auto sources = group->sources();
for (int i=0; i<sources.size(); ++i) {
- writeSourceProperties(writer, i, sources[i]);
+ //writeSourceProperties(writer, i, sources[i]);
+ sources[i]->inject(Channel::Calibration, sources[i]->parameters(), Channel::Left, sources[i]->getCapabilities());
+ sources[i]->inject(sources[i]->getPose());
}
} else {
//group->removeRawCallback(recorder);
diff --git a/components/codecs/include/ftl/codecs/writer.hpp b/components/codecs/include/ftl/codecs/writer.hpp
index 044624ba837f17c83e4a22a16787ec86edd4ad67..94d82f1ecbfb697bdbad35acc5a4cf29dad22410 100644
--- a/components/codecs/include/ftl/codecs/writer.hpp
+++ b/components/codecs/include/ftl/codecs/writer.hpp
@@ -5,6 +5,7 @@
#include <msgpack.hpp>
//#include <Eigen/Eigen>
+#include <ftl/threads.hpp>
#include <ftl/codecs/packet.hpp>
namespace ftl {
@@ -24,6 +25,7 @@ class Writer {
msgpack::sbuffer buffer_;
int64_t timestart_;
bool active_;
+ MUTEX mutex_;
};
}
diff --git a/components/codecs/src/reader.cpp b/components/codecs/src/reader.cpp
index 97a74acd1a16c29e7d5c926382759aaf765b1818..ba68f26c9666da9a84ea86632caa1035bb6c5c88 100644
--- a/components/codecs/src/reader.cpp
+++ b/components/codecs/src/reader.cpp
@@ -37,6 +37,22 @@ bool Reader::begin() {
return true;
}
+/*static void printMsgpack(msgpack::object &obj) {
+ switch (obj.type) {
+ case msgpack::type::NIL: return;
+ case msgpack::type::BOOLEAN: LOG(INFO) << "BOOL " << obj.as<bool>(); return;
+ case msgpack::type::POSITIVE_INTEGER:
+ case msgpack::type::NEGATIVE_INTEGER: LOG(INFO) << "INT " << obj.as<int>(); return;
+ case msgpack::type::FLOAT32: LOG(INFO) << "FLOAT " << obj.as<float>(); return;
+ case msgpack::type::FLOAT64: LOG(INFO) << "DOUBLE " << obj.as<double>(); return;
+ case msgpack::type::STR: LOG(INFO) << "STRING " << obj.as<std::string>(); return;
+ case msgpack::type::BIN: return;
+ case msgpack::type::EXT: return;
+ case msgpack::type::ARRAY: LOG(INFO) << "ARRAY: "; return;
+ case msgpack::type::MAP: LOG(INFO) << "MAP: "; return;
+ }
+}*/
+
bool Reader::read(int64_t ts, const std::function<void(const ftl::codecs::StreamPacket &, ftl::codecs::Packet &)> &f) {
//UNIQUE_LOCK(mtx_, lk);
std::unique_lock<std::mutex> lk(mtx_, std::defer_lock);
@@ -75,10 +91,13 @@ bool Reader::read(int64_t ts, const std::function<void(const ftl::codecs::Stream
//std::tuple<StreamPacket,Packet> data;
msgpack::object obj = msg.get();
+
+ //printMsgpack(obj);
+
try {
obj.convert(data_.emplace_back());
} catch (std::exception &e) {
- LOG(INFO) << "Corrupt message: " << buffer_.nonparsed_size();
+ LOG(INFO) << "Corrupt message: " << buffer_.nonparsed_size() << " - " << e.what();
//partial = true;
//continue;
return false;
diff --git a/components/codecs/src/writer.cpp b/components/codecs/src/writer.cpp
index d7582f11866b701796ab311016d2a01a5218595a..1e3841a8e05f840f808aaddf127833392212af05 100644
--- a/components/codecs/src/writer.cpp
+++ b/components/codecs/src/writer.cpp
@@ -41,7 +41,8 @@ bool Writer::write(const ftl::codecs::StreamPacket &s, const ftl::codecs::Packet
auto data = std::make_tuple(s2,p);
msgpack::sbuffer buffer;
msgpack::pack(buffer, data);
+
+ UNIQUE_LOCK(mutex_, lk);
(*stream_).write(buffer.data(), buffer.size());
- //buffer_.clear();
return true;
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 586e7a5784f01696eb46c7e87d5f58b6f01b9681..6892e9cc7e77a52ede955d6fc7a398af13da790f 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -216,6 +216,8 @@ class Source : public ftl::Configurable {
template <typename... ARGS>
void inject(ftl::codecs::Channel c, ARGS... args);
+ void inject(const Eigen::Matrix4d &pose);
+
protected:
detail::Source *impl_;
Eigen::Matrix4d pose_;
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 4c7485460d087101742c0d0203d88e50817e49c6..52dae351ed27d134119cfbd3bc572c4ae819f355 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -323,3 +323,21 @@ void Source::notify(int64_t ts, cv::Mat &c1, cv::Mat &c2) {
if (callback_) callback_(ts, c1, c2);
}
+
+void Source::inject(const Eigen::Matrix4d &pose) {
+ ftl::codecs::StreamPacket spkt;
+ ftl::codecs::Packet pkt;
+
+ spkt.timestamp = impl_->timestamp_;
+ spkt.channel_count = 0;
+ spkt.channel = Channel::Pose;
+ spkt.streamID = 0;
+ pkt.codec = ftl::codecs::codec_t::POSE;
+ 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)));
+
+ notifyRaw(spkt, pkt);
+}