22#include "openrave-robot-memory_thread.h"
25#include <bsoncxx/builder/basic/document.hpp>
26#include <bsoncxx/json.hpp>
29using namespace bsoncxx;
38:
Thread(
"OpenraveRobotMemoryThread",
Thread::OPMODE_WAITFORWAKEUP),
47 config->
get_string(
"plugins/openrave-robot-memory/openrave-if-name").c_str());
72OpenraveRobotMemoryThread::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());
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());
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());
134 added_object_types_.clear();
virtual void finalize()
Finalize the thread.
OpenraveRobotMemoryThread()
Constructor.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
mongocxx::cursor query(bsoncxx::document::view query, const std::string &collection_name="", mongocxx::options::find query_options=mongocxx::options::find())
Query information from the robot memory.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
bool msgq_first_is()
Check if first message has desired type.
unsigned int msgq_enqueue_copy(Message *message)
Enqueue copy of message at end of queue.
void msgq_pop()
Erase first message from queue.
bool msgq_empty()
Check if queue is empty.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
AddObjectMessage Fawkes BlackBoard Interface Message.
void set_path(const char *new_path)
Set path value.
void set_name(const char *new_name)
Set name value.
MoveObjectMessage Fawkes BlackBoard Interface Message.
void set_z(const float new_z)
Set z value.
void set_x(const float new_x)
Set x value.
void set_name(const char *new_name)
Set name value.
void set_y(const float new_y)
Set y value.
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
void set_y(const float new_y)
Set y value.
void set_w(const float new_w)
Set w value.
void set_name(const char *new_name)
Set name value.
void set_x(const float new_x)
Set x value.
void set_z(const float new_z)
Set z value.
OpenRaveInterface Fawkes BlackBoard Interface.
ConstructSceneMessage Fawkes BlackBoard Interface Message.
OpenraveRobotMemoryInterface Fawkes BlackBoard Interface.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.