Fawkes API Fawkes Development Version
openrave-robot-memory_thread.cpp
1
2/***************************************************************************
3 * openrave-robot-memory_thread.cpp - openrave-robot-memory
4 *
5 * Created: Thu Nov 24 13:14:33 2016
6 * Copyright 2016 Frederik Zwilling
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "openrave-robot-memory_thread.h"
23
24#include <algorithm>
25#include <bsoncxx/builder/basic/document.hpp>
26#include <bsoncxx/json.hpp>
27
28using namespace fawkes;
29using namespace bsoncxx;
30
31/** @class OpenraveRobotMemoryThread 'openrave-robot-memory_thread.h'
32 * Creates an OpenRave Scene for motion planning from data in the robot memory
33 * @author Frederik Zwilling
34 */
35
36/** Constructor. */
38: Thread("OpenraveRobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
40{
41}
42
43void
45{
47 config->get_string("plugins/openrave-robot-memory/openrave-if-name").c_str());
49 config->get_string("plugins/openrave-robot-memory/if-name").c_str());
50}
51
52void
54{
55 // process interface messages
56 while (!or_rm_if_->msgq_empty()) {
58 construct_scene();
59 } else {
60 logger->log_warn(name(), "Unknown message received");
61 }
62 or_rm_if_->msgq_pop();
63 }
64}
65
66void
68{
69}
70
71void
72OpenraveRobotMemoryThread::construct_scene()
73{
74 logger->log_info(name(), "Constructing Scene");
75
76 // add all object types by iterating over config paths
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()) {
80 //object_types->next() yields the whole path, so we have to get the right prefix
81 std::string cfg_name = std::string(object_types->path()).substr(prefix.length());
82 cfg_name = cfg_name.substr(0, cfg_name.find("/"));
83 //don't use the same prefix again
84 if (std::find(added_object_types_.begin(), added_object_types_.end(), cfg_name)
85 != added_object_types_.end())
86 continue;
87 added_object_types_.push_back(cfg_name);
88 logger->log_info(name(), "Adding object type: %s", cfg_name.c_str());
89 std::string cfg_prefix = prefix + cfg_name + "/";
90 std::string collection = config->get_string(cfg_prefix + "collection");
91
92 //construct query
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));
98 logger->log_info(name(), "Querying: %s", to_json(query).c_str());
99 auto cursor = robot_memory->query(query, collection);
100 //while (cur->more()) {
101 for (auto block : cursor) {
102 //logger->log_info(name(), "Adding: %s", cfg_prefix.c_str(), to_json(block).c_str());
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()) {
107 //add new object
108 logger->log_info(name(), "adding %s", block_name.c_str());
110 add_msg.set_name(block_name.c_str());
111 add_msg.set_path(config->get_string(cfg_prefix + "model-path").c_str());
112 openrave_if_->msgq_enqueue_copy(&add_msg);
113 added_objects_.push_back(block_name);
114 }
115 //move object to right position
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);
123 //rotate object
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);
132 }
133 }
134 added_object_types_.clear();
135 logger->log_info(name(), "Finished Constructing Scene");
136}
virtual void finalize()
Finalize the thread.
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.
Definition: blackboard.h:44
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.
Definition: configurable.h:41
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.
Definition: interface.h:351
unsigned int msgq_enqueue_copy(Message *message)
Enqueue copy of message at end of queue.
Definition: interface.cpp:953
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
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.
Definition: logging.h:41
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_name(const char *new_name)
Set name 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.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.