24 #include "pddl-planner_thread.h" 26 #include <utils/misc/string_conversions.h> 35 using namespace mongo;
44 :
Thread(
"PddlPlannerThread",
Thread::OPMODE_WAITFORWAKEUP),
53 std::string cfg_prefix =
"plugins/pddl-planner/";
54 cfg_descripton_path_ =
55 StringConversions::resolve_path(
config->
get_string((cfg_prefix +
"description-folder")));
56 cfg_result_path_ = cfg_descripton_path_ +
config->
get_string((cfg_prefix +
"result-file"));
57 cfg_domain_path_ = cfg_descripton_path_ +
config->
get_string(cfg_prefix +
"domain-description");
58 cfg_problem_path_ = cfg_descripton_path_ +
config->
get_string(cfg_prefix +
"problem-description");
63 std::string planner_string =
config->
get_string((cfg_prefix +
"planner").c_str());
64 if (planner_string ==
"ff") {
65 planner_ = std::bind(&PddlPlannerThread::ff_planner,
this);
67 }
else if (planner_string ==
"fd") {
68 planner_ = std::bind(&PddlPlannerThread::fd_planner,
this);
70 }
else if (planner_string ==
"dbmp") {
71 planner_ = std::bind(&PddlPlannerThread::dbmp_planner,
this);
74 planner_ = std::bind(&PddlPlannerThread::ff_planner,
this);
81 plan_if_->set_active_planner(planner_string.c_str());
82 plan_if_->set_msg_id(0);
83 plan_if_->set_final(
false);
84 plan_if_->set_success(
false);
107 if (!action_list_.empty()) {
108 BSONObj plan = BSONFromActionList();
111 plan_if_->set_success(
true);
115 fromjson(
"{plan:0}"),
118 plan_if_->set_success(
false);
121 plan_if_->set_final(
true);
132 PddlPlannerThread::ff_planner()
136 std::string command =
"ff -o " + cfg_domain_path_ +
" -f " + cfg_problem_path_;
138 std::string result = run_planner(command);
143 action_list_.clear();
146 if (result.find(
"found legal plan as follows", cur_pos) == std::string::npos) {
149 fromjson(
"{plan:1,fail:1,steps:[]}"),
155 result.erase(result.find(
"time spent:", cur_pos));
157 cur_pos = result.find(
"step", cur_pos) + 4;
158 while (result.find(
": ", cur_pos) != std::string::npos) {
159 cur_pos = result.find(
": ", cur_pos) + 2;
160 size_t line_end = result.find(
"\n", cur_pos);
163 result.substr(cur_pos, line_end - cur_pos).c_str(),
167 if (line_end < result.find(
" ", cur_pos)) {
168 a.name = result.substr(cur_pos, line_end - cur_pos);
170 size_t action_end = result.find(
" ", cur_pos);
171 a.name = StringConversions::to_lower(result.substr(cur_pos, action_end - cur_pos));
172 cur_pos = action_end + 1;
173 while (cur_pos < line_end) {
174 size_t arg_end = result.find(
" ", cur_pos);
175 if (arg_end > line_end) {
178 a.args.push_back(result.substr(cur_pos, arg_end - cur_pos));
179 cur_pos = arg_end + 1;
182 action_list_.push_back(a);
187 PddlPlannerThread::dbmp_planner()
191 std::string command =
192 "dbmp.py -p ff --output plan.pddl " + cfg_domain_path_ +
" " + cfg_problem_path_;
194 std::string result = run_planner(command);
200 if (result.find(
"Planner failed", cur_pos) != std::string::npos) {
203 fromjson(
"{plan:1,fail:1,steps:[]}"),
208 std::ifstream planfile(
"plan.pddl");
210 action_list_.clear();
211 while (std::getline(planfile, line)) {
212 std::string time_string =
"Time";
213 if (line.compare(0, time_string.size(), time_string) == 0) {
217 if (line[0] !=
'(' || line[line.size() - 1] !=
')') {
222 std::string action_str = line.substr(1, line.size() - 2);
224 cur_pos = action_str.find(
" ", cur_pos + 1);
225 a.name = StringConversions::to_lower(action_str.substr(0, cur_pos));
226 while (cur_pos != std::string::npos) {
227 size_t word_start = cur_pos + 1;
228 cur_pos = action_str.find(
" ", word_start);
229 a.args.push_back(action_str.substr(word_start, cur_pos - word_start));
231 action_list_.push_back(a);
236 PddlPlannerThread::fd_planner()
240 std::string command =
241 "fast-downward" + std::string(
" ") + cfg_domain_path_ + std::string(
" ") + cfg_problem_path_;
243 if (!cfg_fd_options_.empty()) {
244 command += std::string(
" ") + cfg_fd_options_;
247 std::string result = run_planner(command);
250 std::remove(
"output");
251 std::remove(
"output.sas");
254 if (result.find(
"Solution found!", cur_pos) == std::string::npos) {
258 cur_pos = result.find(
"Solution found!", cur_pos);
259 cur_pos = result.find(
"\n", cur_pos);
260 cur_pos = result.find(
"\n", cur_pos + 1);
263 result.erase(0, cur_pos);
264 size_t end_pos = result.find(
"Plan length: ");
265 result.erase(end_pos, result.size() - 1);
267 std::istringstream iss(result);
271 while (getline(iss, line)) {
273 a.name = line.substr(0, find_nth_space(line, 1));
274 if (find_nth_space(line, 2) != line.rfind(
' ') + 1) {
275 std::stringstream ss(
276 line.substr(find_nth_space(line, 2), line.rfind(
' ') - find_nth_space(line, 2)));
278 while (getline(ss, item,
' ')) {
279 a.args.push_back(item);
282 action_list_.push_back(a);
287 PddlPlannerThread::BSONFromActionList()
289 BSONObjBuilder plan_builder;
290 plan_builder <<
"plan" << 1;
291 plan_builder <<
"msg_id" << plan_if_->msg_id();
292 BSONArrayBuilder action_arr_builder;
293 for (action a : action_list_) {
294 BSONObjBuilder action_builder;
295 action_builder <<
"name" << a.name;
296 BSONArrayBuilder args_builder;
297 for (std::string args : a.args) {
298 args_builder << args;
300 action_builder <<
"args" << args_builder.arr();
301 action_arr_builder << action_builder.obj();
304 plan_builder <<
"actions" << action_arr_builder.arr();
306 return plan_builder.obj();
310 PddlPlannerThread::find_nth_space(
const std::string &s,
size_t nth)
313 unsigned occurrence = 0;
315 while (occurrence != nth && (pos = s.find(
' ', pos + 1)) != std::string::npos) {
323 PddlPlannerThread::print_action_list()
325 unsigned int count = 0;
326 for (action a : action_list_) {
329 for (std::string arg : a.args) {
332 logger->
log_info(
name(),
"Action %d %s with args %s", count, a.name.c_str(), args.c_str());
337 PddlPlannerThread::run_planner(std::string command)
340 std::shared_ptr<FILE> pipe(popen(command.c_str(),
"r"), pclose);
342 throw std::runtime_error(
"popen() failed!");
345 while (!feof(pipe.get())) {
346 if (fgets(buffer, 128, pipe.get()) != NULL)
355 PddlPlannerThread::bb_interface_message_received(
Interface * interface,
358 if (message->is_of_type<PddlPlannerInterface::PlanMessage>()) {
359 PddlPlannerInterface::PlanMessage *msg = (PddlPlannerInterface::PlanMessage *)message;
360 plan_if_->set_msg_id(msg->id());
361 plan_if_->set_success(
false);
362 plan_if_->set_final(
false);
366 logger->
log_error(name(),
"Received unknown message of type %s, ignoring", message->type());
virtual void init()
Initialize the thread.
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 log_error(const char *component, const char *format,...)
Log error message.
Fawkes library namespace.
int update(mongo::Query query, mongo::BSONObj update, const std::string &collection="", bool upsert=false)
Updates documents in the robot memory.
Thread class encapsulation of pthreads.
Base class for all Fawkes BlackBoard interfaces.
Logger * logger
This is the Logger member used to access the logger.
PddlPlannerThread()
Constructor.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Base class for exceptions in Fawkes.
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.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
virtual void finalize()
Finalize the thread.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
virtual void loop()
Thread is only waked up if there was a new interface message to plan.
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.