Fawkes API  Fawkes Development Version
mongodb_tf_transformer.cpp
1 /***************************************************************************
2  * mongodb_tf_transformer.cpp - Read and provide TFs from MongoDB
3  *
4  * Created: Thu Nov 29 22:59:49 2012
5  * Copyright 2012 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version. A runtime exception applies to
12  * this software (see LICENSE.GPL_WRE file mentioned below for details).
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
20  */
21 
22 #include "mongodb_tf_transformer.h"
23 
24 #include <list>
25 
26 #ifdef HAVE_MONGODB_VERSION_H
27 // we are using mongo-cxx-driver which renamed QUERY to MONGO_QUERY
28 # define QUERY MONGO_QUERY
29 #endif
30 
31 using namespace mongo;
32 
33 namespace fawkes {
34 namespace tf {
35 
36 /** @class MongoDBTransformer "mongodb_tf_transformer.h"
37  * Read transforms from MongoDB and answer queries.
38  * @author Tim Niemueller
39  */
40 
41 /** Constructor.
42  * @param mongodb_client MongoDB database client
43  * @param database_name name of database to restore transforms from
44  * @param ensure_index if true ensures that the required index on timestamps exists
45  */
46 MongoDBTransformer::MongoDBTransformer(mongo::DBClientBase *mongodb_client,
47  const std::string & database_name,
48  bool ensure_index)
49 : mongodb_client_(mongodb_client), database_(database_name)
50 {
51  if (ensure_index) {
52 #ifdef HAVE_MONGODB_VERSION_H
53  // mongodb-cxx-driver dropped ensureIndex and names it createIndex
54  mongodb_client_->createIndex(database_ + ".tf", mongo::fromjson("{timestamp:1}"));
55  mongodb_client_->createIndex(database_ + ".TransformInterface",
56  mongo::fromjson("{timestamp:1}"));
57 #else
58  mongodb_client_->ensureIndex(database_ + ".tf", mongo::fromjson("{timestamp:1}"));
59  mongodb_client_->ensureIndex(database_ + ".TransformInterface",
60  mongo::fromjson("{timestamp:1}"));
61 #endif
62  }
63 }
64 
65 /** Destructor. */
67 {
68 }
69 
70 /** Restore transforms from database.
71  * @param start start time of range to restore
72  * @param end end time of range to restore
73  * @param new_start the new start time to which the transform times
74  * will be reset, i.e. from the transforms time stamp the @p start
75  * time is subtracted and @p new_start is added.
76  */
77 void
79 {
80  restore(start.in_msec(), end.in_msec(), new_start.in_msec());
81 }
82 
83 void
84 MongoDBTransformer::restore_tf_doc(BSONObj &doc, long long start_msec, long long new_start_msec)
85 {
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);
91  Time time(timestamp);
92  rx = rot[0].Double();
93  ry = rot[1].Double();
94  rz = rot[2].Double();
95  rw = rot[3].Double();
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();
101 
102  tf::Quaternion q(rx, ry, rz, rw);
103  tf::assert_quaternion_valid(q);
104  tf::Transform t(q, tf::Vector3(tx, ty, tz));
105  tf::StampedTransform transform(t, time, frame, child_frame);
106  set_transform(transform, "MongoDBTransformer");
107 }
108 
109 /** Restore transforms from database.
110  * @param start_msec start time of range to restore since the epoch in msec
111  * @param end_msec end time of range to restore since the epoch in msec
112  * @param new_start_msec the new start time since the epoch in msec to which the
113  * transform times will be reset, i.e. from the transforms time stamp the
114  * @p start time is subtracted and @p new_start is added.
115  */
116 void
117 MongoDBTransformer::restore(long long start_msec, long long end_msec, long long new_start_msec)
118 {
119  cache_time_ = (double)(end_msec - start_msec) / 1000.;
120 
121  if (new_start_msec == 0) {
122  new_start_msec = start_msec;
123  }
124 
125  std::list<std::string> collections = mongodb_client_->getCollectionNames(database_);
126 
127 #if __cplusplus >= 201103L
128  std::unique_ptr<DBClientCursor> cursor;
129 #else
130  std::auto_ptr<DBClientCursor> cursor;
131 #endif
132  BSONObj doc;
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)) {
136  continue;
137  }
138 
139  cursor = mongodb_client_->query(
140  *c, QUERY("timestamp" << GTE << start_msec << LT << end_msec).sort("timestamp"));
141 
142  while (cursor->more()) {
143  doc = cursor->next();
144  if (doc.hasField("transforms")) {
145  // multi transforms document
146  BSONObj::iterator i = doc.getObjectField("transforms").begin();
147  while (i.more()) {
148  BSONElement e = i.next();
149  BSONObj o = e.Obj();
150  restore_tf_doc(o, start_msec, new_start_msec);
151  }
152  } else {
153  restore_tf_doc(doc, start_msec, new_start_msec);
154  }
155  }
156  }
157 }
158 
159 } // end namespace tf
160 } // end namespace fawkes
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.
Definition: time.h:92
long in_msec() const
Convert the stored time into milli-seconds.
Definition: time.cpp:228
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:187
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.