24#include "pddl-planner_thread.h"
26#include <utils/misc/string_conversions.h>
28#include <bsoncxx/builder/basic/document.hpp>
29#include <bsoncxx/json.hpp>
37using namespace mongocxx;
38using namespace bsoncxx;
39using bsoncxx::builder::basic::kvp;
48:
Thread(
"PddlPlannerThread",
Thread::OPMODE_WAITFORWAKEUP),
57 std::string cfg_prefix =
"plugins/pddl-planner/";
58 cfg_descripton_path_ =
59 StringConversions::resolve_path(
config->
get_string((cfg_prefix +
"description-folder")));
60 cfg_result_path_ = cfg_descripton_path_ +
config->
get_string((cfg_prefix +
"result-file"));
61 cfg_domain_path_ = cfg_descripton_path_ +
config->
get_string(cfg_prefix +
"domain-description");
62 cfg_problem_path_ = cfg_descripton_path_ +
config->
get_string(cfg_prefix +
"problem-description");
67 std::string planner_string =
config->
get_string((cfg_prefix +
"planner").c_str());
68 if (planner_string ==
"ff") {
69 planner_ = std::bind(&PddlPlannerThread::ff_planner,
this);
71 }
else if (planner_string ==
"fd") {
72 planner_ = std::bind(&PddlPlannerThread::fd_planner,
this);
74 }
else if (planner_string ==
"dbmp") {
75 planner_ = std::bind(&PddlPlannerThread::dbmp_planner,
this);
78 planner_ = std::bind(&PddlPlannerThread::ff_planner,
this);
85 plan_if_->set_active_planner(planner_string.c_str());
86 plan_if_->set_msg_id(0);
87 plan_if_->set_final(
false);
88 plan_if_->set_success(
false);
111 if (!action_list_.empty()) {
112 auto plan = BSONFromActionList();
118 plan_if_->set_success(
true);
122 builder::basic::make_document(kvp(
"plan", 0)),
125 plan_if_->set_success(
false);
128 plan_if_->set_final(
true);
139PddlPlannerThread::ff_planner()
143 std::string command =
"ff -o " + cfg_domain_path_ +
" -f " + cfg_problem_path_;
145 std::string result = run_planner(command);
150 action_list_.clear();
153 if (result.find(
"found legal plan as follows", cur_pos) == std::string::npos) {
156 builder::basic::make_document(kvp(
"plan", 1),
158 kvp(
"steps", builder::basic::array())),
164 result.erase(result.find(
"time spent:", cur_pos));
166 cur_pos = result.find(
"step", cur_pos) + 4;
167 while (result.find(
": ", cur_pos) != std::string::npos) {
168 cur_pos = result.find(
": ", cur_pos) + 2;
169 size_t line_end = result.find(
"\n", cur_pos);
172 result.substr(cur_pos, line_end - cur_pos).c_str(),
176 if (line_end < result.find(
" ", cur_pos)) {
177 a.name = result.substr(cur_pos, line_end - cur_pos);
179 size_t action_end = result.find(
" ", cur_pos);
180 a.name = StringConversions::to_lower(result.substr(cur_pos, action_end - cur_pos));
181 cur_pos = action_end + 1;
182 while (cur_pos < line_end) {
183 size_t arg_end = result.find(
" ", cur_pos);
184 if (arg_end > line_end) {
187 a.args.push_back(result.substr(cur_pos, arg_end - cur_pos));
188 cur_pos = arg_end + 1;
191 action_list_.push_back(a);
196PddlPlannerThread::dbmp_planner()
200 std::string command =
201 "dbmp.py -p ff --output plan.pddl " + cfg_domain_path_ +
" " + cfg_problem_path_;
203 std::string result = run_planner(command);
209 if (result.find(
"Planner failed", cur_pos) != std::string::npos) {
212 builder::basic::make_document(kvp(
"plan", 1),
214 kvp(
"steps", builder::basic::array())),
219 std::ifstream planfile(
"plan.pddl");
221 action_list_.clear();
222 while (std::getline(planfile, line)) {
223 std::string time_string =
"Time";
224 if (line.compare(0, time_string.size(), time_string) == 0) {
228 if (line[0] !=
'(' || line[line.size() - 1] !=
')') {
233 std::string action_str = line.substr(1, line.size() - 2);
235 cur_pos = action_str.find(
" ", cur_pos + 1);
236 a.name = StringConversions::to_lower(action_str.substr(0, cur_pos));
237 while (cur_pos != std::string::npos) {
238 size_t word_start = cur_pos + 1;
239 cur_pos = action_str.find(
" ", word_start);
240 a.args.push_back(action_str.substr(word_start, cur_pos - word_start));
242 action_list_.push_back(a);
247PddlPlannerThread::fd_planner()
251 std::string command =
252 "fast-downward" + std::string(
" ") + cfg_domain_path_ + std::string(
" ") + cfg_problem_path_;
254 if (!cfg_fd_options_.empty()) {
255 command += std::string(
" ") + cfg_fd_options_;
258 std::string result = run_planner(command);
261 std::remove(
"output");
262 std::remove(
"output.sas");
265 if (result.find(
"Solution found!", cur_pos) == std::string::npos) {
269 cur_pos = result.find(
"Solution found!", cur_pos);
270 cur_pos = result.find(
"\n", cur_pos);
271 cur_pos = result.find(
"\n", cur_pos + 1);
274 result.erase(0, cur_pos);
275 size_t end_pos = result.find(
"Plan length: ");
276 result.erase(end_pos, result.size() - 1);
278 std::istringstream iss(result);
282 while (getline(iss, line)) {
284 a.name = line.substr(0, find_nth_space(line, 1));
285 if (find_nth_space(line, 2) != line.rfind(
' ') + 1) {
286 std::stringstream ss(
287 line.substr(find_nth_space(line, 2), line.rfind(
' ') - find_nth_space(line, 2)));
289 while (getline(ss, item,
' ')) {
290 a.args.push_back(item);
293 action_list_.push_back(a);
298PddlPlannerThread::BSONFromActionList()
300 using namespace bsoncxx::builder;
301 basic::document plan;
302 plan.append(basic::kvp(
"plan", 1));
303 plan.append(basic::kvp(
"msg_id",
static_cast<int64_t
>(plan_if_->msg_id())));
304 plan.append(basic::kvp(
"actions", [&](basic::sub_array actions) {
305 for (action &a : action_list_) {
306 basic::document action;
307 action.append(basic::kvp(
"name", a.name));
308 action.append(basic::kvp(
"args", [a](basic::sub_array args) {
309 for (std::string arg : a.args) {
313 actions.append(action);
317 return plan.extract();
321PddlPlannerThread::find_nth_space(
const std::string &s,
size_t nth)
324 unsigned occurrence = 0;
326 while (occurrence != nth && (pos = s.find(
' ', pos + 1)) != std::string::npos) {
334PddlPlannerThread::print_action_list()
336 unsigned int count = 0;
337 for (action a : action_list_) {
340 for (std::string arg : a.args) {
343 logger->
log_info(
name(),
"Action %d %s with args %s", count, a.name.c_str(), args.c_str());
348PddlPlannerThread::run_planner(std::string command)
351 std::shared_ptr<FILE> pipe(popen(command.c_str(),
"r"), pclose);
353 throw std::runtime_error(
"popen() failed!");
356 while (!feof(pipe.get())) {
357 if (fgets(buffer, 128, pipe.get()) != NULL)
366PddlPlannerThread::bb_interface_message_received(
Interface * interface,
369 if (message->is_of_type<PddlPlannerInterface::PlanMessage>()) {
370 PddlPlannerInterface::PlanMessage *msg = (PddlPlannerInterface::PlanMessage *)message;
371 plan_if_->set_msg_id(msg->id());
372 plan_if_->set_success(
false);
373 plan_if_->set_final(
false);
377 logger->
log_error(name(),
"Received unknown message of type %s, ignoring", message->type());
PddlPlannerThread()
Constructor.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Thread is only waked up if there was a new interface message to plan.
int update(const bsoncxx::document::view &query, const bsoncxx::document::view &update, const std::string &collection="", bool upsert=false)
Updates documents in the robot memory.
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 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_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.
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 set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Fawkes library namespace.