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
32using namespace mongocxx;
33
34namespace fawkes {
35namespace 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 */
47MongoDBTransformer::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 */
73void
75{
76 restore(start.in_msec(), end.in_msec(), new_start.in_msec());
77}
78
79void
80MongoDBTransformer::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 */
114void
115MongoDBTransformer::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
A class for handling time.
Definition: time.h:93
long in_msec() const
Convert the stored time into milli-seconds.
Definition: time.cpp:228
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:187
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
MongoDBTransformer(mongocxx::client *mongodb_client, const std::string &database_name, bool ensure_index=true)
Constructor.
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Fawkes library namespace.