Fawkes API  Fawkes Development Version
pddl-planner_thread.cpp
1 
2 /***************************************************************************
3  * pddl-planner_thread.cpp - pddl-planner
4  *
5  * Created: Wed Dec 7 19:09:44 2016
6  * Copyright 2016 Frederik Zwilling
7  * 2017 Matthias Loebach
8  * 2017 Till Hofmann
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "pddl-planner_thread.h"
25 
26 #include <utils/misc/string_conversions.h>
27 
28 #include <fstream>
29 #include <iostream>
30 #include <sstream>
31 #include <stdio.h>
32 #include <stdlib.h>
33 
34 using namespace fawkes;
35 using namespace mongo;
36 
37 /** @class PddlPlannerThread 'pddl-planner_thread.h'
38  * Starts a pddl planner and writes the resulting plan into the robot memory
39  * @author Frederik Zwilling
40  */
41 
42 /** Constructor. */
44 : Thread("PddlPlannerThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlackBoardInterfaceListener("PddlPlannerThread")
46 {
47 }
48 
49 void
51 {
52  //read config
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");
59  cfg_fd_options_ = config->get_string(cfg_prefix + "fd-search-opts");
60  cfg_collection_ = config->get_string(cfg_prefix + "collection");
61 
62  //set configured planner
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);
66  logger->log_info(name(), "Fast-Forward planner selected.");
67  } else if (planner_string == "fd") {
68  planner_ = std::bind(&PddlPlannerThread::fd_planner, this);
69  logger->log_info(name(), "Fast-Downward planner selected.");
70  } else if (planner_string == "dbmp") {
71  planner_ = std::bind(&PddlPlannerThread::dbmp_planner, this);
72  logger->log_info(name(), "DBMP selected.");
73  } else {
74  planner_ = std::bind(&PddlPlannerThread::ff_planner, this);
75  logger->log_warn(name(), "No planner configured.\nDefaulting to ff.");
76  }
77 
78  //setup interface
79  plan_if_ = blackboard->open_for_writing<PddlPlannerInterface>(
80  config->get_string(cfg_prefix + "interface-name").c_str());
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);
85  plan_if_->write();
86 
87  //setup interface listener
89  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
90 
91  // If we receive multiple wakeup() calls during loop, only call the loop once afterwards
92  // We want to only re-plan once after a loop run since multiple runs would plan on the same problem
93  this->set_coalesce_wakeups(true);
94 }
95 
96 /**
97  * Thread is only waked up if there was a new interface message to plan
98  */
99 void
101 {
102  logger->log_info(name(), "Starting PDDL Planning...");
103 
104  //writes plan into action_list_
105  planner_();
106 
107  if (!action_list_.empty()) {
108  BSONObj plan = BSONFromActionList();
109  robot_memory->update(fromjson("{plan:{$exists:true}}"), plan, cfg_collection_, true);
110  print_action_list();
111  plan_if_->set_success(true);
112  } else {
113  logger->log_error(name(), "Updating plan failed, action list empty!");
114  robot_memory->update(fromjson("{plan:{$exists:true}}"),
115  fromjson("{plan:0}"),
116  cfg_collection_,
117  true);
118  plan_if_->set_success(false);
119  }
120 
121  plan_if_->set_final(true);
122  plan_if_->write();
123 }
124 
125 void
127 {
128  blackboard->close(plan_if_);
129 }
130 
131 void
132 PddlPlannerThread::ff_planner()
133 {
134  logger->log_info(name(), "Starting PDDL Planning with Fast-Forward...");
135 
136  std::string command = "ff -o " + cfg_domain_path_ + " -f " + cfg_problem_path_;
137  logger->log_info(name(), "Calling %s", command.c_str());
138  std::string result = run_planner(command);
139 
140  //Parse Result and write it into the robot memory
141  logger->log_info(name(), "Parsing result");
142 
143  action_list_.clear();
144 
145  size_t cur_pos = 0;
146  if (result.find("found legal plan as follows", cur_pos) == std::string::npos) {
147  logger->log_error(name(), "Planning Failed: %s", result.c_str());
148  robot_memory->update(fromjson("{plan:{$exists:true}}"),
149  fromjson("{plan:1,fail:1,steps:[]}"),
150  cfg_collection_,
151  true);
152  return;
153  }
154  //remove stuff that could confuse us later
155  result.erase(result.find("time spent:", cur_pos));
156 
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);
161  logger->log_info(name(),
162  "line:%s (%zu-%zu)",
163  result.substr(cur_pos, line_end - cur_pos).c_str(),
164  cur_pos,
165  line_end);
166  action a;
167  if (line_end < result.find(" ", cur_pos)) {
168  a.name = result.substr(cur_pos, line_end - cur_pos);
169  } else {
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) {
176  arg_end = line_end;
177  }
178  a.args.push_back(result.substr(cur_pos, arg_end - cur_pos));
179  cur_pos = arg_end + 1;
180  }
181  }
182  action_list_.push_back(a);
183  }
184 }
185 
186 void
187 PddlPlannerThread::dbmp_planner()
188 {
189  logger->log_info(name(), "Starting PDDL Planning with DBMP...");
190 
191  std::string command =
192  "dbmp.py -p ff --output plan.pddl " + cfg_domain_path_ + " " + cfg_problem_path_;
193  logger->log_info(name(), "Calling %s", command.c_str());
194  std::string result = run_planner(command);
195 
196  //Parse Result and write it into the robot memory
197  logger->log_info(name(), "Parsing result");
198 
199  size_t cur_pos = 0;
200  if (result.find("Planner failed", cur_pos) != std::string::npos) {
201  logger->log_error(name(), "Planning Failed: %s", result.c_str());
202  robot_memory->update(fromjson("{plan:{$exists:true}}"),
203  fromjson("{plan:1,fail:1,steps:[]}"),
204  cfg_collection_,
205  true);
206  return;
207  }
208  std::ifstream planfile("plan.pddl");
209  std::string line;
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) {
214  // makespan, skip
215  continue;
216  }
217  if (line[0] != '(' || line[line.size() - 1] != ')') {
218  logger->log_error(name(), "Expected parantheses in line '%s'!", line.c_str());
219  return;
220  }
221  // remove parantheses
222  std::string action_str = line.substr(1, line.size() - 2);
223  action a;
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));
230  }
231  action_list_.push_back(a);
232  }
233 }
234 
235 void
236 PddlPlannerThread::fd_planner()
237 {
238  logger->log_info(name(), "Starting PDDL Planning with Fast-Downward...");
239 
240  std::string command =
241  "fast-downward" + std::string(" ") + cfg_domain_path_ + std::string(" ") + cfg_problem_path_;
242 
243  if (!cfg_fd_options_.empty()) {
244  command += std::string(" ") + cfg_fd_options_;
245  }
246 
247  std::string result = run_planner(command);
248 
249  logger->log_info(name(), "Removing temporary planner output.");
250  std::remove("output");
251  std::remove("output.sas");
252 
253  size_t cur_pos = 0;
254  if (result.find("Solution found!", cur_pos) == std::string::npos) {
255  logger->log_error(name(), "Planning Failed: %s", result.c_str());
256  throw Exception("No solution found");
257  } else {
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);
261  logger->log_info(name(), "Planner found solution.");
262  }
263  result.erase(0, cur_pos);
264  size_t end_pos = result.find("Plan length: ");
265  result.erase(end_pos, result.size() - 1);
266 
267  std::istringstream iss(result);
268  std::string line;
269  // remove surplus line
270  getline(iss, line);
271  while (getline(iss, line)) {
272  action a;
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)));
277  std::string item;
278  while (getline(ss, item, ' ')) {
279  a.args.push_back(item);
280  }
281  }
282  action_list_.push_back(a);
283  }
284 }
285 
286 BSONObj
287 PddlPlannerThread::BSONFromActionList()
288 {
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;
299  }
300  action_builder << "args" << args_builder.arr();
301  action_arr_builder << action_builder.obj();
302  }
303 
304  plan_builder << "actions" << action_arr_builder.arr();
305 
306  return plan_builder.obj();
307 }
308 
309 size_t
310 PddlPlannerThread::find_nth_space(const std::string &s, size_t nth)
311 {
312  size_t pos = 0;
313  unsigned occurrence = 0;
314 
315  while (occurrence != nth && (pos = s.find(' ', pos + 1)) != std::string::npos) {
316  ++occurrence;
317  }
318 
319  return pos + 1;
320 }
321 
322 void
323 PddlPlannerThread::print_action_list()
324 {
325  unsigned int count = 0;
326  for (action a : action_list_) {
327  count++;
328  std::string args;
329  for (std::string arg : a.args) {
330  args += arg + " ";
331  }
332  logger->log_info(name(), "Action %d %s with args %s", count, a.name.c_str(), args.c_str());
333  }
334 }
335 
336 std::string
337 PddlPlannerThread::run_planner(std::string command)
338 {
339  logger->log_info(name(), "Running planner with command: %s", command.c_str());
340  std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
341  if (!pipe)
342  throw std::runtime_error("popen() failed!");
343  char buffer[128];
344  std::string result;
345  while (!feof(pipe.get())) {
346  if (fgets(buffer, 128, pipe.get()) != NULL)
347  result += buffer;
348  }
349  logger->log_info(name(), "Planner finished run.");
350 
351  return result;
352 }
353 
354 bool
355 PddlPlannerThread::bb_interface_message_received(Interface * interface,
356  fawkes::Message *message) throw()
357 {
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);
363  plan_if_->write();
364  wakeup(); //activates loop where the generation is done
365  } else {
366  logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
367  }
368  return false;
369 }
virtual void init()
Initialize the thread.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
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.
Definition: multi.cpp:237
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.
Definition: thread.h:45
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
PddlPlannerThread()
Constructor.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
Base class for exceptions in Fawkes.
Definition: exception.h:35
const char * name() const
Get name of thread.
Definition: thread.h:100
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.
Definition: thread.cpp:729
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.
Definition: configurable.h:41
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.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.