21 #include "transform_computable.h"
23 #include <bsoncxx/builder/basic/document.hpp>
26 using namespace mongocxx;
27 using namespace bsoncxx;
45 robot_memory_ = robot_memory;
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));
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));
67 TransformComputable::~TransformComputable()
70 robot_memory_->remove_computable(comp);
74 std::list<document::value>
75 TransformComputable::compute_transform(
const document::view &query,
const std::string &collection)
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") {
84 query_other_frames.append(basic::kvp(it->key(), it->get_value()));
86 query_other_frames.append(basic::kvp(
"frame", [](basic::sub_document subdoc) {
87 subdoc.append(basic::kvp(
"$exists",
true));
89 cursor cur = robot_memory_->query(query_other_frames, collection);
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();
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()));
116 tf_->transform_pose(target_frame.c_str(), src_stamped_pose, res_stamped_pose);
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()));
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());
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());
137 res.push_back(res_pos.extract());
Class holding information for a single computable this class also enhances computed documents by addi...
Access to the robot memory based on mongodb.
Interface for configuration handling.
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.
A class for handling time.
Fawkes library namespace.