22 #include "openrave-robot-memory_thread.h"
25 #include <bsoncxx/builder/basic/document.hpp>
26 #include <bsoncxx/json.hpp>
29 using namespace bsoncxx;
38 :
Thread(
"OpenraveRobotMemoryThread",
Thread::OPMODE_WAITFORWAKEUP),
47 config->
get_string(
"plugins/openrave-robot-memory/openrave-if-name").c_str());
72 OpenraveRobotMemoryThread::construct_scene()
77 std::string prefix =
"plugins/openrave-robot-memory/object-types/";
78 std::unique_ptr<Configuration::ValueIterator> object_types(
config->
search(prefix.c_str()));
79 while (object_types->next()) {
81 std::string cfg_name = std::string(object_types->path()).substr(prefix.length());
82 cfg_name = cfg_name.substr(0, cfg_name.find(
"/"));
84 if (std::find(added_object_types_.begin(), added_object_types_.end(), cfg_name)
85 != added_object_types_.end())
87 added_object_types_.push_back(cfg_name);
89 std::string cfg_prefix = prefix + cfg_name +
"/";
93 using namespace bsoncxx::builder;
94 basic::document query;
95 query.append(builder::concatenate(from_json(
config->
get_string(cfg_prefix +
"query"))));
96 query.append(basic::kvp(
"frame",
"base_link"));
97 query.append(basic::kvp(
"allow_tf",
true));
101 for (
auto block : cursor) {
103 std::string block_name =
104 block[
config->
get_string(cfg_prefix +
"name-key")].get_utf8().value.to_string();
105 if (std::find(added_objects_.begin(), added_objects_.end(), block_name)
106 == added_objects_.end()) {
110 add_msg.
set_name(block_name.c_str());
112 openrave_if_->msgq_enqueue_copy(&add_msg);
113 added_objects_.push_back(block_name);
117 move_msg.
set_name(block_name.c_str());
118 array::view translation = block[
"translation"].get_array();
119 move_msg.
set_x(translation[0].get_double());
120 move_msg.
set_y(translation[1].get_double());
121 move_msg.
set_z(translation[2].get_double());
122 openrave_if_->msgq_enqueue_copy(&move_msg);
125 rotate_msg.
set_name(block_name.c_str());
126 array::view rotation = block[
"rotation"].get_array();
127 rotate_msg.
set_x(rotation[0].get_double());
128 rotate_msg.
set_y(rotation[1].get_double());
129 rotate_msg.
set_z(rotation[2].get_double());
130 rotate_msg.
set_w(rotation[3].get_double());
131 openrave_if_->msgq_enqueue_copy(&rotate_msg);
134 added_object_types_.clear();