Fawkes API  Fawkes Development Version
stn-generator_thread.cpp
1 
2 /***************************************************************************
3  * stn-generator_thread.cpp - stn-generator
4  *
5  * Created: Sat May 6 20:16:21 2017
6  * Copyright 2017 Matthias Loebach
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 "stn-generator_thread.h"
23 
24 #include <utils/misc/string_conversions.h>
25 
26 #include <chrono>
27 #include <fstream>
28 #include <streambuf>
29 #include <thread>
30 
31 using namespace fawkes;
32 using namespace mongo;
33 
34 /** @class StnGeneratorThread 'stn-generator_thread.h'
35  * Generates an STN representation of a sequential task plan
36  * @author Matthias Loebach
37  */
38 
39 /** Constructor. */
41 : Thread("StnGeneratorThread", Thread::OPMODE_WAITFORWAKEUP),
42  BlackBoardInterfaceListener("StnGeneratorThread")
43 {
44 }
45 
46 void
48 {
49  logger->log_info(name(), "reading config");
50  std::string cfg_prefix = "plugins/stn-generator/";
51  cfg_plan_collection_ = config->get_string(cfg_prefix + "plan/collection");
52  cfg_output_collection_ = config->get_string(cfg_prefix + "output/collection");
53  cfg_publish_to_robot_memory_ = config->get_bool(cfg_prefix + "output/publish-to-rm");
54  cfg_draw_graph_ = config->get_bool(cfg_prefix + "output/draw-graph");
55 
56  std::string pddl_domain_path =
57  StringConversions::resolve_path(config->get_string(cfg_prefix + "domain-file"));
58  cfg_pddl_problem_path_ =
59  StringConversions::resolve_path(config->get_string(cfg_prefix + "problem-file"));
60 
61  std::ifstream s(pddl_domain_path);
62  if (!s.good()) {
63  logger->log_error(name(), "Could not open domain-file at %s", pddl_domain_path.c_str());
64  }
65  std::string pddl_domain;
66 
67  s.seekg(0, std::ios::end);
68  pddl_domain.reserve(s.tellg());
69  s.seekg(0, std::ios::beg);
70  pddl_domain.assign((std::istreambuf_iterator<char>(s)), std::istreambuf_iterator<char>());
71 
72  if (config->get_bool(cfg_prefix + "generate-classic-domain")) {
73  std::string classic_dom_path =
74  StringConversions::resolve_path(config->get_string(cfg_prefix + "classic-domain-file"));
75  stn_ = new stn::Stn(logger, classic_dom_path);
76  } else {
77  stn_ = new stn::Stn(logger);
78  }
79  stn_->set_pddl_domain(pddl_domain);
80  logger->log_info(name(), "Created STN object from domain");
81 
82  plan_if_ = blackboard->open_for_reading<PddlPlannerInterface>(
83  config->get_string(cfg_prefix + "plan/interface").c_str());
84  bbil_add_data_interface(plan_if_);
85  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_DATA);
86 }
87 
88 void
90 {
91  std::ifstream s(cfg_pddl_problem_path_);
92  if (!s.good()) {
93  logger->log_error(name(), "Could not open problem-file at %s", cfg_pddl_problem_path_.c_str());
94  }
95  std::string pddl_problem;
96  s.seekg(0, std::ios::end);
97  pddl_problem.reserve(s.tellg());
98  s.seekg(0, std::ios::beg);
99  pddl_problem.assign((std::istreambuf_iterator<char>(s)), std::istreambuf_iterator<char>());
100  stn_->read_initial_state(pddl_problem);
101 
102  QResCursor cursor = robot_memory->query(fromjson("{plan:1}"), cfg_plan_collection_);
103  while (cursor->more()) {
104  BSONObj obj = cursor->next();
105  std::vector<BSONElement> actions = obj.getField("actions").Array();
106  for (auto &a : actions) {
107  BSONObj o = a.Obj();
108  std::string args;
109  bool first = true;
110  for (auto &arg : o.getField("args").Array()) {
111  if (!first) {
112  args += " ";
113  }
114  first = false;
115  args += arg.str();
116  }
117  stn_->add_plan_action(o.getField("name").str(), args);
118  logger->log_debug(name(), "Added Plan action %s to STN", o.getField("name").str().c_str());
119  }
120  }
121  stn_->generate();
122  if (cfg_draw_graph_) {
123  try {
124  stn_->drawGraph();
125  } catch (std::out_of_range &e) {
126  logger->log_warn(name(), "Failed to draw graph: %s", e.what());
127  }
128  }
129  logger->log_info(name(), "STN Generation finished.");
130 
131  if (cfg_publish_to_robot_memory_) {
132  //TODO reset actions in robot-memory
133  for (auto &action : stn_->get_bson()) {
134  BSONObjBuilder rm_action;
135  rm_action << "relation"
136  << "proposed-stn-action";
137  rm_action.appendElements(action);
138  robot_memory->insert(rm_action.obj(), cfg_output_collection_);
139  }
140  // ensure all actions are written to RM before acknowledment
141  std::this_thread::sleep_for(std::chrono::milliseconds(500));
142  num_published_actions_ += stn_->get_bson().size();
143  BSONObjBuilder rm_final;
144  rm_final << "relation"
145  << "stn-sync";
146  rm_final << "state"
147  << "synced";
148  rm_final << "count" << std::to_string(num_published_actions_);
149  robot_memory->insert(rm_final.obj(), cfg_output_collection_);
150  }
151 }
152 
153 void
155 {
156  delete stn_;
157 }
158 
159 void
161 {
162  if (interface->uid() == plan_if_->uid()) {
163  plan_if_->read();
164  if (plan_if_->is_final()) {
165  logger->log_info(name(), "Planning is final, starting STN generation");
166  wakeup();
167  }
168  } else {
169  logger->log_error(name(), "Received data change for wrong interface");
170  }
171 }
void read_initial_state(const std::string &pddl_problem_string)
Read the initial state from the given PDDL problem.
Definition: stn.cpp:93
QResCursor query(mongo::Query query, const std::string &collection="")
Query information from the robot memory.
A Simple Temporal Network.
Definition: stn.h:41
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
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.
StnGeneratorThread()
Constructor.
Thread class encapsulation of pthreads.
Definition: thread.h:45
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
void generate()
Regenerate the STN.
Definition: stn.cpp:208
virtual void init()
Initialize the thread.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
int insert(mongo::BSONObj obj, const std::string &collection="")
Inserts a document into the robot memory.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
void add_plan_action(const std::string &name, const std::string &params)
Add a (grounded action).
Definition: stn.cpp:73
virtual void loop()
Code to execute in the thread.
void set_pddl_domain(const std::string &pddl_domain_string)
Set the domain of the STN to the given PDDL domain.
Definition: stn.cpp:126
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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 void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
std::vector< mongo::BSONObj > get_bson()
Get a BSON representation of the STN.
Definition: stn.cpp:360
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void finalize()
Finalize the thread.
void drawGraph()
Render a graph representation of the STN.
Definition: stn.cpp:302
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.