24#include "pddl_robot_memory_thread.h"
26#include <utils/misc/string_conversions.h>
28#include <bsoncxx/exception/exception.hpp>
32using namespace mongocxx;
33using namespace bsoncxx;
34using namespace bsoncxx::builder;
61PddlRobotMemoryThread::PddlRobotMemoryThread()
62:
Thread(
"PddlRobotMemoryThread",
Thread::OPMODE_WAITFORWAKEUP),
72 input_path = StringConversions::resolve_path(
73 "@BASEDIR@/src/" +
config->
get_string(
"plugins/pddl-robot-memory/input-problem-description"));
74 output_path = StringConversions::resolve_path(
75 "@BASEDIR@/src/" +
config->
get_string(
"plugins/pddl-robot-memory/output-problem-description"));
82 gen_if->set_msg_id(0);
83 gen_if->set_final(
false);
90 if (
config->
get_bool(
"plugins/pddl-robot-memory/generate-on-init")) {
103 std::ifstream istream(input_path);
104 if (istream.is_open()) {
106 std::string((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
112 input =
"{{=<< >>=}}" + input;
115 ctemplate::TemplateDictionary dict(
"pddl-rm");
117 basic::document facets;
121 std::map<std::string, std::string> templates;
122 while (input.find(
"<<#", cur_pos) != std::string::npos) {
123 cur_pos = input.find(
"<<#", cur_pos) + 3;
124 size_t tpl_end_pos = input.find(
">>", cur_pos);
126 size_t q_del_pos = input.find(
"|", cur_pos);
127 if (q_del_pos == std::string::npos || q_del_pos > tpl_end_pos)
130 std::string template_name = input.substr(cur_pos, q_del_pos - cur_pos);
131 std::string query_str = input.substr(q_del_pos + 1, tpl_end_pos - (q_del_pos + 1));
132 if (templates.find(template_name) != templates.end()) {
133 if (templates[template_name] != query_str) {
135 "Template with same name '%s' but different query '%s' vs '%s'!",
136 template_name.c_str(),
138 templates[template_name].c_str());
140 input.erase(q_del_pos, tpl_end_pos - q_del_pos);
144 templates[template_name] = query_str;
146 input.erase(q_del_pos, tpl_end_pos - q_del_pos);
160 facets.append(basic::kvp(template_name, [query_str](basic::sub_array array) {
161 basic::document query;
162 query.append(basic::kvp(
"$match", from_json(query_str)));
163 array.append(query.view());
165 }
catch (bsoncxx::exception &e) {
167 "Template query failed: %s\n%s",
173 basic::document aggregate_query;
174 mongocxx::pipeline aggregate_pipeline{};
175 aggregate_pipeline.facet(facets.view());
177 for (
auto doc : res) {
178 for (document::element ele : doc) {
180 if (ele && ele.type() == type::k_array) {
181 array::view subarray{ele.get_array().value};
182 for (array::element msg : subarray) {
183 if (msg.type() == bsoncxx::type::k_document) {
184 ctemplate::TemplateDictionary *entry_dict =
185 dict.AddSectionDictionary(std::string(ele.key()).c_str());
186 fill_dict_from_document(entry_dict, msg.get_document().view());
189 (std::string(
"Error while retrieving domain facts and objects: expected document "
191 + bsoncxx::to_string(msg.type()))
198 "Error while retrieving domain facts and objects: expected k_array type but got: ")
199 + bsoncxx::to_string(ele.type()))
206 dict.SetValue(
"GOAL", goal);
209 ctemplate::StringToTemplateCache(
"tpl-cache", input, ctemplate::DO_NOT_STRIP);
210 if (!ctemplate::TemplateNamelist::IsAllSyntaxOkay(ctemplate::DO_NOT_STRIP)) {
212 std::vector<std::string> error_list =
213 ctemplate::TemplateNamelist::GetBadSyntaxList(
false, ctemplate::DO_NOT_STRIP);
214 for (std::string error : error_list) {
220 ctemplate::ExpandTemplate(
"tpl-cache", ctemplate::DO_NOT_STRIP, &dict, &output);
224 std::ofstream ostream(output_path);
225 if (ostream.is_open()) {
226 ostream << output.c_str();
233 gen_if->set_final(
true);
244PddlRobotMemoryThread::bb_interface_message_received(
Interface * interface,
247 if (message->is_of_type<PddlGenInterface::GenerateMessage>()) {
248 PddlGenInterface::GenerateMessage *msg = (PddlGenInterface::GenerateMessage *)message;
249 gen_if->set_msg_id(msg->id());
250 gen_if->set_final(
false);
252 if (std::string(msg->goal()) !=
"")
256 logger->
log_error(name(),
"Received unknown message of type %s, ignoring", message->type());
268PddlRobotMemoryThread::fill_dict_from_document(ctemplate::TemplateDictionary *dict,
269 const bsoncxx::document::view &doc,
272 for (
auto elem : doc) {
273 switch (elem.type()) {
275 dict->SetValue(prefix + std::string(elem.key()), std::to_string(elem.get_double()));
278 dict->SetValue(prefix + std::string(elem.key()), elem.get_utf8().value.to_string());
281 dict->SetValue(prefix + std::string(elem.key()), std::to_string(elem.get_bool()));
284 dict->SetIntValue(prefix + std::string(elem.key()), elem.get_int32());
287 dict->SetIntValue(prefix + std::string(elem.key()), elem.get_int64());
289 case type::k_document:
290 fill_dict_from_document(dict,
291 elem.get_document().view(),
292 prefix + std::string(elem.key()) +
"_");
295 dict->SetValue(prefix + std::string(elem.key()), elem.get_oid().value.to_string());
297 case type::k_array: {
301 array::view array = elem.get_array().value;
303 for (
auto e : array) {
304 b.append(basic::kvp(std::to_string(i++), e.get_value()));
306 fill_dict_from_document(dict, b.view(), prefix + std::string(elem.key()) +
"_");
308 std::string array_string;
309 for (
auto e : array) {
313 case type::k_int64: array_string += std::to_string(e.get_int64());
break;
314 case type::k_utf8: array_string += e.get_utf8().value.to_string();
break;
315 default:
throw Exception(
"Not implemented");
318 dict->SetValue(prefix + std::string(elem.key()), array_string);
321 default: dict->SetValue(prefix + std::string(elem.key()),
"INVALID_VALUE_TYPE");
virtual void loop()
Thread is only waked up if there is a new interface message to generate a pddl.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
mongocxx::cursor aggregate(mongocxx::pipeline &pipeline, const std::string &collection="")
Performs an aggregation operation on the robot memory (https://docs.mongodb.com/v3....
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Base class for all Fawkes BlackBoard interfaces.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
virtual void log_error(const char *component, const char *format,...)
Log error message.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void wakeup()
Wake up thread.
Fawkes library namespace.