22 #include "mongodb_tf_transformer.h" 26 #ifdef HAVE_MONGODB_VERSION_H 28 # define QUERY MONGO_QUERY 31 using namespace mongo;
50 MongoDBTransformer::MongoDBTransformer(mongo::DBClientBase *mongodb_client,
51 std::string database_name,
bool ensure_index)
52 : mongodb_client_(mongodb_client), database_(database_name)
55 #ifdef HAVE_MONGODB_VERSION_H 57 mongodb_client_->createIndex(database_ +
".tf", mongo::fromjson(
"{timestamp:1}"));
58 mongodb_client_->createIndex(database_ +
".TransformInterface",
59 mongo::fromjson(
"{timestamp:1}"));
61 mongodb_client_->ensureIndex(database_ +
".tf", mongo::fromjson(
"{timestamp:1}"));
62 mongodb_client_->ensureIndex(database_ +
".TransformInterface",
63 mongo::fromjson(
"{timestamp:1}"));
89 MongoDBTransformer::restore_tf_doc(BSONObj &doc,
90 long long start_msec,
long long new_start_msec)
92 std::vector<BSONElement> trans = doc[
"translation"].Array();
93 std::vector<BSONElement> rot = doc[
"rotation"].Array();
94 double rx, ry, rz, rw, tx, ty, tz;
95 std::string frame, child_frame;
96 long timestamp = new_start_msec + (doc[
"timestamp"].Long() - start_msec);
100 rz = rot[2].Double();
101 rw = rot[3].Double();
102 tx = trans[0].Double();
103 ty = trans[1].Double();
104 tz = trans[2].Double();
105 frame = doc[
"frame"].String();
106 child_frame = doc[
"child_frame"].String();
108 tf::Quaternion q(rx, ry, rz, rw);
109 tf::assert_quaternion_valid(q);
110 tf::Transform t(q, tf::Vector3(tx, ty, tz));
126 cache_time_ = (double)(end_msec - start_msec) / 1000.;
128 if (new_start_msec == 0) {
129 new_start_msec = start_msec;
132 std::list<std::string> collections =
133 mongodb_client_->getCollectionNames(database_);
135 #if __cplusplus >= 201103L 136 std::unique_ptr<DBClientCursor> cursor;
138 std::auto_ptr<DBClientCursor> cursor;
141 std::list<std::string>::iterator c;
142 for (c = collections.begin(); c != collections.end(); ++c) {
143 if ((c->find(database_ +
".TransformInterface.") != 0 ) &&
144 (c->find(database_ +
".tf") != 0) )
149 cursor = mongodb_client_->query(*c,
150 QUERY(
"timestamp" << GTE << start_msec << LT << end_msec).sort(
"timestamp"));
152 while (cursor->more()) {
153 doc = cursor->next();
154 if (doc.hasField(
"transforms")) {
156 BSONObj::iterator i = doc.getObjectField(
"transforms").begin();
158 BSONElement e = i.next();
160 restore_tf_doc(o, start_msec, new_start_msec);
163 restore_tf_doc(doc, start_msec, new_start_msec);
Fawkes library namespace.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
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.