Fawkes API
Fawkes Development Version
|
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();
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
bool msgq_empty()
Check if queue is empty.
void set_x(const float new_x)
Set x value.
virtual void init()
Initialize the thread.
void set_y(const float new_y)
Set y value.
ConstructSceneMessage Fawkes BlackBoard Interface Message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void finalize()
Finalize the thread.
OpenraveRobotMemoryThread()
Constructor.
Thread aspect to use blocked timing.
MoveObjectMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
void set_path(const char *new_path)
Set path value.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
void set_name(const char *new_name)
Set name value.
void set_name(const char *new_name)
Set name value.
Logger * logger
This is the Logger member used to access the logger.
void set_name(const char *new_name)
Set name value.
Fawkes library namespace.
virtual void loop()
Code to execute in the thread.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_z(const float new_z)
Set z value.
Configuration * config
This is the Configuration member used to access the configuration.
OpenRaveInterface Fawkes BlackBoard Interface.
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
Thread class encapsulation of pthreads.
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 std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
void set_z(const float new_z)
Set z value.
void set_y(const float new_y)
Set y value.
AddObjectMessage Fawkes BlackBoard Interface Message.
void set_x(const float new_x)
Set x value.
void set_w(const float new_w)
Set w value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
OpenraveRobotMemoryInterface Fawkes BlackBoard Interface.