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>
37 using namespace mongocxx;
38 using namespace bsoncxx;
47 :
Thread(
"PddlPlannerThread",
Thread::OPMODE_WAITFORWAKEUP),
56 std::string cfg_prefix =
"plugins/pddl-planner/";
57 cfg_descripton_path_ =
58 StringConversions::resolve_path(
config->
get_string((cfg_prefix +
"description-folder")));
59 cfg_result_path_ = cfg_descripton_path_ +
config->
get_string((cfg_prefix +
"result-file"));
60 cfg_domain_path_ = cfg_descripton_path_ +
config->
get_string(cfg_prefix +
"domain-description");
61 cfg_problem_path_ = cfg_descripton_path_ +
config->
get_string(cfg_prefix +
"problem-description");
66 std::string planner_string =
config->
get_string((cfg_prefix +
"planner").c_str());
67 if (planner_string ==
"ff") {
68 planner_ = std::bind(&PddlPlannerThread::ff_planner,
this);
70 }
else if (planner_string ==
"fd") {
71 planner_ = std::bind(&PddlPlannerThread::fd_planner,
this);
73 }
else if (planner_string ==
"dbmp") {
74 planner_ = std::bind(&PddlPlannerThread::dbmp_planner,
this);
77 planner_ = std::bind(&PddlPlannerThread::ff_planner,
this);
84 plan_if_->set_active_planner(planner_string.c_str());
85 plan_if_->set_msg_id(0);
86 plan_if_->set_final(
false);
87 plan_if_->set_success(
false);
110 if (!action_list_.empty()) {
111 auto plan = BSONFromActionList();
112 robot_memory->
update(from_json(
"{plan:{$exists:true}}").view(), plan, cfg_collection_,
true);
114 plan_if_->set_success(
true);
118 from_json(
"{plan:0}").view(),
121 plan_if_->set_success(
false);
124 plan_if_->set_final(
true);
135 PddlPlannerThread::ff_planner()
139 std::string command =
"ff -o " + cfg_domain_path_ +
" -f " + cfg_problem_path_;
141 std::string result = run_planner(command);
146 action_list_.clear();
149 if (result.find(
"found legal plan as follows", cur_pos) == std::string::npos) {
152 from_json(
"{plan:1,fail:1,steps:[]}").view(),
158 result.erase(result.find(
"time spent:", cur_pos));
160 cur_pos = result.find(
"step", cur_pos) + 4;
161 while (result.find(
": ", cur_pos) != std::string::npos) {
162 cur_pos = result.find(
": ", cur_pos) + 2;
163 size_t line_end = result.find(
"\n", cur_pos);
166 result.substr(cur_pos, line_end - cur_pos).c_str(),
170 if (line_end < result.find(
" ", cur_pos)) {
171 a.name = result.substr(cur_pos, line_end - cur_pos);
173 size_t action_end = result.find(
" ", cur_pos);
174 a.name = StringConversions::to_lower(result.substr(cur_pos, action_end - cur_pos));
175 cur_pos = action_end + 1;
176 while (cur_pos < line_end) {
177 size_t arg_end = result.find(
" ", cur_pos);
178 if (arg_end > line_end) {
181 a.args.push_back(result.substr(cur_pos, arg_end - cur_pos));
182 cur_pos = arg_end + 1;
185 action_list_.push_back(a);
190 PddlPlannerThread::dbmp_planner()
194 std::string command =
195 "dbmp.py -p ff --output plan.pddl " + cfg_domain_path_ +
" " + cfg_problem_path_;
197 std::string result = run_planner(command);
203 if (result.find(
"Planner failed", cur_pos) != std::string::npos) {
206 from_json(
"{plan:1,fail:1,steps:[]}").view(),
211 std::ifstream planfile(
"plan.pddl");
213 action_list_.clear();
214 while (std::getline(planfile, line)) {
215 std::string time_string =
"Time";
216 if (line.compare(0, time_string.size(), time_string) == 0) {
220 if (line[0] !=
'(' || line[line.size() - 1] !=
')') {
225 std::string action_str = line.substr(1, line.size() - 2);
227 cur_pos = action_str.find(
" ", cur_pos + 1);
228 a.name = StringConversions::to_lower(action_str.substr(0, cur_pos));
229 while (cur_pos != std::string::npos) {
230 size_t word_start = cur_pos + 1;
231 cur_pos = action_str.find(
" ", word_start);
232 a.args.push_back(action_str.substr(word_start, cur_pos - word_start));
234 action_list_.push_back(a);
239 PddlPlannerThread::fd_planner()
243 std::string command =
244 "fast-downward" + std::string(
" ") + cfg_domain_path_ + std::string(
" ") + cfg_problem_path_;
246 if (!cfg_fd_options_.empty()) {
247 command += std::string(
" ") + cfg_fd_options_;
250 std::string result = run_planner(command);
253 std::remove(
"output");
254 std::remove(
"output.sas");
257 if (result.find(
"Solution found!", cur_pos) == std::string::npos) {
261 cur_pos = result.find(
"Solution found!", cur_pos);
262 cur_pos = result.find(
"\n", cur_pos);
263 cur_pos = result.find(
"\n", cur_pos + 1);
266 result.erase(0, cur_pos);
267 size_t end_pos = result.find(
"Plan length: ");
268 result.erase(end_pos, result.size() - 1);
270 std::istringstream iss(result);
274 while (getline(iss, line)) {
276 a.name = line.substr(0, find_nth_space(line, 1));
277 if (find_nth_space(line, 2) != line.rfind(
' ') + 1) {
278 std::stringstream ss(
279 line.substr(find_nth_space(line, 2), line.rfind(
' ') - find_nth_space(line, 2)));
281 while (getline(ss, item,
' ')) {
282 a.args.push_back(item);
285 action_list_.push_back(a);
290 PddlPlannerThread::BSONFromActionList()
292 using namespace bsoncxx::builder;
293 basic::document plan;
294 plan.append(basic::kvp(
"plan", 1));
295 plan.append(basic::kvp(
"msg_id", static_cast<int64_t>(plan_if_->msg_id())));
296 plan.append(basic::kvp(
"actions", [&](basic::sub_array actions) {
297 for (action &a : action_list_) {
298 basic::document action;
299 action.append(basic::kvp(
"name", a.name));
300 action.append(basic::kvp(
"args", [a](basic::sub_array args) {
301 for (std::string arg : a.args) {
308 return plan.extract();
312 PddlPlannerThread::find_nth_space(
const std::string &s,
size_t nth)
315 unsigned occurrence = 0;
317 while (occurrence != nth && (pos = s.find(
' ', pos + 1)) != std::string::npos) {
325 PddlPlannerThread::print_action_list()
327 unsigned int count = 0;
328 for (action a : action_list_) {
331 for (std::string arg : a.args) {
334 logger->
log_info(
name(),
"Action %d %s with args %s", count, a.name.c_str(), args.c_str());
339 PddlPlannerThread::run_planner(std::string command)
342 std::shared_ptr<FILE> pipe(popen(command.c_str(),
"r"), pclose);
344 throw std::runtime_error(
"popen() failed!");
347 while (!feof(pipe.get())) {
348 if (fgets(buffer, 128, pipe.get()) != NULL)
357 PddlPlannerThread::bb_interface_message_received(
Interface * interface,
360 if (message->is_of_type<PddlPlannerInterface::PlanMessage>()) {
361 PddlPlannerInterface::PlanMessage *msg = (PddlPlannerInterface::PlanMessage *)message;
362 plan_if_->set_msg_id(msg->id());
363 plan_if_->set_success(
false);
364 plan_if_->set_final(
false);
368 logger->
log_error(
name(),
"Received unknown message of type %s, ignoring", message->type());