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
33using namespace fawkes;
34using namespace mongocxx;
35using 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
49void
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());
88 blackboard->register_listener(this, BlackBoard::BBIL_FLAG_DATA);
89}
90
91void
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
154void
156{
157 delete stn_;
158}
159
160void
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 bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
virtual void loop()
Code to execute in the thread.
StnGeneratorThread()
Constructor.
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:80
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
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
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
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:130
std::vector< bsoncxx::document::value > get_bson()
Get a BSON representation of the STN.
Definition: stn.cpp:364
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:212
void drawGraph()
Render a graph representation of the STN.
Definition: stn.cpp:306
Fawkes library namespace.