22 #include "openrave-robot-memory_thread.h" 27 using namespace mongo;
36 :
Thread(
"OpenraveRobotMemoryThread",
Thread::OPMODE_WAITFORWAKEUP),
45 config->
get_string(
"plugins/openrave-robot-memory/openrave-if-name").c_str());
70 OpenraveRobotMemoryThread::construct_scene()
75 std::string prefix =
"plugins/openrave-robot-memory/object-types/";
76 std::unique_ptr<Configuration::ValueIterator> object_types(
config->
search(prefix.c_str()));
77 while (object_types->next()) {
79 std::string cfg_name = std::string(object_types->path()).substr(prefix.length());
80 cfg_name = cfg_name.substr(0, cfg_name.find(
"/"));
82 if (std::find(added_object_types_.begin(), added_object_types_.end(), cfg_name)
83 != added_object_types_.end())
85 added_object_types_.push_back(cfg_name);
87 std::string cfg_prefix = prefix + cfg_name +
"/";
91 BSONObjBuilder query_builder;
92 query_builder.appendElements(fromjson(
config->
get_string(cfg_prefix +
"query")));
93 query_builder <<
"frame" 95 <<
"allow_tf" <<
true;
96 BSONObj query = query_builder.obj();
100 BSONObj block = cur->next();
102 std::string block_name = block.getStringField(
config->
get_string(cfg_prefix +
"name-key"));
103 if (std::find(added_objects_.begin(), added_objects_.end(), block_name)
104 == added_objects_.end()) {
108 add_msg.
set_name(block_name.c_str());
111 added_objects_.push_back(block_name);
115 move_msg.
set_name(block_name.c_str());
116 move_msg.
set_x(block.getField(
"translation").Array()[0].Double());
117 move_msg.
set_y(block.getField(
"translation").Array()[1].Double());
118 move_msg.
set_z(block.getField(
"translation").Array()[2].Double());
122 rotate_msg.
set_name(block_name.c_str());
123 rotate_msg.
set_x(block.getField(
"rotation").Array()[0].Double());
124 rotate_msg.
set_y(block.getField(
"rotation").Array()[1].Double());
125 rotate_msg.
set_z(block.getField(
"rotation").Array()[2].Double());
126 rotate_msg.
set_w(block.getField(
"rotation").Array()[3].Double());
130 added_object_types_.clear();
void set_path(const char *new_path)
Set path value.
QResCursor query(mongo::Query query, const std::string &collection="")
Query information from the robot memory.
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
void set_name(const char *new_name)
Set name value.
bool msgq_empty()
Check if queue is empty.
void set_x(const float new_x)
Set x value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
unsigned int msgq_enqueue_copy(Message *message)
Enqueue copy of message at end of queue.
Fawkes library namespace.
ConstructSceneMessage Fawkes BlackBoard Interface Message.
void set_y(const float new_y)
Set y value.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void finalize()
Finalize the thread.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
void set_y(const float new_y)
Set y value.
virtual void loop()
Code to execute in the thread.
OpenraveRobotMemoryThread()
Constructor.
const char * name() const
Get name of thread.
void set_z(const float new_z)
Set z value.
bool msgq_first_is()
Check if first message has desired type.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
void set_x(const float new_x)
Set x value.
void set_z(const float new_z)
Set z value.
void set_name(const char *new_name)
Set name value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
OpenraveRobotMemoryInterface Fawkes BlackBoard Interface.
void set_name(const char *new_name)
Set name value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void set_w(const float new_w)
Set w value.
MoveObjectMessage Fawkes BlackBoard Interface Message.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
OpenRaveInterface Fawkes BlackBoard Interface.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
AddObjectMessage Fawkes BlackBoard Interface Message.