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 #if 0 /* just to make Emacs auto-indent happy */
36  }
37 }
38 #endif
39 
40 /** @class MongoDBTransformer "mongodb_tf_transformer.h"
41  * Read transforms from MongoDB and answer queries.
42  * @author Tim Niemueller
43  */
44 
45 /** Constructor.
46  * @param mongodb_client MongoDB database client
47  * @param database_name name of database to restore transforms from
48  * @param ensure_index if true ensures that the required index on timestamps exists
49  */
50 MongoDBTransformer::MongoDBTransformer(mongo::DBClientBase *mongodb_client,
51  std::string database_name, bool ensure_index)
52  : mongodb_client_(mongodb_client), database_(database_name)
53 {
54  if (ensure_index) {
55 #ifdef HAVE_MONGODB_VERSION_H
56  // mongodb-cxx-driver dropped ensureIndex and names it createIndex
57  mongodb_client_->createIndex(database_ + ".tf", mongo::fromjson("{timestamp:1}"));
58  mongodb_client_->createIndex(database_ + ".TransformInterface",
59  mongo::fromjson("{timestamp:1}"));
60 #else
61  mongodb_client_->ensureIndex(database_ + ".tf", mongo::fromjson("{timestamp:1}"));
62  mongodb_client_->ensureIndex(database_ + ".TransformInterface",
63  mongo::fromjson("{timestamp:1}"));
64 #endif
65  }
66 }
67 
68 
69 /** Destructor. */
71 {
72 }
73 
74 /** Restore transforms from database.
75  * @param start start time of range to restore
76  * @param end end time of range to restore
77  * @param new_start the new start time to which the transform times
78  * will be reset, i.e. from the transforms time stamp the @p start
79  * time is subtracted and @p new_start is added.
80  */
81 void
83 {
84  restore(start.in_msec(), end.in_msec(), new_start.in_msec());
85 }
86 
87 
88 void
89 MongoDBTransformer::restore_tf_doc(BSONObj &doc,
90  long long start_msec, long long new_start_msec)
91 {
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);
97  Time time(timestamp);
98  rx = rot[0].Double();
99  ry = rot[1].Double();
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();
107 
108  tf::Quaternion q(rx, ry, rz, rw);
109  tf::assert_quaternion_valid(q);
110  tf::Transform t(q, tf::Vector3(tx, ty, tz));
111  tf::StampedTransform transform(t, time, frame, child_frame);
112  set_transform(transform, "MongoDBTransformer");
113 }
114 
115 
116 /** Restore transforms from database.
117  * @param start_msec start time of range to restore since the epoch in msec
118  * @param end_msec end time of range to restore since the epoch in msec
119  * @param new_start_msec the new start time since the epoch in msec to which the
120  * transform times will be reset, i.e. from the transforms time stamp the
121  * @p start time is subtracted and @p new_start is added.
122  */
123 void
124 MongoDBTransformer::restore(long long start_msec, long long end_msec, long long new_start_msec)
125 {
126  cache_time_ = (double)(end_msec - start_msec) / 1000.;
127 
128  if (new_start_msec == 0) {
129  new_start_msec = start_msec;
130  }
131 
132  std::list<std::string> collections =
133  mongodb_client_->getCollectionNames(database_);
134 
135 #if __cplusplus >= 201103L
136  std::unique_ptr<DBClientCursor> cursor;
137 #else
138  std::auto_ptr<DBClientCursor> cursor;
139 #endif
140  BSONObj doc;
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) )
145  {
146  continue;
147  }
148 
149  cursor = mongodb_client_->query(*c,
150  QUERY("timestamp" << GTE << start_msec << LT << end_msec).sort("timestamp"));
151 
152  while (cursor->more()) {
153  doc = cursor->next();
154  if (doc.hasField("transforms")) {
155  // multi transforms document
156  BSONObj::iterator i = doc.getObjectField("transforms").begin();
157  while (i.more()) {
158  BSONElement e = i.next();
159  BSONObj o = e.Obj();
160  restore_tf_doc(o, start_msec, new_start_msec);
161  }
162  } else {
163  restore_tf_doc(doc, start_msec, new_start_msec);
164  }
165  }
166  }
167 }
168 
169 } // end namespace tf
170 } // end namespace fawkes
Fawkes library namespace.
Definition: mongodb.h:29
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:91
long in_msec() const
Convert the stored time into milli-seconds.
Definition: time.cpp:242
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
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.