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