21 #include "transform_computable.h" 24 using namespace mongo;
42 robot_memory_ = robot_memory;
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));
59 TransformComputable::~TransformComputable()
62 robot_memory_->remove_computable(comp);
66 std::list<mongo::BSONObj>
67 TransformComputable::compute_transform(
const mongo::BSONObj &query,
const std::string &collection)
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);
76 std::list<mongo::BSONObj> res;
77 std::string target_frame = query.getField(
"frame").String();
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();
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()));
95 tf_->transform_pose(target_frame.c_str(), src_stamped_pose, res_stamped_pose);
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());
Fawkes library namespace.
Class holding information for a single computable this class also enhances computed documents by addi...
A class for handling time.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
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.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.