22 #include "stn-generator_thread.h" 24 #include <utils/misc/string_conversions.h> 32 using namespace mongo;
41 :
Thread(
"StnGeneratorThread",
Thread::OPMODE_WAITFORWAKEUP),
50 std::string cfg_prefix =
"plugins/stn-generator/";
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");
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"));
61 std::ifstream s(pddl_domain_path);
65 std::string pddl_domain;
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>());
73 std::string classic_dom_path =
74 StringConversions::resolve_path(
config->
get_string(cfg_prefix +
"classic-domain-file"));
91 std::ifstream s(cfg_pddl_problem_path_);
93 logger->
log_error(
name(),
"Could not open problem-file at %s", cfg_pddl_problem_path_.c_str());
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>());
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) {
110 for (
auto &arg : o.getField(
"args").Array()) {
118 logger->
log_debug(
name(),
"Added Plan action %s to STN", o.getField(
"name").str().c_str());
122 if (cfg_draw_graph_) {
125 }
catch (std::out_of_range &e) {
131 if (cfg_publish_to_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);
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" 148 rm_final <<
"count" << std::to_string(num_published_actions_);
162 if (interface->uid() == plan_if_->uid()) {
164 if (plan_if_->is_final()) {
165 logger->
log_info(name(),
"Planning is final, starting STN generation");
169 logger->
log_error(name(),
"Received data change for wrong interface");
void read_initial_state(const std::string &pddl_problem_string)
Read the initial state from the given PDDL problem.
QResCursor query(mongo::Query query, const std::string &collection="")
Query information from the robot memory.
A Simple Temporal Network.
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.
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.
Base class for all Fawkes BlackBoard interfaces.
void generate()
Regenerate the STN.
virtual void init()
Initialize the thread.
Logger * logger
This is the Logger member used to access the logger.
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.
void add_plan_action(const std::string &name, const std::string ¶ms)
Add a (grounded action).
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.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
const char * name() const
Get name of thread.
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.
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.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.