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/agents/"
74  + config->get_string("plugins/pddl-robot-memory/input-problem-description"));
75  output_path = StringConversions::resolve_path(
76  "@BASEDIR@/src/agents/"
77  + config->get_string("plugins/pddl-robot-memory/output-problem-description"));
78  if (config->exists("plugins/pddl-robot-memory/goal"))
79  goal = config->get_string("plugins/pddl-robot-memory/goal");
80 
81  //setup interface
82  gen_if = blackboard->open_for_writing<PddlGenInterface>(
83  config->get_string("plugins/pddl-robot-memory/interface-name").c_str());
84  gen_if->set_msg_id(0);
85  gen_if->set_final(false);
86  gen_if->write();
87 
88  //setup interface listener
90  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
91 
92  if (config->get_bool("plugins/pddl-robot-memory/generate-on-init")) {
93  wakeup(); //activates loop where the generation is done
94  }
95 }
96 
97 /**
98  * Thread is only waked up if there is a new interface message to generate a pddl
99  */
100 void
102 {
103  //read input template of problem description
104  std::string input;
105  std::ifstream istream(input_path);
106  if (istream.is_open()) {
107  input =
108  std::string((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
109  istream.close();
110  } else {
111  logger->log_error(name(), "Could not open %s", input_path.c_str());
112  }
113  //set template delimeters to << >>
114  input = "{{=<< >>=}}" + input;
115 
116  //Dictionary how to fill the templates
117  ctemplate::TemplateDictionary dict("pddl-rm");
118 
119  basic::document facets;
120 
121  //find queries in template
122  size_t cur_pos = 0;
123  std::map<std::string, std::string> templates;
124  while (input.find("<<#", cur_pos) != std::string::npos) {
125  cur_pos = input.find("<<#", cur_pos) + 3;
126  size_t tpl_end_pos = input.find(">>", cur_pos);
127  //is a query in the template? (indicated by '|')
128  size_t q_del_pos = input.find("|", cur_pos);
129  if (q_del_pos == std::string::npos || q_del_pos > tpl_end_pos)
130  continue; //no query to execute
131  //parse: template name | query
132  std::string template_name = input.substr(cur_pos, q_del_pos - cur_pos);
133  std::string query_str = input.substr(q_del_pos + 1, tpl_end_pos - (q_del_pos + 1));
134  if (templates.find(template_name) != templates.end()) {
135  if (templates[template_name] != query_str) {
136  logger->log_error(name(),
137  "Template with same name '%s' but different query '%s' vs '%s'!",
138  template_name.c_str(),
139  query_str.c_str(),
140  templates[template_name].c_str());
141  } else {
142  input.erase(q_del_pos, tpl_end_pos - q_del_pos);
143  continue;
144  }
145  }
146  templates[template_name] = query_str;
147  //remove query stuff from input (its not part of the ctemplate features)
148  input.erase(q_del_pos, tpl_end_pos - q_del_pos);
149 
150  try {
151  //fill dictionary to expand query template:
152  /*
153  QResCursor cursor = robot_memory->query(fromjson(query_str), collection);
154  while(cursor->more())
155  {
156  BSONObj obj = cursor->next();
157  //dictionary for one entry
158  ctemplate::TemplateDictionary *entry_dict = dict.AddSectionDictionary(template_name);
159  fill_dict_from_document(entry_dict, obj);
160  }
161  */
162  facets.append(basic::kvp(template_name, [query_str](basic::sub_array array) {
163  basic::document query;
164  query.append(basic::kvp("$match", from_json(query_str)));
165  array.append(query.view());
166  }));
167  } catch (bsoncxx::exception &e) {
168  logger->log_error("PddlRobotMemory",
169  "Template query failed: %s\n%s",
170  e.what(),
171  query_str.c_str());
172  }
173  }
174 
175  basic::document aggregate_query;
176  aggregate_query.append(basic::kvp("$facet", facets.view()));
177  std::vector<document::view> aggregate_pipeline{aggregate_query.view()};
178  auto res = robot_memory->aggregate(aggregate_pipeline, collection);
179  auto result = res.view()["result"]["0"].get_document().view();
180  //BSONObj result = res.getField("result").Obj()["0"].Obj();
181  for (auto e : result) {
182  for (auto f : e.get_document().view()) {
183  ctemplate::TemplateDictionary *entry_dict = dict.AddSectionDictionary(std::string(e.key()));
184  fill_dict_from_document(entry_dict, f.get_document().view());
185  }
186  }
187 
188  //Add goal to dictionary
189  dict.SetValue("GOAL", goal);
190 
191  //prepare template expanding
192  ctemplate::StringToTemplateCache("tpl-cache", input, ctemplate::DO_NOT_STRIP);
193  if (!ctemplate::TemplateNamelist::IsAllSyntaxOkay(ctemplate::DO_NOT_STRIP)) {
194  logger->log_error(name(), "Syntax error in template %s:", input_path.c_str());
195  std::vector<std::string> error_list =
196  ctemplate::TemplateNamelist::GetBadSyntaxList(false, ctemplate::DO_NOT_STRIP);
197  for (std::string error : error_list) {
198  logger->log_error(name(), "%s", error.c_str());
199  }
200  }
201  //Let ctemplate expand the input
202  std::string output;
203  ctemplate::ExpandTemplate("tpl-cache", ctemplate::DO_NOT_STRIP, &dict, &output);
204 
205  //generate output
206  logger->log_info(name(), "Output:\n%s", output.c_str());
207  std::ofstream ostream(output_path);
208  if (ostream.is_open()) {
209  ostream << output.c_str();
210  ostream.close();
211  } else {
212  logger->log_error(name(), "Could not open %s", output_path.c_str());
213  }
214 
215  logger->log_info(name(), "Generation of PDDL problem description finished");
216  gen_if->set_final(true);
217  gen_if->write();
218 }
219 
220 void
222 {
223  blackboard->close(gen_if);
224 }
225 
226 bool
227 PddlRobotMemoryThread::bb_interface_message_received(Interface * interface,
228  fawkes::Message *message) throw()
229 {
230  if (message->is_of_type<PddlGenInterface::GenerateMessage>()) {
231  PddlGenInterface::GenerateMessage *msg = (PddlGenInterface::GenerateMessage *)message;
232  gen_if->set_msg_id(msg->id());
233  gen_if->set_final(false);
234  gen_if->write();
235  if (std::string(msg->goal()) != "")
236  goal = msg->goal();
237  wakeup(); //activates loop where the generation is done
238  } else {
239  logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
240  }
241  return false;
242 }
243 
244 /**
245  * Fills a dictionary with key value pairs from a document. Recursive to handle subdocuments
246  * @param dict Dictionary to fill
247  * @param obj Document
248  * @param prefix Prefix of previous super-documents keys
249  */
250 void
251 PddlRobotMemoryThread::fill_dict_from_document(ctemplate::TemplateDictionary *dict,
252  const bsoncxx::document::view &doc,
253  std::string prefix)
254 {
255  for (auto elem : doc) {
256  switch (elem.type()) {
257  case type::k_double:
258  dict->SetValue(prefix + std::string(elem.key()), std::to_string(elem.get_double()));
259  break;
260  case type::k_utf8:
261  dict->SetValue(prefix + std::string(elem.key()), elem.get_utf8().value.to_string());
262  break;
263  case type::k_bool:
264  dict->SetValue(prefix + std::string(elem.key()), std::to_string(elem.get_bool()));
265  break;
266  case type::k_int32:
267  dict->SetIntValue(prefix + std::string(elem.key()), elem.get_int32());
268  break;
269  case type::k_int64:
270  dict->SetIntValue(prefix + std::string(elem.key()), elem.get_int64());
271  break;
272  case type::k_document:
273  fill_dict_from_document(dict,
274  elem.get_document().view(),
275  prefix + std::string(elem.key()) + "_");
276  break;
277  case type::k_oid: //ObjectId
278  dict->SetValue(prefix + std::string(elem.key()), elem.get_oid().value.to_string());
279  break;
280  case type::k_array: {
281  // access array elements as if they were a subdocument with key-value pairs
282  // using the indices as keys
283  basic::document b;
284  array::view array = elem.get_array();
285  uint i = 0;
286  for (auto e : array) {
287  b.append(basic::kvp(std::to_string(i++), e.get_document().view()));
288  }
289  fill_dict_from_document(dict, b.view(), prefix + std::string(elem.key()) + "_");
290  // additionally feed the whole array as space-separated list
291  std::string array_string;
292  for (auto e : array) {
293  // TODO: This only works for string arrays, adapt to other types.
294  array_string += " " + e.get_utf8().value.to_string();
295  }
296  dict->SetValue(prefix + std::string(elem.key()), array_string);
297  break;
298  }
299  default: dict->SetValue(prefix + std::string(elem.key()), "INVALID_VALUE_TYPE");
300  }
301  }
302 }
fawkes::MultiLogger::log_error
virtual void log_error(const char *component, const char *format,...)
Definition: multi.cpp:243
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
fawkes::Message
Definition: message.h:41
fawkes::Thread::wakeup
void wakeup()
Wake up thread.
Definition: thread.cpp:1001
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:47
PddlRobotMemoryThread::finalize
virtual void finalize()
Finalize the thread.
Definition: pddl_robot_memory_thread.cpp:221
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
RobotMemory::aggregate
bsoncxx::document::value aggregate(const std::vector< bsoncxx::document::view > &pipeline, const std::string &collection="")
Aggregation call on the robot memory.
Definition: robot_memory.cpp:233
PddlRobotMemoryThread::init
virtual void init()
Initialize the thread.
Definition: pddl_robot_memory_thread.cpp:68
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes
fawkes::RobotMemoryAspect::robot_memory
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
Definition: robot_memory_aspect.h:58
fawkes::BlackBoardInterfaceListener::bbil_add_message_interface
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
Definition: interface_listener.cpp:247
fawkes::Interface
Definition: interface.h:78
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
PddlRobotMemoryThread::loop
virtual void loop()
Thread is only waked up if there is a new interface message to generate a pddl.
Definition: pddl_robot_memory_thread.cpp:101
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Configuration::exists
virtual bool exists(const char *path)=0
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0