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 <bsoncxx/builder/basic/document.hpp>
29 #include <bsoncxx/json.hpp>
30 #include <fstream>
31 #include <iostream>
32 #include <sstream>
33 #include <stdio.h>
34 #include <stdlib.h>
35 
36 using namespace fawkes;
37 using namespace mongocxx;
38 using namespace bsoncxx;
39 using bsoncxx::builder::basic::kvp;
40 
41 /** @class PddlPlannerThread 'pddl-planner_thread.h'
42  * Starts a pddl planner and writes the resulting plan into the robot memory
43  * @author Frederik Zwilling
44  */
45 
46 /** Constructor. */
48 : Thread("PddlPlannerThread", Thread::OPMODE_WAITFORWAKEUP),
49  BlackBoardInterfaceListener("PddlPlannerThread")
50 {
51 }
52 
53 void
55 {
56  //read config
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");
63  cfg_fd_options_ = config->get_string(cfg_prefix + "fd-search-opts");
64  cfg_collection_ = config->get_string(cfg_prefix + "collection");
65 
66  //set configured planner
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);
70  logger->log_info(name(), "Fast-Forward planner selected.");
71  } else if (planner_string == "fd") {
72  planner_ = std::bind(&PddlPlannerThread::fd_planner, this);
73  logger->log_info(name(), "Fast-Downward planner selected.");
74  } else if (planner_string == "dbmp") {
75  planner_ = std::bind(&PddlPlannerThread::dbmp_planner, this);
76  logger->log_info(name(), "DBMP selected.");
77  } else {
78  planner_ = std::bind(&PddlPlannerThread::ff_planner, this);
79  logger->log_warn(name(), "No planner configured.\nDefaulting to ff.");
80  }
81 
82  //setup interface
83  plan_if_ = blackboard->open_for_writing<PddlPlannerInterface>(
84  config->get_string(cfg_prefix + "interface-name").c_str());
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);
89  plan_if_->write();
90 
91  //setup interface listener
93  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
94 
95  // If we receive multiple wakeup() calls during loop, only call the loop once afterwards
96  // We want to only re-plan once after a loop run since multiple runs would plan on the same problem
97  this->set_coalesce_wakeups(true);
98 }
99 
100 /**
101  * Thread is only waked up if there was a new interface message to plan
102  */
103 void
105 {
106  logger->log_info(name(), "Starting PDDL Planning...");
107 
108  //writes plan into action_list_
109  planner_();
110 
111  if (!action_list_.empty()) {
112  auto plan = BSONFromActionList();
113  robot_memory->update(builder::basic::make_document(kvp("plan", "{$exists:true}")),
114  plan,
115  cfg_collection_,
116  true);
117  print_action_list();
118  plan_if_->set_success(true);
119  } else {
120  logger->log_error(name(), "Updating plan failed, action list empty!");
121  robot_memory->update(builder::basic::make_document(kvp("plan", "{$exists:true}")),
122  builder::basic::make_document(kvp("plan", 0)),
123  cfg_collection_,
124  true);
125  plan_if_->set_success(false);
126  }
127 
128  plan_if_->set_final(true);
129  plan_if_->write();
130 }
131 
132 void
134 {
135  blackboard->close(plan_if_);
136 }
137 
138 void
139 PddlPlannerThread::ff_planner()
140 {
141  logger->log_info(name(), "Starting PDDL Planning with Fast-Forward...");
142 
143  std::string command = "ff -o " + cfg_domain_path_ + " -f " + cfg_problem_path_;
144  logger->log_info(name(), "Calling %s", command.c_str());
145  std::string result = run_planner(command);
146 
147  //Parse Result and write it into the robot memory
148  logger->log_info(name(), "Parsing result");
149 
150  action_list_.clear();
151 
152  size_t cur_pos = 0;
153  if (result.find("found legal plan as follows", cur_pos) == std::string::npos) {
154  logger->log_error(name(), "Planning Failed: %s", result.c_str());
155  robot_memory->update(builder::basic::make_document(kvp("plan", "{$exists:true}")),
156  builder::basic::make_document(kvp("plan", 1),
157  kvp("fail", 1),
158  kvp("steps", builder::basic::array())),
159  cfg_collection_,
160  true);
161  return;
162  }
163  //remove stuff that could confuse us later
164  result.erase(result.find("time spent:", cur_pos));
165 
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);
170  logger->log_info(name(),
171  "line:%s (%zu-%zu)",
172  result.substr(cur_pos, line_end - cur_pos).c_str(),
173  cur_pos,
174  line_end);
175  action a;
176  if (line_end < result.find(" ", cur_pos)) {
177  a.name = result.substr(cur_pos, line_end - cur_pos);
178  } else {
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) {
185  arg_end = line_end;
186  }
187  a.args.push_back(result.substr(cur_pos, arg_end - cur_pos));
188  cur_pos = arg_end + 1;
189  }
190  }
191  action_list_.push_back(a);
192  }
193 }
194 
195 void
196 PddlPlannerThread::dbmp_planner()
197 {
198  logger->log_info(name(), "Starting PDDL Planning with DBMP...");
199 
200  std::string command =
201  "dbmp.py -p ff --output plan.pddl " + cfg_domain_path_ + " " + cfg_problem_path_;
202  logger->log_info(name(), "Calling %s", command.c_str());
203  std::string result = run_planner(command);
204 
205  //Parse Result and write it into the robot memory
206  logger->log_info(name(), "Parsing result");
207 
208  size_t cur_pos = 0;
209  if (result.find("Planner failed", cur_pos) != std::string::npos) {
210  logger->log_error(name(), "Planning Failed: %s", result.c_str());
211  robot_memory->update(builder::basic::make_document(kvp("plan", "{$exists:true}")),
212  builder::basic::make_document(kvp("plan", 1),
213  kvp("fail", 1),
214  kvp("steps", builder::basic::array())),
215  cfg_collection_,
216  true);
217  return;
218  }
219  std::ifstream planfile("plan.pddl");
220  std::string line;
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) {
225  // makespan, skip
226  continue;
227  }
228  if (line[0] != '(' || line[line.size() - 1] != ')') {
229  logger->log_error(name(), "Expected parantheses in line '%s'!", line.c_str());
230  return;
231  }
232  // remove parantheses
233  std::string action_str = line.substr(1, line.size() - 2);
234  action a;
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));
241  }
242  action_list_.push_back(a);
243  }
244 }
245 
246 void
247 PddlPlannerThread::fd_planner()
248 {
249  logger->log_info(name(), "Starting PDDL Planning with Fast-Downward...");
250 
251  std::string command =
252  "fast-downward" + std::string(" ") + cfg_domain_path_ + std::string(" ") + cfg_problem_path_;
253 
254  if (!cfg_fd_options_.empty()) {
255  command += std::string(" ") + cfg_fd_options_;
256  }
257 
258  std::string result = run_planner(command);
259 
260  logger->log_info(name(), "Removing temporary planner output.");
261  std::remove("output");
262  std::remove("output.sas");
263 
264  size_t cur_pos = 0;
265  if (result.find("Solution found!", cur_pos) == std::string::npos) {
266  logger->log_error(name(), "Planning Failed: %s", result.c_str());
267  throw Exception("No solution found");
268  } else {
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);
272  logger->log_info(name(), "Planner found solution.");
273  }
274  result.erase(0, cur_pos);
275  size_t end_pos = result.find("Plan length: ");
276  result.erase(end_pos, result.size() - 1);
277 
278  std::istringstream iss(result);
279  std::string line;
280  // remove surplus line
281  getline(iss, line);
282  while (getline(iss, line)) {
283  action a;
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)));
288  std::string item;
289  while (getline(ss, item, ' ')) {
290  a.args.push_back(item);
291  }
292  }
293  action_list_.push_back(a);
294  }
295 }
296 
297 document::value
298 PddlPlannerThread::BSONFromActionList()
299 {
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) {
310  args.append(arg);
311  }
312  }));
313  actions.append(action);
314  }
315  }));
316 
317  return plan.extract();
318 }
319 
320 size_t
321 PddlPlannerThread::find_nth_space(const std::string &s, size_t nth)
322 {
323  size_t pos = 0;
324  unsigned occurrence = 0;
325 
326  while (occurrence != nth && (pos = s.find(' ', pos + 1)) != std::string::npos) {
327  ++occurrence;
328  }
329 
330  return pos + 1;
331 }
332 
333 void
334 PddlPlannerThread::print_action_list()
335 {
336  unsigned int count = 0;
337  for (action a : action_list_) {
338  count++;
339  std::string args;
340  for (std::string arg : a.args) {
341  args += arg + " ";
342  }
343  logger->log_info(name(), "Action %d %s with args %s", count, a.name.c_str(), args.c_str());
344  }
345 }
346 
347 std::string
348 PddlPlannerThread::run_planner(std::string command)
349 {
350  logger->log_info(name(), "Running planner with command: %s", command.c_str());
351  std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
352  if (!pipe)
353  throw std::runtime_error("popen() failed!");
354  char buffer[128];
355  std::string result;
356  while (!feof(pipe.get())) {
357  if (fgets(buffer, 128, pipe.get()) != NULL)
358  result += buffer;
359  }
360  logger->log_info(name(), "Planner finished run.");
361 
362  return result;
363 }
364 
365 bool
366 PddlPlannerThread::bb_interface_message_received(Interface * interface,
367  fawkes::Message *message) noexcept
368 {
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);
374  plan_if_->write();
375  wakeup(); //activates loop where the generation is done
376  } else {
377  logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
378  }
379  return false;
380 }
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.
Definition: blackboard.h:44
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.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
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.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
Fawkes library namespace.