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 
31 using namespace fawkes;
32 using namespace mongocxx;
33 using namespace bsoncxx;
34 using 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 
61 PddlRobotMemoryThread::PddlRobotMemoryThread()
62 : Thread("PddlRobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
63  BlackBoardInterfaceListener("PddlRobotMemoryThread")
64 {
65 }
66 
67 void
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  */
98 void
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) {
134  logger->log_error(name(),
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 
237 void
239 {
240  blackboard->close(gen_if);
241 }
242 
243 bool
244 PddlRobotMemoryThread::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  */
267 void
268 PddlRobotMemoryThread::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.