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 
40 /** @class PddlPlannerThread 'pddl-planner_thread.h'
41  * Starts a pddl planner and writes the resulting plan into the robot memory
42  * @author Frederik Zwilling
43  */
44 
45 /** Constructor. */
47 : Thread("PddlPlannerThread", Thread::OPMODE_WAITFORWAKEUP),
48  BlackBoardInterfaceListener("PddlPlannerThread")
49 {
50 }
51 
52 void
54 {
55  //read config
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");
62  cfg_fd_options_ = config->get_string(cfg_prefix + "fd-search-opts");
63  cfg_collection_ = config->get_string(cfg_prefix + "collection");
64 
65  //set configured planner
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);
69  logger->log_info(name(), "Fast-Forward planner selected.");
70  } else if (planner_string == "fd") {
71  planner_ = std::bind(&PddlPlannerThread::fd_planner, this);
72  logger->log_info(name(), "Fast-Downward planner selected.");
73  } else if (planner_string == "dbmp") {
74  planner_ = std::bind(&PddlPlannerThread::dbmp_planner, this);
75  logger->log_info(name(), "DBMP selected.");
76  } else {
77  planner_ = std::bind(&PddlPlannerThread::ff_planner, this);
78  logger->log_warn(name(), "No planner configured.\nDefaulting to ff.");
79  }
80 
81  //setup interface
82  plan_if_ = blackboard->open_for_writing<PddlPlannerInterface>(
83  config->get_string(cfg_prefix + "interface-name").c_str());
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);
88  plan_if_->write();
89 
90  //setup interface listener
92  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
93 
94  // If we receive multiple wakeup() calls during loop, only call the loop once afterwards
95  // We want to only re-plan once after a loop run since multiple runs would plan on the same problem
96  this->set_coalesce_wakeups(true);
97 }
98 
99 /**
100  * Thread is only waked up if there was a new interface message to plan
101  */
102 void
104 {
105  logger->log_info(name(), "Starting PDDL Planning...");
106 
107  //writes plan into action_list_
108  planner_();
109 
110  if (!action_list_.empty()) {
111  auto plan = BSONFromActionList();
112  robot_memory->update(from_json("{plan:{$exists:true}}").view(), plan, cfg_collection_, true);
113  print_action_list();
114  plan_if_->set_success(true);
115  } else {
116  logger->log_error(name(), "Updating plan failed, action list empty!");
117  robot_memory->update(from_json("{plan:{$exists:true}}").view(),
118  from_json("{plan:0}").view(),
119  cfg_collection_,
120  true);
121  plan_if_->set_success(false);
122  }
123 
124  plan_if_->set_final(true);
125  plan_if_->write();
126 }
127 
128 void
130 {
131  blackboard->close(plan_if_);
132 }
133 
134 void
135 PddlPlannerThread::ff_planner()
136 {
137  logger->log_info(name(), "Starting PDDL Planning with Fast-Forward...");
138 
139  std::string command = "ff -o " + cfg_domain_path_ + " -f " + cfg_problem_path_;
140  logger->log_info(name(), "Calling %s", command.c_str());
141  std::string result = run_planner(command);
142 
143  //Parse Result and write it into the robot memory
144  logger->log_info(name(), "Parsing result");
145 
146  action_list_.clear();
147 
148  size_t cur_pos = 0;
149  if (result.find("found legal plan as follows", cur_pos) == std::string::npos) {
150  logger->log_error(name(), "Planning Failed: %s", result.c_str());
151  robot_memory->update(from_json("{plan:{$exists:true}}").view(),
152  from_json("{plan:1,fail:1,steps:[]}").view(),
153  cfg_collection_,
154  true);
155  return;
156  }
157  //remove stuff that could confuse us later
158  result.erase(result.find("time spent:", cur_pos));
159 
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);
164  logger->log_info(name(),
165  "line:%s (%zu-%zu)",
166  result.substr(cur_pos, line_end - cur_pos).c_str(),
167  cur_pos,
168  line_end);
169  action a;
170  if (line_end < result.find(" ", cur_pos)) {
171  a.name = result.substr(cur_pos, line_end - cur_pos);
172  } else {
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) {
179  arg_end = line_end;
180  }
181  a.args.push_back(result.substr(cur_pos, arg_end - cur_pos));
182  cur_pos = arg_end + 1;
183  }
184  }
185  action_list_.push_back(a);
186  }
187 }
188 
189 void
190 PddlPlannerThread::dbmp_planner()
191 {
192  logger->log_info(name(), "Starting PDDL Planning with DBMP...");
193 
194  std::string command =
195  "dbmp.py -p ff --output plan.pddl " + cfg_domain_path_ + " " + cfg_problem_path_;
196  logger->log_info(name(), "Calling %s", command.c_str());
197  std::string result = run_planner(command);
198 
199  //Parse Result and write it into the robot memory
200  logger->log_info(name(), "Parsing result");
201 
202  size_t cur_pos = 0;
203  if (result.find("Planner failed", cur_pos) != std::string::npos) {
204  logger->log_error(name(), "Planning Failed: %s", result.c_str());
205  robot_memory->update(from_json("{plan:{$exists:true}}").view(),
206  from_json("{plan:1,fail:1,steps:[]}").view(),
207  cfg_collection_,
208  true);
209  return;
210  }
211  std::ifstream planfile("plan.pddl");
212  std::string line;
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) {
217  // makespan, skip
218  continue;
219  }
220  if (line[0] != '(' || line[line.size() - 1] != ')') {
221  logger->log_error(name(), "Expected parantheses in line '%s'!", line.c_str());
222  return;
223  }
224  // remove parantheses
225  std::string action_str = line.substr(1, line.size() - 2);
226  action a;
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));
233  }
234  action_list_.push_back(a);
235  }
236 }
237 
238 void
239 PddlPlannerThread::fd_planner()
240 {
241  logger->log_info(name(), "Starting PDDL Planning with Fast-Downward...");
242 
243  std::string command =
244  "fast-downward" + std::string(" ") + cfg_domain_path_ + std::string(" ") + cfg_problem_path_;
245 
246  if (!cfg_fd_options_.empty()) {
247  command += std::string(" ") + cfg_fd_options_;
248  }
249 
250  std::string result = run_planner(command);
251 
252  logger->log_info(name(), "Removing temporary planner output.");
253  std::remove("output");
254  std::remove("output.sas");
255 
256  size_t cur_pos = 0;
257  if (result.find("Solution found!", cur_pos) == std::string::npos) {
258  logger->log_error(name(), "Planning Failed: %s", result.c_str());
259  throw Exception("No solution found");
260  } else {
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);
264  logger->log_info(name(), "Planner found solution.");
265  }
266  result.erase(0, cur_pos);
267  size_t end_pos = result.find("Plan length: ");
268  result.erase(end_pos, result.size() - 1);
269 
270  std::istringstream iss(result);
271  std::string line;
272  // remove surplus line
273  getline(iss, line);
274  while (getline(iss, line)) {
275  action a;
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)));
280  std::string item;
281  while (getline(ss, item, ' ')) {
282  a.args.push_back(item);
283  }
284  }
285  action_list_.push_back(a);
286  }
287 }
288 
289 document::value
290 PddlPlannerThread::BSONFromActionList()
291 {
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) {
302  args.append(arg);
303  }
304  }));
305  }
306  }));
307 
308  return plan.extract();
309 }
310 
311 size_t
312 PddlPlannerThread::find_nth_space(const std::string &s, size_t nth)
313 {
314  size_t pos = 0;
315  unsigned occurrence = 0;
316 
317  while (occurrence != nth && (pos = s.find(' ', pos + 1)) != std::string::npos) {
318  ++occurrence;
319  }
320 
321  return pos + 1;
322 }
323 
324 void
325 PddlPlannerThread::print_action_list()
326 {
327  unsigned int count = 0;
328  for (action a : action_list_) {
329  count++;
330  std::string args;
331  for (std::string arg : a.args) {
332  args += arg + " ";
333  }
334  logger->log_info(name(), "Action %d %s with args %s", count, a.name.c_str(), args.c_str());
335  }
336 }
337 
338 std::string
339 PddlPlannerThread::run_planner(std::string command)
340 {
341  logger->log_info(name(), "Running planner with command: %s", command.c_str());
342  std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
343  if (!pipe)
344  throw std::runtime_error("popen() failed!");
345  char buffer[128];
346  std::string result;
347  while (!feof(pipe.get())) {
348  if (fgets(buffer, 128, pipe.get()) != NULL)
349  result += buffer;
350  }
351  logger->log_info(name(), "Planner finished run.");
352 
353  return result;
354 }
355 
356 bool
357 PddlPlannerThread::bb_interface_message_received(Interface * interface,
358  fawkes::Message *message) throw()
359 {
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);
365  plan_if_->write();
366  wakeup(); //activates loop where the generation is done
367  } else {
368  logger->log_error(name(), "Received unknown message of type %s, ignoring", message->type());
369  }
370  return false;
371 }
fawkes::MultiLogger::log_error
virtual void log_error(const char *component, const char *format,...)
Definition: multi.cpp:243
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
fawkes::Message
Definition: message.h:41
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
PddlPlannerThread::PddlPlannerThread
PddlPlannerThread()
Constructor.
Definition: pddl-planner_thread.cpp:46
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:47
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Thread::set_coalesce_wakeups
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:735
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes
fawkes::RobotMemoryAspect::robot_memory
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
Definition: robot_memory_aspect.h:58
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::BlackBoardInterfaceListener::bbil_add_message_interface
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
Definition: interface_listener.cpp:247
fawkes::Interface
Definition: interface.h:78
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
PddlPlannerThread::loop
virtual void loop()
Thread is only waked up if there was a new interface message to plan.
Definition: pddl-planner_thread.cpp:103
PddlPlannerThread::finalize
virtual void finalize()
Finalize the thread.
Definition: pddl-planner_thread.cpp:129
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
PddlPlannerThread::init
virtual void init()
Initialize the thread.
Definition: pddl-planner_thread.cpp:53
RobotMemory::update
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.
Definition: robot_memory.cpp:380
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Exception
Definition: exception.h:41