22 #include "mongodb_tf_transformer.h" 26 #ifdef HAVE_MONGODB_VERSION_H 28 # define QUERY MONGO_QUERY 31 using namespace mongo;
46 MongoDBTransformer::MongoDBTransformer(mongo::DBClientBase *mongodb_client,
47 const std::string & database_name,
49 : mongodb_client_(mongodb_client), database_(database_name)
52 #ifdef HAVE_MONGODB_VERSION_H 54 mongodb_client_->createIndex(database_ +
".tf", mongo::fromjson(
"{timestamp:1}"));
55 mongodb_client_->createIndex(database_ +
".TransformInterface",
56 mongo::fromjson(
"{timestamp:1}"));
58 mongodb_client_->ensureIndex(database_ +
".tf", mongo::fromjson(
"{timestamp:1}"));
59 mongodb_client_->ensureIndex(database_ +
".TransformInterface",
60 mongo::fromjson(
"{timestamp:1}"));
84 MongoDBTransformer::restore_tf_doc(BSONObj &doc,
long long start_msec,
long long new_start_msec)
86 std::vector<BSONElement> trans = doc[
"translation"].Array();
87 std::vector<BSONElement> rot = doc[
"rotation"].Array();
88 double rx, ry, rz, rw, tx, ty, tz;
89 std::string frame, child_frame;
90 long timestamp = new_start_msec + (doc[
"timestamp"].Long() - start_msec);
96 tx = trans[0].Double();
97 ty = trans[1].Double();
98 tz = trans[2].Double();
99 frame = doc[
"frame"].String();
100 child_frame = doc[
"child_frame"].String();
102 tf::Quaternion q(rx, ry, rz, rw);
103 tf::assert_quaternion_valid(q);
104 tf::Transform t(q, tf::Vector3(tx, ty, tz));
119 cache_time_ = (double)(end_msec - start_msec) / 1000.;
121 if (new_start_msec == 0) {
122 new_start_msec = start_msec;
125 std::list<std::string> collections = mongodb_client_->getCollectionNames(database_);
127 #if __cplusplus >= 201103L 128 std::unique_ptr<DBClientCursor> cursor;
130 std::auto_ptr<DBClientCursor> cursor;
133 std::list<std::string>::iterator c;
134 for (c = collections.begin(); c != collections.end(); ++c) {
135 if ((c->find(database_ +
".TransformInterface.") != 0) && (c->find(database_ +
".tf") != 0)) {
139 cursor = mongodb_client_->query(
140 *c, QUERY(
"timestamp" << GTE << start_msec << LT << end_msec).sort(
"timestamp"));
142 while (cursor->more()) {
143 doc = cursor->next();
144 if (doc.hasField(
"transforms")) {
146 BSONObj::iterator i = doc.getObjectField(
"transforms").begin();
148 BSONElement e = i.next();
150 restore_tf_doc(o, start_msec, new_start_msec);
153 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.