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 using namespace fawkes;
24 using namespace mongo;
25 
26 /** @class TransformComputable transform_computable.h
27  * Computable proving positions in other frames by using transforms
28  * @author Frederik Zwilling
29  */
30 
31 /** Constructor for Transform computable with objects of thread aspects.
32  * @param robot_memory Robot Memory
33  * @param tf Transform
34  * @param logger Logger
35  * @param config Configuration
36  */
39  fawkes::Logger * logger,
40  fawkes::Configuration * config)
41 {
42  robot_memory_ = robot_memory;
43  tf_ = tf;
44  logger_ = logger;
45  config_ = config;
46 
47  //register computable
48  Query query = fromjson("{frame:{$exists:true},allow_tf:true}");
49  std::vector<std::string> collections =
50  config->get_strings("plugins/robot-memory/computables/transform/collections");
51  int priority = config->get_int("plugins/robot-memory/computables/transform/priority");
52  float caching_time = config->get_float("plugins/robot-memory/computables/transform/caching-time");
53  for (std::string col : collections) {
54  computables.push_back(robot_memory_->register_computable(
55  query, col, &TransformComputable::compute_transform, this, caching_time, priority));
56  }
57 }
58 
59 TransformComputable::~TransformComputable()
60 {
61  for (Computable *comp : computables) {
62  robot_memory_->remove_computable(comp);
63  }
64 }
65 
66 std::list<mongo::BSONObj>
67 TransformComputable::compute_transform(const mongo::BSONObj &query, const std::string &collection)
68 {
69  //get positions in other frames
70  BSONObjBuilder query_other_frames;
71  query_other_frames.appendElements(query.removeField("frame").removeField("allow_tf"));
72  query_other_frames.append("frame", fromjson("{$exists:true}"));
73  QResCursor cur = robot_memory_->query(query_other_frames.obj(), collection);
74 
75  //transform them is possible
76  std::list<mongo::BSONObj> res;
77  std::string target_frame = query.getField("frame").String();
78  while (cur->more()) {
79  BSONObj pos = cur->next();
80  if (pos.hasField("frame") && pos.hasField("translation") && pos.hasField("rotation")) {
81  std::string src_frame = pos.getField("frame").String();
82  Time now(0, 0);
83  if (tf_->can_transform(target_frame.c_str(), src_frame.c_str(), now)) {
84  BSONObjBuilder res_pos;
85  std::vector<BSONElement> src_trans = pos.getField("translation").Array();
86  std::vector<BSONElement> src_rot = pos.getField("rotation").Array();
87  fawkes::tf::Transform pose_tf(
88  fawkes::tf::Quaternion(
89  src_rot[0].Double(), src_rot[1].Double(), src_rot[2].Double(), src_rot[3].Double()),
90  fawkes::tf::Vector3(src_trans[0].Double(), src_trans[1].Double(), src_trans[2].Double()));
91  fawkes::tf::Stamped<fawkes::tf::Pose> src_stamped_pose(pose_tf,
92  Time(0, 0),
93  src_frame.c_str());
95  tf_->transform_pose(target_frame.c_str(), src_stamped_pose, res_stamped_pose);
96 
97  res_pos.appendElements(pos.removeField("frame")
98  .removeField("translation")
99  .removeField("rotation")
100  .removeField("_id"));
101  res_pos.append("frame", target_frame);
102  res_pos.append("allow_tf", true);
103  BSONArrayBuilder arrb_trans;
104  arrb_trans.append(res_stamped_pose.getOrigin().x());
105  arrb_trans.append(res_stamped_pose.getOrigin().y());
106  arrb_trans.append(res_stamped_pose.getOrigin().z());
107  res_pos.append("translation", arrb_trans.arr());
108  BSONArrayBuilder arrb_rot;
109  arrb_rot.append(res_stamped_pose.getRotation().x());
110  arrb_rot.append(res_stamped_pose.getRotation().y());
111  arrb_rot.append(res_stamped_pose.getRotation().z());
112  arrb_rot.append(res_stamped_pose.getRotation().w());
113  res_pos.append("rotation", arrb_rot.arr());
114  res.push_back(res_pos.obj());
115  }
116  // else
117  // {
118  // logger_->log_info(name_, "Cant transform %s to %s", src_frame.c_str(), target_frame.c_str());
119  // }
120  }
121  }
122  return res;
123 }
Fawkes library namespace.
Class holding information for a single computable this class also enhances computed documents by addi...
Definition: computable.h:29
A class for handling time.
Definition: time.h:92
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
TransformComputable(RobotMemory *robot_memory, fawkes::tf::Transformer *tf, fawkes::Logger *logger, fawkes::Configuration *config)
Constructor for Transform computable with objects of thread aspects.
virtual std::vector< std::string > get_strings(const char *path)=0
Get list of values from configuration which is of type string.
Access to the robot memory based on mongodb.
Definition: robot_memory.h:48
Coordinate transforms between any two frames in a system.
Definition: transformer.h:64
Interface for configuration handling.
Definition: config.h:64
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Interface for logging.
Definition: logger.h:41