22#include "mongodb_tf_transformer.h"
24#include <bsoncxx/builder/basic/document.hpp>
27#ifdef HAVE_MONGODB_VERSION_H
29# define QUERY MONGO_QUERY
32using namespace mongocxx;
48 const std::string &database_name,
50: mongodb_client_(mongodb_client), database_(database_name)
53 using namespace bsoncxx::builder;
54 mongodb_client_->database(database_)[
"tf"].create_index(
55 basic::make_document(basic::kvp(
"timestamp", 1)));
56 mongodb_client_->database(database_)[
"TransformInterface"].create_index(
57 basic::make_document(basic::kvp(
"timestamp", 1)));
80MongoDBTransformer::restore_tf_doc(
const bsoncxx::document::view &doc,
82 long long new_start_msec)
84 bsoncxx::array::view trans = doc[
"translation"].get_array();
85 bsoncxx::array::view rot = doc[
"rotation"].get_array();
86 double rx, ry, rz, rw, tx, ty, tz;
87 std::string frame, child_frame;
88 long timestamp = new_start_msec + (doc[
"timestamp"].get_int64() - start_msec);
90 rx = rot[0].get_double();
91 ry = rot[1].get_double();
92 rz = rot[2].get_double();
93 rw = rot[3].get_double();
94 tx = trans[0].get_double();
95 ty = trans[1].get_double();
96 tz = trans[2].get_double();
97 frame = doc[
"frame"].get_utf8().value.to_string();
98 child_frame = doc[
"child_frame"].get_utf8().value.to_string();
100 tf::Quaternion q(rx, ry, rz, rw);
101 tf::assert_quaternion_valid(q);
102 tf::Transform t(q, tf::Vector3(tx, ty, tz));
117 cache_time_ = (double)(end_msec - start_msec) / 1000.;
119 if (new_start_msec == 0) {
120 new_start_msec = start_msec;
125 std::list<std::string> collections;
126 for (
auto c : mongodb_client_->database(database_).list_collections()) {
127 collections.push_back(c[
"name"].get_utf8().value.to_string());
130 std::list<std::string>::iterator c;
131 for (c = collections.begin(); c != collections.end(); ++c) {
132 if ((c->find(database_ +
".TransformInterface.") != 0) && (c->find(database_ +
".tf") != 0)) {
136 auto collection = mongodb_client_->database(database_)[*c];
137 using namespace bsoncxx::builder;
138 auto result = collection.find(
139 basic::make_document(
140 basic::kvp(
"timestamp",
141 [start_msec, end_msec](basic::sub_document subdoc) {
142 subdoc.append(basic::kvp(
"$gt",
static_cast<int64_t
>(start_msec)));
143 subdoc.append(basic::kvp(
"$lt",
static_cast<int64_t
>(end_msec)));
145 mongocxx::options::find().sort(basic::make_document(basic::kvp(
"timestamp", 1))));
147 for (
auto doc : result) {
148 if (doc.find(
"transforms") != doc.end()) {
149 bsoncxx::array::view transforms = doc[
"transforms"].get_array();
150 for (
auto el : transforms) {
151 restore_tf_doc(el.get_document().view(), start_msec, new_start_msec);
154 restore_tf_doc(doc, start_msec, new_start_msec);
A class for handling time.
long in_msec() const
Convert the stored time into milli-seconds.
float cache_time_
How long to cache transform history.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
Fawkes library namespace.