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 
25 using namespace fawkes;
26 using namespace mongocxx;
27 using 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 
67 TransformComputable::~TransformComputable()
68 {
69  for (Computable *comp : computables) {
70  robot_memory_->remove_computable(comp);
71  }
72 }
73 
74 std::list<document::value>
75 TransformComputable::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());
115  fawkes::tf::Stamped<fawkes::tf::Pose> res_stamped_pose;
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 }
RobotMemory
Access to the robot memory based on mongodb.
Definition: robot_memory.h:47
fawkes::tf::Transformer
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
fawkes::tf::Stamped< fawkes::tf::Pose >
fawkes::Configuration
Interface for configuration handling.
Definition: config.h:65
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes
Fawkes library namespace.
fawkes::Configuration::get_strings
virtual std::vector< std::string > get_strings(const char *path)=0
Get list of values from configuration which is of type string.
fawkes::Time
A class for handling time.
Definition: time.h:93
TransformComputable::TransformComputable
TransformComputable(RobotMemory *robot_memory, fawkes::tf::Transformer *tf, fawkes::Logger *logger, fawkes::Configuration *config)
Constructor for Transform computable with objects of thread aspects.
Definition: transform_computable.cpp:40
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Computable
Class holding information for a single computable this class also enhances computed documents by addi...
Definition: computable.h:32