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 <fstream>
29 
30 using namespace fawkes;
31 using namespace mongo;
32 
33 /** @class PddlRobotMemoryThread 'pddl_robot_memory_thread.h'
34  * Generate PDDL files from the robot memory
35  *
36  * This plugin uses the template engine ctemplate to generate a pddl
37  * from a template file and the robot memory.
38  *
39  * The template file can use the following templates to generate some output
40  * for each document returned by a query:
41  *
42  * Example: \c "<<#ONTABLE|{relation:'on-table'}>> (on-table <<object>>) <</ONTABLE>>"
43  * Yields: (on-table a) (on-table b)
44  * When these documents are in the database:
45  * {relation:'on-table', object:'a'}, {relation:'on-table', object:'b'}
46  *
47  * The selection template \c "<<#UNIQUENAME|query>> something <</UNIQUENAME>>"
48  * queries the robot memory and inserts 'something' for each returned document.
49  *
50  * Variable templates \c "<<key>>" inside the selection are substituted by the values
51  * of that key in the document returned by the query. You can also access subdocuments
52  * and arrays as follows:
53  * (e.g. \c "<<position_translation_0>>" for a document {position:{translation:[0,1,2], orientation:[0,1,2]}})
54  *
55  * @author Frederik Zwilling
56  */
57 
58 PddlRobotMemoryThread::PddlRobotMemoryThread()
59 : Thread("PddlRobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
60  BlackBoardInterfaceListener("PddlRobotMemoryThread")
61 {
62 }
63 
64 void
66 {
67  //read config values
68  collection = config->get_string("plugins/pddl-robot-memory/collection");
69  input_path = StringConversions::resolve_path(
70  "@BASEDIR@/src/agents/"
71  + config->get_string("plugins/pddl-robot-memory/input-problem-description"));
72  output_path = StringConversions::resolve_path(
73  "@BASEDIR@/src/agents/"
74  + config->get_string("plugins/pddl-robot-memory/output-problem-description"));
75  if (config->exists("plugins/pddl-robot-memory/goal"))
76  goal = config->get_string("plugins/pddl-robot-memory/goal");
77 
78  //setup interface
79  gen_if = blackboard->open_for_writing<PddlGenInterface>(
80  config->get_string("plugins/pddl-robot-memory/interface-name").c_str());
81  gen_if->set_msg_id(0);
82  gen_if->set_final(false);
83  gen_if->write();
84 
85  //setup interface listener
87  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
88 
89  if (config->get_bool("plugins/pddl-robot-memory/generate-on-init")) {
90  wakeup(); //activates loop where the generation is done
91  }
92 }
93 
94 /**
95  * Thread is only waked up if there is a new interface message to generate a pddl
96  */
97 void
99 {
100  //read input template of problem description
101  std::string input;
102  std::ifstream istream(input_path);
103  if (istream.is_open()) {
104  input =
105  std::string((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
106  istream.close();
107  } else {
108  logger->log_error(name(), "Could not open %s", input_path.c_str());
109  }
110  //set template delimeters to << >>
111  input = "{{=<< >>=}}" + input;
112 
113  //Dictionary how to fill the templates
114  ctemplate::TemplateDictionary dict("pddl-rm");
115 
116  BSONObjBuilder facets;
117 
118  //find queries in template
119  size_t cur_pos = 0;
120  std::map<std::string, std::string> templates;
121  while (input.find("<<#", cur_pos) != std::string::npos) {
122  cur_pos = input.find("<<#", cur_pos) + 3;
123  size_t tpl_end_pos = input.find(">>", cur_pos);
124  //is a query in the template? (indicated by '|')
125  size_t q_del_pos = input.find("|", cur_pos);
126  if (q_del_pos == std::string::npos || q_del_pos > tpl_end_pos)
127  continue; //no query to execute
128  //parse: template name | query
129  std::string template_name = input.substr(cur_pos, q_del_pos - cur_pos);
130  std::string query_str = input.substr(q_del_pos + 1, tpl_end_pos - (q_del_pos + 1));
131  if (templates.find(template_name) != templates.end()) {
132  if (templates[template_name] != query_str) {
133  logger->log_error(name(),
134  "Template with same name '%s' but different query '%s' vs '%s'!",
135  template_name.c_str(),
136  query_str.c_str(),
137  templates[template_name].c_str());
138  } else {
139  input.erase(q_del_pos, tpl_end_pos - q_del_pos);
140  continue;
141  }
142  }
143  templates[template_name] = query_str;
144  //remove query stuff from input (its not part of the ctemplate features)
145  input.erase(q_del_pos, tpl_end_pos - q_del_pos);
146 
147  try {
148  //fill dictionary to expand query template:
149  /*
150  QResCursor cursor = robot_memory->query(fromjson(query_str), collection);
151  while(cursor->more())
152  {
153  BSONObj obj = cursor->next();
154  //dictionary for one entry
155  ctemplate::TemplateDictionary *entry_dict = dict.AddSectionDictionary(template_name);
156  fill_dict_from_document(entry_dict, obj);
157  }
158  */
159  mongo::BufBuilder & bb = facets.subarrayStart(template_name);
160  mongo::BSONArrayBuilder *arrb = new mongo::BSONArrayBuilder(bb);
161  BSONObjBuilder query;
162  query.append("$match", fromjson(query_str));
163  arrb->append(query.obj());
164  delete arrb;
165 #ifdef HAVE_MONGODB_VERSION_H
166  } catch (mongo::MsgAssertionException &e) {
167 #else
168  } catch (mongo::AssertionException &e) {
169 #endif
170  logger->log_error("PddlRobotMemory",
171  "Template query failed: %s\n%s",
172  e.what(),
173  query_str.c_str());
174  }
175  }
176 
177  BSONObjBuilder aggregate_query;
178  aggregate_query.append("$facet", facets.obj());
179  BSONObj aggregate_query_obj(aggregate_query.obj());
180  std::vector<mongo::BSONObj> aggregate_pipeline{aggregate_query_obj};
181  BSONObj res = robot_memory->aggregate(aggregate_pipeline, collection);
182  BSONObj result = res.getField("result").Obj()["0"].Obj();
183  for (BSONObj::iterator i = result.begin(); i.more();) {
184  BSONElement e = i.next();
185  for (BSONObj::iterator j = e.Obj().begin(); j.more();) {
186  BSONElement f = j.next();
187  ctemplate::TemplateDictionary *entry_dict = dict.AddSectionDictionary(e.fieldName());
188  fill_dict_from_document(entry_dict, f.Obj());
189  }
190  }
191 
192  //Add goal to dictionary
193  dict.SetValue("GOAL", goal);
194 
195  //prepare template expanding
196  ctemplate::StringToTemplateCache("tpl-cache", input, ctemplate::DO_NOT_STRIP);
197  if (!ctemplate::TemplateNamelist::IsAllSyntaxOkay(ctemplate::DO_NOT_STRIP)) {
198  logger->log_error(name(), "Syntax error in template %s:", input_path.c_str());
199  std::vector<std::string> error_list =
200  ctemplate::TemplateNamelist::GetBadSyntaxList(false, ctemplate::DO_NOT_STRIP);
201  for (std::string error : error_list) {
202  logger->log_error(name(), "%s", error.c_str());
203  }
204  }
205  //Let ctemplate expand the input
206  std::string output;
207  ctemplate::ExpandTemplate("tpl-cache", ctemplate::DO_NOT_STRIP, &dict, &output);
208 
209  //generate output
210  logger->log_info(name(), "Output:\n%s", output.c_str());
211  std::ofstream ostream(output_path);
212  if (ostream.is_open()) {
213  ostream << output.c_str();
214  ostream.close();
215  } else {
216  logger->log_error(name(), "Could not open %s", output_path.c_str());
217  }
218 
219  logger->log_info(name(), "Generation of PDDL problem description finished");
220  gen_if->set_final(true);
221  gen_if->write();
222 }
223 
224 void
226 {
227  blackboard->close(gen_if);
228 }
229 
230 bool
231 PddlRobotMemoryThread::bb_interface_message_received(Interface * interface,
232  fawkes::Message *message) throw()
233 {
234  if (message->is_of_type<PddlGenInterface::GenerateMessage>()) {
235  PddlGenInterface::GenerateMessage *msg = (PddlGenInterface::GenerateMessage *)message;
236  gen_if->set_msg_id(msg->id());
237  gen_if->set_final(false);
238  gen_if->write();
239  if (std::string(msg->goal()) != "")
240  goal = msg->goal();
241  wakeup(); //activates loop where the generation is done
242  } else {
243  logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
244  }
245  return false;
246 }
247 
248 /**
249  * Fills a dictionary with key value pairs from a document. Recursive to handle subdocuments
250  * @param dict Dictionary to fill
251  * @param obj Document
252  * @param prefix Prefix of previous super-documents keys
253  */
254 void
255 PddlRobotMemoryThread::fill_dict_from_document(ctemplate::TemplateDictionary *dict,
256  BSONObj obj,
257  std::string prefix)
258 {
259  for (BSONObjIterator it = obj.begin(); it.more();) {
260  BSONElement elem = it.next();
261  switch (elem.type()) {
262  case mongo::NumberDouble:
263  dict->SetValue(prefix + elem.fieldName(), std::to_string(elem.Double()));
264  break;
265  case mongo::String: dict->SetValue(prefix + elem.fieldName(), elem.String()); break;
266  case mongo::Bool: dict->SetValue(prefix + elem.fieldName(), std::to_string(elem.Bool())); break;
267  case mongo::NumberInt: dict->SetIntValue(prefix + elem.fieldName(), elem.Int()); break;
268  case mongo::NumberLong:
269  dict->SetValue(prefix + elem.fieldName(), std::to_string(elem.Long()));
270  break;
271  case mongo::Object:
272  fill_dict_from_document(dict, elem.Obj(), prefix + elem.fieldName() + "_");
273  break;
274  case 7: //ObjectId
275  dict->SetValue(prefix + elem.fieldName(), elem.OID().toString());
276  break;
277  case mongo::Array: {
278  // access array elements as if they were a subdocument with key-value pairs
279  // using the indices as keys
280  BSONObjBuilder b;
281  for (size_t i = 0; i < elem.Array().size(); i++) {
282  b.append(elem.Array()[i]);
283  }
284  fill_dict_from_document(dict, b.obj(), prefix + elem.fieldName() + "_");
285  // additionally feed the whole array as space-separated list
286  std::string array_string;
287  for (size_t i = 0; i < elem.Array().size(); i++) {
288  // TODO: This only works for string arrays, adapt to other types.
289  array_string += " " + elem.Array()[i].String();
290  }
291  dict->SetValue(prefix + elem.fieldName(), array_string);
292  break;
293  }
294  default: dict->SetValue(prefix + elem.fieldName(), "INVALID_VALUE_TYPE");
295  }
296  }
297 }
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
mongo::BSONObj aggregate(const std::vector< mongo::BSONObj > &pipeline, const std::string &collection="")
Aggregation call on the robot memory.
Thread class encapsulation of pthreads.
Definition: thread.h:45
virtual void finalize()
Finalize the thread.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void loop()
Thread is only waked up if there is a new interface message to generate a pddl.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
void wakeup()
Wake up thread.
Definition: thread.cpp:995
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
virtual bool exists(const char *path)=0
Check if a given value exists.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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 std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.