Fawkes API Fawkes Development Version
pddl_robot_memory_thread.cpp
1
2/***************************************************************************
3 * pddl_robot_memory_thread.cpp - pddl_robot_memory
4 *
5 * Plugin created: Thu Oct 13 13:34:05 2016
6
7 * Copyright 2016 Frederik Zwilling
8 *
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24#include "pddl_robot_memory_thread.h"
25
26#include <utils/misc/string_conversions.h>
27
28#include <bsoncxx/exception/exception.hpp>
29#include <fstream>
30
31using namespace fawkes;
32using namespace mongocxx;
33using namespace bsoncxx;
34using namespace bsoncxx::builder;
35
36/** @class PddlRobotMemoryThread 'pddl_robot_memory_thread.h'
37 * Generate PDDL files from the robot memory
38 *
39 * This plugin uses the template engine ctemplate to generate a pddl
40 * from a template file and the robot memory.
41 *
42 * The template file can use the following templates to generate some output
43 * for each document returned by a query:
44 *
45 * Example: \c "<<#ONTABLE|{relation:'on-table'}>> (on-table <<object>>) <</ONTABLE>>"
46 * Yields: (on-table a) (on-table b)
47 * When these documents are in the database:
48 * {relation:'on-table', object:'a'}, {relation:'on-table', object:'b'}
49 *
50 * The selection template \c "<<#UNIQUENAME|query>> something <</UNIQUENAME>>"
51 * queries the robot memory and inserts 'something' for each returned document.
52 *
53 * Variable templates \c "<<key>>" inside the selection are substituted by the values
54 * of that key in the document returned by the query. You can also access subdocuments
55 * and arrays as follows:
56 * (e.g. \c "<<position_translation_0>>" for a document {position:{translation:[0,1,2], orientation:[0,1,2]}})
57 *
58 * @author Frederik Zwilling
59 */
60
61PddlRobotMemoryThread::PddlRobotMemoryThread()
62: Thread("PddlRobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
63 BlackBoardInterfaceListener("PddlRobotMemoryThread")
64{
65}
66
67void
69{
70 //read config values
71 collection = config->get_string("plugins/pddl-robot-memory/collection");
72 input_path = StringConversions::resolve_path(
73 "@BASEDIR@/src/" + config->get_string("plugins/pddl-robot-memory/input-problem-description"));
74 output_path = StringConversions::resolve_path(
75 "@BASEDIR@/src/" + config->get_string("plugins/pddl-robot-memory/output-problem-description"));
76 if (config->exists("plugins/pddl-robot-memory/goal"))
77 goal = config->get_string("plugins/pddl-robot-memory/goal");
78
79 //setup interface
80 gen_if = blackboard->open_for_writing<PddlGenInterface>(
81 config->get_string("plugins/pddl-robot-memory/interface-name").c_str());
82 gen_if->set_msg_id(0);
83 gen_if->set_final(false);
84 gen_if->write();
85
86 //setup interface listener
88 blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
89
90 if (config->get_bool("plugins/pddl-robot-memory/generate-on-init")) {
91 wakeup(); //activates loop where the generation is done
92 }
93}
94
95/**
96 * Thread is only waked up if there is a new interface message to generate a pddl
97 */
98void
100{
101 //read input template of problem description
102 std::string input;
103 std::ifstream istream(input_path);
104 if (istream.is_open()) {
105 input =
106 std::string((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
107 istream.close();
108 } else {
109 logger->log_error(name(), "Could not open %s", input_path.c_str());
110 }
111 //set template delimeters to << >>
112 input = "{{=<< >>=}}" + input;
113
114 //Dictionary how to fill the templates
115 ctemplate::TemplateDictionary dict("pddl-rm");
116
117 basic::document facets;
118
119 //find queries in template
120 size_t cur_pos = 0;
121 std::map<std::string, std::string> templates;
122 while (input.find("<<#", cur_pos) != std::string::npos) {
123 cur_pos = input.find("<<#", cur_pos) + 3;
124 size_t tpl_end_pos = input.find(">>", cur_pos);
125 //is a query in the template? (indicated by '|')
126 size_t q_del_pos = input.find("|", cur_pos);
127 if (q_del_pos == std::string::npos || q_del_pos > tpl_end_pos)
128 continue; //no query to execute
129 //parse: template name | query
130 std::string template_name = input.substr(cur_pos, q_del_pos - cur_pos);
131 std::string query_str = input.substr(q_del_pos + 1, tpl_end_pos - (q_del_pos + 1));
132 if (templates.find(template_name) != templates.end()) {
133 if (templates[template_name] != query_str) {
135 "Template with same name '%s' but different query '%s' vs '%s'!",
136 template_name.c_str(),
137 query_str.c_str(),
138 templates[template_name].c_str());
139 } else {
140 input.erase(q_del_pos, tpl_end_pos - q_del_pos);
141 continue;
142 }
143 }
144 templates[template_name] = query_str;
145 //remove query stuff from input (its not part of the ctemplate features)
146 input.erase(q_del_pos, tpl_end_pos - q_del_pos);
147
148 try {
149 //fill dictionary to expand query template:
150 /*
151 QResCursor cursor = robot_memory->query(fromjson(query_str), collection);
152 while(cursor->more())
153 {
154 BSONObj obj = cursor->next();
155 //dictionary for one entry
156 ctemplate::TemplateDictionary *entry_dict = dict.AddSectionDictionary(template_name);
157 fill_dict_from_document(entry_dict, obj);
158 }
159 */
160 facets.append(basic::kvp(template_name, [query_str](basic::sub_array array) {
161 basic::document query;
162 query.append(basic::kvp("$match", from_json(query_str)));
163 array.append(query.view());
164 }));
165 } catch (bsoncxx::exception &e) {
166 logger->log_error("PddlRobotMemory",
167 "Template query failed: %s\n%s",
168 e.what(),
169 query_str.c_str());
170 }
171 }
172
173 basic::document aggregate_query;
174 mongocxx::pipeline aggregate_pipeline{};
175 aggregate_pipeline.facet(facets.view());
176 auto res = robot_memory->aggregate(aggregate_pipeline, collection);
177 for (auto doc : res) {
178 for (document::element ele : doc) {
179 // check validity && type before trying to iterate
180 if (ele && ele.type() == type::k_array) {
181 array::view subarray{ele.get_array().value};
182 for (array::element msg : subarray) {
183 if (msg.type() == bsoncxx::type::k_document) {
184 ctemplate::TemplateDictionary *entry_dict =
185 dict.AddSectionDictionary(std::string(ele.key()).c_str());
186 fill_dict_from_document(entry_dict, msg.get_document().view());
187 } else {
188 throw Exception(
189 (std::string("Error while retrieving domain facts and objects: expected document "
190 "type but got ")
191 + bsoncxx::to_string(msg.type()))
192 .c_str());
193 }
194 }
195 } else {
196 throw Exception(
197 (std::string(
198 "Error while retrieving domain facts and objects: expected k_array type but got: ")
199 + bsoncxx::to_string(ele.type()))
200 .c_str());
201 }
202 }
203 }
204
205 //Add goal to dictionary
206 dict.SetValue("GOAL", goal);
207
208 //prepare template expanding
209 ctemplate::StringToTemplateCache("tpl-cache", input, ctemplate::DO_NOT_STRIP);
210 if (!ctemplate::TemplateNamelist::IsAllSyntaxOkay(ctemplate::DO_NOT_STRIP)) {
211 logger->log_error(name(), "Syntax error in template %s:", input_path.c_str());
212 std::vector<std::string> error_list =
213 ctemplate::TemplateNamelist::GetBadSyntaxList(false, ctemplate::DO_NOT_STRIP);
214 for (std::string error : error_list) {
215 logger->log_error(name(), "%s", error.c_str());
216 }
217 }
218 //Let ctemplate expand the input
219 std::string output;
220 ctemplate::ExpandTemplate("tpl-cache", ctemplate::DO_NOT_STRIP, &dict, &output);
221
222 //generate output
223 logger->log_info(name(), "Output:\n%s", output.c_str());
224 std::ofstream ostream(output_path);
225 if (ostream.is_open()) {
226 ostream << output.c_str();
227 ostream.close();
228 } else {
229 logger->log_error(name(), "Could not open %s", output_path.c_str());
230 }
231
232 logger->log_info(name(), "Generation of PDDL problem description finished");
233 gen_if->set_final(true);
234 gen_if->write();
235}
236
237void
239{
240 blackboard->close(gen_if);
241}
242
243bool
244PddlRobotMemoryThread::bb_interface_message_received(Interface * interface,
245 fawkes::Message *message) noexcept
246{
247 if (message->is_of_type<PddlGenInterface::GenerateMessage>()) {
248 PddlGenInterface::GenerateMessage *msg = (PddlGenInterface::GenerateMessage *)message;
249 gen_if->set_msg_id(msg->id());
250 gen_if->set_final(false);
251 gen_if->write();
252 if (std::string(msg->goal()) != "")
253 goal = msg->goal();
254 wakeup(); //activates loop where the generation is done
255 } else {
256 logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
257 }
258 return false;
259}
260
261/**
262 * Fills a dictionary with key value pairs from a document. Recursive to handle subdocuments
263 * @param dict Dictionary to fill
264 * @param obj Document
265 * @param prefix Prefix of previous super-documents keys
266 */
267void
268PddlRobotMemoryThread::fill_dict_from_document(ctemplate::TemplateDictionary *dict,
269 const bsoncxx::document::view &doc,
270 std::string prefix)
271{
272 for (auto elem : doc) {
273 switch (elem.type()) {
274 case type::k_double:
275 dict->SetValue(prefix + std::string(elem.key()), std::to_string(elem.get_double()));
276 break;
277 case type::k_utf8:
278 dict->SetValue(prefix + std::string(elem.key()), elem.get_utf8().value.to_string());
279 break;
280 case type::k_bool:
281 dict->SetValue(prefix + std::string(elem.key()), std::to_string(elem.get_bool()));
282 break;
283 case type::k_int32:
284 dict->SetIntValue(prefix + std::string(elem.key()), elem.get_int32());
285 break;
286 case type::k_int64:
287 dict->SetIntValue(prefix + std::string(elem.key()), elem.get_int64());
288 break;
289 case type::k_document:
290 fill_dict_from_document(dict,
291 elem.get_document().view(),
292 prefix + std::string(elem.key()) + "_");
293 break;
294 case type::k_oid: //ObjectId
295 dict->SetValue(prefix + std::string(elem.key()), elem.get_oid().value.to_string());
296 break;
297 case type::k_array: {
298 // access array elements as if they were a subdocument with key-value pairs
299 // using the indices as keys
300 basic::document b;
301 array::view array = elem.get_array().value;
302 uint i = 0;
303 for (auto e : array) {
304 b.append(basic::kvp(std::to_string(i++), e.get_value()));
305 }
306 fill_dict_from_document(dict, b.view(), prefix + std::string(elem.key()) + "_");
307 // additionally feed the whole array as space-separated list
308 std::string array_string;
309 for (auto e : array) {
310 // TODO:adapt to other types.
311 array_string += " ";
312 switch (e.type()) {
313 case type::k_int64: array_string += std::to_string(e.get_int64()); break;
314 case type::k_utf8: array_string += e.get_utf8().value.to_string(); break;
315 default: throw Exception("Not implemented");
316 }
317 }
318 dict->SetValue(prefix + std::string(elem.key()), array_string);
319 break;
320 }
321 default: dict->SetValue(prefix + std::string(elem.key()), "INVALID_VALUE_TYPE");
322 }
323 }
324}
virtual void loop()
Thread is only waked up if there is a new interface message to generate a pddl.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
mongocxx::cursor aggregate(mongocxx::pipeline &pipeline, const std::string &collection="")
Performs an aggregation operation on the robot memory (https://docs.mongodb.com/v3....
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
virtual void log_error(const char *component, const char *format,...)=0
Log error 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
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
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
void wakeup()
Wake up thread.
Definition: thread.cpp:995
Fawkes library namespace.