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 <bsoncxx/builder/basic/document.hpp>
25 #include <list>
26 
27 #ifdef HAVE_MONGODB_VERSION_H
28 // we are using mongo-cxx-driver which renamed QUERY to MONGO_QUERY
29 # define QUERY MONGO_QUERY
30 #endif
31 
32 using namespace mongocxx;
33 
34 namespace fawkes {
35 namespace tf {
36 
37 /** @class MongoDBTransformer "mongodb_tf_transformer.h"
38  * Read transforms from MongoDB and answer queries.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor.
43  * @param mongodb_client MongoDB database client
44  * @param database_name name of database to restore transforms from
45  * @param ensure_index if true ensures that the required index on timestamps exists
46  */
47 MongoDBTransformer::MongoDBTransformer(mongocxx::client * mongodb_client,
48  const std::string &database_name,
49  bool ensure_index)
50 : mongodb_client_(mongodb_client), database_(database_name)
51 {
52  if (ensure_index) {
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)));
58  }
59 }
60 
61 /** Destructor. */
63 {
64 }
65 
66 /** Restore transforms from database.
67  * @param start start time of range to restore
68  * @param end end time of range to restore
69  * @param new_start the new start time to which the transform times
70  * will be reset, i.e. from the transforms time stamp the @p start
71  * time is subtracted and @p new_start is added.
72  */
73 void
75 {
76  restore(start.in_msec(), end.in_msec(), new_start.in_msec());
77 }
78 
79 void
80 MongoDBTransformer::restore_tf_doc(const bsoncxx::document::view &doc,
81  long long start_msec,
82  long long new_start_msec)
83 {
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);
89  Time time(timestamp);
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();
99 
100  tf::Quaternion q(rx, ry, rz, rw);
101  tf::assert_quaternion_valid(q);
102  tf::Transform t(q, tf::Vector3(tx, ty, tz));
103  tf::StampedTransform transform(t, time, frame, child_frame);
104  set_transform(transform, "MongoDBTransformer");
105 }
106 
107 /** Restore transforms from database.
108  * @param start_msec start time of range to restore since the epoch in msec
109  * @param end_msec end time of range to restore since the epoch in msec
110  * @param new_start_msec the new start time since the epoch in msec to which the
111  * transform times will be reset, i.e. from the transforms time stamp the
112  * @p start time is subtracted and @p new_start is added.
113  */
114 void
115 MongoDBTransformer::restore(long start_msec, long end_msec, long new_start_msec)
116 {
117  cache_time_ = (double)(end_msec - start_msec) / 1000.;
118 
119  if (new_start_msec == 0) {
120  new_start_msec = start_msec;
121  }
122 
123  // requires mongo-cxx-driver 3.4.0
124  //std::list<std::string> collections = mongodb_client_->database(database_).list_collection_names();
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());
128  }
129 
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)) {
133  continue;
134  }
135 
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)));
144  })),
145  mongocxx::options::find().sort(basic::make_document(basic::kvp("timestamp", 1))));
146 
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);
152  }
153  } else {
154  restore_tf_doc(doc, start_msec, new_start_msec);
155  }
156  }
157  }
158 }
159 
160 } // end namespace tf
161 } // end namespace fawkes
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
fawkes::Time::in_msec
long in_msec() const
Convert the stored time into milli-seconds.
Definition: time.cpp:233
fawkes::tf::MongoDBTransformer::restore
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
Definition: mongodb_tf_transformer.h:59
fawkes
fawkes::tf::MongoDBTransformer::~MongoDBTransformer
virtual ~MongoDBTransformer()
Destructor.
Definition: mongodb_tf_transformer.cpp:62
fawkes::tf::BufferCore::cache_time_
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:194
fawkes::Time
Definition: time.h:96
fawkes::tf::BufferCore::set_transform
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
Definition: buffer_core.cpp:170