Fawkes API Fawkes Development Version
transform_computable.cpp
1/***************************************************************************
2 * transform_computable.cpp - Computable for doing transforms
3 *
4 * Created: 4:11:27 PM 2016
5 * Copyright 2016 Frederik Zwilling
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.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21#include "transform_computable.h"
22
23#include <bsoncxx/builder/basic/document.hpp>
24
25using namespace fawkes;
26using namespace mongocxx;
27using namespace bsoncxx;
28
29/** @class TransformComputable transform_computable.h
30 * Computable proving positions in other frames by using transforms
31 * @author Frederik Zwilling
32 */
33
34/** Constructor for Transform computable with objects of thread aspects.
35 * @param robot_memory Robot Memory
36 * @param tf Transform
37 * @param logger Logger
38 * @param config Configuration
39 */
42 fawkes::Logger * logger,
43 fawkes::Configuration * config)
44{
45 robot_memory_ = robot_memory;
46 tf_ = tf;
47 logger_ = logger;
48 config_ = config;
49
50 //register computable
51 using namespace bsoncxx::builder;
52 basic::document query;
53 query.append(basic::kvp("frame", [](basic::sub_document subdoc) {
54 subdoc.append(basic::kvp("$exists", true));
55 }));
56 query.append(basic::kvp("allow_tf", true));
57 std::vector<std::string> collections =
58 config->get_strings("plugins/robot-memory/computables/transform/collections");
59 int priority = config->get_int("plugins/robot-memory/computables/transform/priority");
60 float caching_time = config->get_float("plugins/robot-memory/computables/transform/caching-time");
61 for (std::string col : collections) {
62 computables.push_back(robot_memory_->register_computable(
63 query.extract(), col, &TransformComputable::compute_transform, this, caching_time, priority));
64 }
65}
66
67TransformComputable::~TransformComputable()
68{
69 for (Computable *comp : computables) {
70 robot_memory_->remove_computable(comp);
71 }
72}
73
74std::list<document::value>
75TransformComputable::compute_transform(const document::view &query, const std::string &collection)
76{
77 //get positions in other frames
78 using namespace bsoncxx::builder;
79 basic::document query_other_frames;
80 for (auto it = query.begin(); it != query.end(); it++) {
81 if (it->key() == "frame" || it->key() == "allow_tf") {
82 continue;
83 }
84 query_other_frames.append(basic::kvp(it->key(), it->get_value()));
85 }
86 query_other_frames.append(basic::kvp("frame", [](basic::sub_document subdoc) {
87 subdoc.append(basic::kvp("$exists", true));
88 }));
89 cursor cur = robot_memory_->query(query_other_frames, collection);
90
91 //transform them is possible
92 std::list<document::value> res;
93 std::string target_frame = query["frame"].get_utf8().value.to_string();
94 auto it = cur.begin();
95 while (it != cur.end()) {
96 document::view pos = *it;
97 if (pos.find("frame") != pos.end() && pos.find("translation") != pos.end()
98 && pos.find("rotation") != pos.end()) {
99 std::string src_frame = pos["frame"].get_utf8().value.to_string();
100 Time now(0, 0);
101 if (tf_->can_transform(target_frame.c_str(), src_frame.c_str(), now)) {
102 basic::document res_pos;
103 array::view src_trans = pos["translation"].get_array();
104 array::view src_rot = pos["rotation"].get_array();
105 fawkes::tf::Transform pose_tf(fawkes::tf::Quaternion(src_rot[0].get_double(),
106 src_rot[1].get_double(),
107 src_rot[2].get_double(),
108 src_rot[3].get_double()),
109 fawkes::tf::Vector3(src_trans[0].get_double(),
110 src_trans[1].get_double(),
111 src_trans[2].get_double()));
112 fawkes::tf::Stamped<fawkes::tf::Pose> src_stamped_pose(pose_tf,
113 Time(0, 0),
114 src_frame.c_str());
116 tf_->transform_pose(target_frame.c_str(), src_stamped_pose, res_stamped_pose);
117
118 for (auto it = pos.begin(); it != pos.end(); it++) {
119 if (!(it->key() == "frame" || it->key() == "translation" || it->key() == "rotation"
120 || it->key() == "_id")) {
121 res_pos.append(basic::kvp(it->key(), it->get_value()));
122 }
123 }
124 res_pos.append(basic::kvp("frame", target_frame));
125 res_pos.append(basic::kvp("allow_tf", true));
126 res_pos.append(basic::kvp("translation", [res_stamped_pose](basic::sub_array array) {
127 array.append(res_stamped_pose.getOrigin().x());
128 array.append(res_stamped_pose.getOrigin().y());
129 array.append(res_stamped_pose.getOrigin().z());
130 }));
131 res_pos.append(basic::kvp("rotation", [res_stamped_pose](basic::sub_array array) {
132 array.append(res_stamped_pose.getRotation().x());
133 array.append(res_stamped_pose.getRotation().y());
134 array.append(res_stamped_pose.getRotation().z());
135 array.append(res_stamped_pose.getRotation().w());
136 }));
137 res.push_back(res_pos.extract());
138 }
139 // else
140 // {
141 // logger_->log_info(name_, "Cant transform %s to %s", src_frame.c_str(), target_frame.c_str());
142 // }
143 }
144 }
145 return res;
146}
Class holding information for a single computable this class also enhances computed documents by addi...
Definition: computable.h:32
Access to the robot memory based on mongodb.
Definition: robot_memory.h:47
mongocxx::cursor query(bsoncxx::document::view query, const std::string &collection_name="", mongocxx::options::find query_options=mongocxx::options::find())
Query information from the robot memory.
Computable * register_computable(bsoncxx::document::value &&query_to_compute, const std::string &collection, std::list< bsoncxx::document::value >(T::*compute_func)(const bsoncxx::document::view &, const std::string &), T *obj, double caching_time=0.0, int priority=0)
Registers a Computable which provides information in the robot memory that is computed on demand.
Definition: robot_memory.h:158
void remove_computable(Computable *computable)
Remove previously registered computable.
TransformComputable(RobotMemory *robot_memory, fawkes::tf::Transformer *tf, fawkes::Logger *logger, fawkes::Configuration *config)
Constructor for Transform computable with objects of thread aspects.
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::vector< std::string > get_strings(const char *path)=0
Get list of values from configuration which is of type string.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Interface for logging.
Definition: logger.h:42
A class for handling time.
Definition: time.h:93
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
Fawkes library namespace.