24 #include "pddl_robot_memory_thread.h" 26 #include <utils/misc/string_conversions.h> 31 using namespace mongo;
58 PddlRobotMemoryThread::PddlRobotMemoryThread()
59 :
Thread(
"PddlRobotMemoryThread",
Thread::OPMODE_WAITFORWAKEUP),
69 input_path = StringConversions::resolve_path(
70 "@BASEDIR@/src/agents/" 71 +
config->
get_string(
"plugins/pddl-robot-memory/input-problem-description"));
72 output_path = StringConversions::resolve_path(
73 "@BASEDIR@/src/agents/" 74 +
config->
get_string(
"plugins/pddl-robot-memory/output-problem-description"));
81 gen_if->set_msg_id(0);
82 gen_if->set_final(
false);
89 if (
config->
get_bool(
"plugins/pddl-robot-memory/generate-on-init")) {
102 std::ifstream istream(input_path);
103 if (istream.is_open()) {
105 std::string((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
111 input =
"{{=<< >>=}}" + input;
114 ctemplate::TemplateDictionary dict(
"pddl-rm");
116 BSONObjBuilder facets;
120 std::map<std::string, std::string> templates;
121 while (input.find(
"<<#", cur_pos) != std::string::npos) {
122 cur_pos = input.find(
"<<#", cur_pos) + 3;
123 size_t tpl_end_pos = input.find(
">>", cur_pos);
125 size_t q_del_pos = input.find(
"|", cur_pos);
126 if (q_del_pos == std::string::npos || q_del_pos > tpl_end_pos)
129 std::string template_name = input.substr(cur_pos, q_del_pos - cur_pos);
130 std::string query_str = input.substr(q_del_pos + 1, tpl_end_pos - (q_del_pos + 1));
131 if (templates.find(template_name) != templates.end()) {
132 if (templates[template_name] != query_str) {
134 "Template with same name '%s' but different query '%s' vs '%s'!",
135 template_name.c_str(),
137 templates[template_name].c_str());
139 input.erase(q_del_pos, tpl_end_pos - q_del_pos);
143 templates[template_name] = query_str;
145 input.erase(q_del_pos, tpl_end_pos - q_del_pos);
159 mongo::BufBuilder & bb = facets.subarrayStart(template_name);
160 mongo::BSONArrayBuilder *arrb =
new mongo::BSONArrayBuilder(bb);
161 BSONObjBuilder query;
162 query.append(
"$match", fromjson(query_str));
163 arrb->append(query.obj());
165 #ifdef HAVE_MONGODB_VERSION_H 166 }
catch (mongo::MsgAssertionException &e) {
168 }
catch (mongo::AssertionException &e) {
171 "Template query failed: %s\n%s",
177 BSONObjBuilder aggregate_query;
178 aggregate_query.append(
"$facet", facets.obj());
179 BSONObj aggregate_query_obj(aggregate_query.obj());
180 std::vector<mongo::BSONObj> aggregate_pipeline{aggregate_query_obj};
182 BSONObj result = res.getField(
"result").Obj()[
"0"].Obj();
183 for (BSONObj::iterator i = result.begin(); i.more();) {
184 BSONElement e = i.next();
185 for (BSONObj::iterator j = e.Obj().begin(); j.more();) {
186 BSONElement f = j.next();
187 ctemplate::TemplateDictionary *entry_dict = dict.AddSectionDictionary(e.fieldName());
188 fill_dict_from_document(entry_dict, f.Obj());
193 dict.SetValue(
"GOAL", goal);
196 ctemplate::StringToTemplateCache(
"tpl-cache", input, ctemplate::DO_NOT_STRIP);
197 if (!ctemplate::TemplateNamelist::IsAllSyntaxOkay(ctemplate::DO_NOT_STRIP)) {
199 std::vector<std::string> error_list =
200 ctemplate::TemplateNamelist::GetBadSyntaxList(
false, ctemplate::DO_NOT_STRIP);
201 for (std::string error : error_list) {
207 ctemplate::ExpandTemplate(
"tpl-cache", ctemplate::DO_NOT_STRIP, &dict, &output);
211 std::ofstream ostream(output_path);
212 if (ostream.is_open()) {
213 ostream << output.c_str();
220 gen_if->set_final(
true);
231 PddlRobotMemoryThread::bb_interface_message_received(
Interface * interface,
234 if (message->is_of_type<PddlGenInterface::GenerateMessage>()) {
235 PddlGenInterface::GenerateMessage *msg = (PddlGenInterface::GenerateMessage *)message;
236 gen_if->set_msg_id(msg->id());
237 gen_if->set_final(
false);
239 if (std::string(msg->goal()) !=
"")
243 logger->
log_error(name(),
"Received unknown message of type %s, ignoring", message->type());
255 PddlRobotMemoryThread::fill_dict_from_document(ctemplate::TemplateDictionary *dict,
259 for (BSONObjIterator it = obj.begin(); it.more();) {
260 BSONElement elem = it.next();
261 switch (elem.type()) {
262 case mongo::NumberDouble:
263 dict->SetValue(prefix + elem.fieldName(), std::to_string(elem.Double()));
265 case mongo::String: dict->SetValue(prefix + elem.fieldName(), elem.String());
break;
266 case mongo::Bool: dict->SetValue(prefix + elem.fieldName(), std::to_string(elem.Bool()));
break;
267 case mongo::NumberInt: dict->SetIntValue(prefix + elem.fieldName(), elem.Int());
break;
268 case mongo::NumberLong:
269 dict->SetValue(prefix + elem.fieldName(), std::to_string(elem.Long()));
272 fill_dict_from_document(dict, elem.Obj(), prefix + elem.fieldName() +
"_");
275 dict->SetValue(prefix + elem.fieldName(), elem.OID().toString());
281 for (
size_t i = 0; i < elem.Array().size(); i++) {
282 b.append(elem.Array()[i]);
284 fill_dict_from_document(dict, b.obj(), prefix + elem.fieldName() +
"_");
286 std::string array_string;
287 for (
size_t i = 0; i < elem.Array().size(); i++) {
289 array_string +=
" " + elem.Array()[i].String();
291 dict->SetValue(prefix + elem.fieldName(), array_string);
294 default: dict->SetValue(prefix + elem.fieldName(),
"INVALID_VALUE_TYPE");
Base class for all messages passed through interfaces in Fawkes BlackBoard.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
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.
mongo::BSONObj aggregate(const std::vector< mongo::BSONObj > &pipeline, const std::string &collection="")
Aggregation call on the robot memory.
Thread class encapsulation of pthreads.
virtual void finalize()
Finalize the thread.
Base class for all Fawkes BlackBoard interfaces.
Logger * logger
This is the Logger member used to access the logger.
virtual void loop()
Thread is only waked up if there is a new interface message to generate a pddl.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
void wakeup()
Wake up thread.
const char * name() const
Get name of thread.
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 bool exists(const char *path)=0
Check if a given value exists.
Configuration * config
This is the Configuration member used to access the configuration.
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 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.
virtual void close(Interface *interface)=0
Close interface.