tfdm_interface.cpp
Go to the documentation of this file.
00001 #include "tfd_modules/tfdm_interface.h"
00002 #include <stdio.h>
00003 #include <fstream>
00004 #include <iostream>
00005 #include <sstream>
00006 #include <sys/stat.h>
00007 #include <ros/ros.h>
00008 #include "planParser.h"
00009 #include "domainParser.h"
00010 #include <pluginlib/class_list_macros.h>
00011 
00012 PLUGINLIB_DECLARE_CLASS(tfd_modules, tfdm_interface,
00013         tfd_modules::TFDMInterface, continual_planning_executive::PlannerInterface)
00014 
00015 namespace tfd_modules
00016 {
00017 
00018     TFDMInterface::TFDMInterface()
00019     {
00020         _problemFileName = "/tmp/problem.pddl";
00021     }
00022 
00023     TFDMInterface::~TFDMInterface()
00024     {
00025     }
00026 
00027     void TFDMInterface::setTimeout(double secs)
00028     {
00029         ros::param::set("tfd_modules/timeout_if_plan_found", secs);
00030         ros::param::set("tfd_modules/timeout_while_no_plan_found", secs);
00031     }
00032 
00033     void TFDMInterface::initialize(const std::string & domainFile, const std::vector<std::string> & options)
00034     {
00035         _domainFile = domainFile;
00036 
00037         DomainParser _domain;
00038         ROS_ASSERT(_domain.parse(_domainFile));
00039 
00040         _domainName = _domain.getName();
00041         //_domain.dumpTree();
00042 
00043         std::stringstream ss;
00044         for(std::vector<std::string>::const_iterator it = options.begin(); it != options.end(); it++) {
00045             ss << *it << " ";
00046         }
00047         setModuleOptions(ss.str());
00048     }
00049 
00050     bool TFDMInterface::writeProblem(const SymbolicState & init, const SymbolicState & goal) const
00051     {
00052         std::ofstream f(_problemFileName.c_str());
00053         if(!f.good())
00054             return false;
00055 
00056         // write problem
00057         f << "(define (problem p01)\n";
00058         f << "  (:domain " << _domainName << ")\n";
00059         f << "  (:moduleoptions " << _moduleOptions << ")\n";
00060 
00061         init.toPDDLProblem(f);
00062         goal.toPDDLGoal(f);
00063         f << ")\n";
00064 
00065         if(!f.good())
00066             return false;
00067         f.close();
00068 
00069         return true;
00070     }
00071 
00072     continual_planning_executive::PlannerInterface::PlannerResult TFDMInterface::plan(
00073             const SymbolicState & init, const SymbolicState & goal, Plan & plan)
00074     {
00075         if(!writeProblem(init, goal))
00076             return PR_FAILURE_OTHER;
00077 
00078         // call planner
00079         std::string planNamePrefix = "/tmp/plan";
00080         PlannerResult result = callPlanner(this->_domainFile, _problemFileName, planNamePrefix);
00081         std::string planName = planNamePrefix + ".best";
00082 
00083         // parse plan
00084         if(result == PR_SUCCESS_TIMEOUT || result == PR_SUCCESS) {
00085             std::ifstream planFile(planName.c_str());
00086             bool ok = PlanParser::parsePlan(planFile, plan);
00087             planFile.close();
00088             if(!ok) {
00089                 ROS_WARN_STREAM("No plan generated or failure in parsing!" << std::endl);
00090                 return PR_FAILURE_OTHER;
00091             }
00092         }
00093 
00094         // get result
00095         return result;
00096     }
00097 
00098     continual_planning_executive::PlannerInterface::PlannerResult TFDMInterface::callPlanner(const std::string & domain, const std::string & problem, const std::string & planNamePrefix)
00099     {
00100         const bool failOnPlannerError = false;  // default: switch off
00101 
00102         string curPlan = planNamePrefix + ".best";
00103         remove(curPlan.c_str());
00104 
00105         // set plan name
00106         ros::param::set("tfd_modules/plan_name", planNamePrefix);
00107         // Determine the planner call command
00108         std::stringstream plannerCmdStr;
00109         plannerCmdStr << "rosrun tfd_modules tfd_plan " << domain << " " << problem;
00110         plannerCmdStr << " __name:=tfd_modules";
00111 
00112         // Later: other features for optional debugging???
00113         std::string plannerCmd = plannerCmdStr.str();
00114 
00115         ROS_INFO_STREAM("calling planner: " << plannerCmd << std::endl);
00116 
00117         // ACTUAL PLANNER CALL
00118         int ret = system(plannerCmd.c_str());
00119         if(ret % 256 == 0)   // for child processed -> returning exitstatus * 256
00120             ret /= 256;
00121         else
00122             ret %= 256;
00123         ROS_INFO_STREAM("Planner returned: " << ret);
00124 
00125         bool plannerError = false;
00126         bool plannerTimeout = false;
00127         if(ret != 0) {
00128             ROS_WARN_STREAM("Something in the symbolic planner failed! (Signal " << ret << ")\n");
00129             plannerError = true;
00130             if(ret == 137) {
00131                 plannerTimeout = true;
00132                 ROS_WARN_STREAM("Planner timeout!" << std::endl);
00133             } else {
00134                 ROS_WARN_STREAM("Planner failure: " << ret << std::endl);
00135                 if(failOnPlannerError) {
00136                     ROS_FATAL("Aborting run\n");
00137                     exit(1);
00138                 }
00139             }
00140         }
00141 
00142         // check if plan.best exists for return value
00143         struct stat st;
00144         bool planWritten = false;
00145         if(stat(curPlan.c_str(), &st) == 0) {
00146             planWritten = true;
00147         }
00148 
00149         if(planWritten) {
00150             if(plannerTimeout)
00151                 return PR_SUCCESS_TIMEOUT;
00152             // FIXME: on error still return success as there is a new plan
00153             return PR_SUCCESS;
00154         } 
00155 
00156         if(plannerError) {
00157             if(plannerTimeout)
00158                 return PR_FAILURE_TIMEOUT;
00159             return PR_FAILURE_OTHER;
00160         }
00161 
00162         // FIXME: when completely explored and no plan, the planner returns fine (no error)
00163         // so this really IS an error
00164         return PR_FAILURE_UNREACHABLE;
00165     }
00166 
00167     continual_planning_executive::PlannerInterface::PlannerResult TFDMInterface::monitor(
00168             const SymbolicState & init, const SymbolicState & goal, const Plan & plan)
00169     {
00170         if(!writeProblem(init, goal))
00171             return PR_FAILURE_OTHER;
00172 
00173         // write the plan as input
00174         std::string planNamePrefix = "/tmp/monitor_plan";
00175         std::ofstream f(planNamePrefix.c_str());
00176         if(!f.good())
00177             return PR_FAILURE_OTHER;
00178         f << plan;
00179         if(!f.good())
00180             return PR_FAILURE_OTHER;
00181         f.close();
00182 
00183         PlannerResult result = callMonitoring(this->_domainFile, _problemFileName, planNamePrefix);
00184         
00185         // std::string planName = planNamePrefix + ".monitored";
00186         // could output monitored plan here?
00187 
00188         return result;
00189     }
00190 
00191     continual_planning_executive::PlannerInterface::PlannerResult TFDMInterface::callMonitoring(
00192             const std::string & domain, const std::string & problem, const std::string & planNamePrefix)
00193     {
00194         const bool failOnPlannerError = false;  // default: switch off
00195 
00196         // plan is written to planNamePrefix, the monitored plan should arrive at planNamePrefix.monitored
00197         // planNamePrefix is given as cmd line param
00198         // and also as plan_name ROS param for writing the monitored plan
00199         string curMonitoredPlan = planNamePrefix + ".monitored";
00200         remove(curMonitoredPlan.c_str());
00201 
00202         // set plan name
00203         ros::param::set("tfd_modules/plan_name", planNamePrefix);
00204         
00205         // Determine the planner call command
00206         std::stringstream plannerCmdStr;
00207         plannerCmdStr << "rosrun tfd_modules tfd_monitor " << domain << " " << problem;
00208         plannerCmdStr << " " << planNamePrefix;
00209         plannerCmdStr << " __name:=tfd_modules";
00210 
00211         // Later: other features for optional debugging???
00212         std::string plannerCmd = plannerCmdStr.str();
00213 
00214         ROS_INFO_STREAM("calling monitoring: " << plannerCmd << std::endl);
00215 
00216         // ACTUAL PLANNER CALL
00217         int ret = system(plannerCmd.c_str());
00218         if(ret % 256 == 0)   // for child processed -> returning exitstatus * 256
00219             ret /= 256;
00220         else
00221             ret %= 256;
00222         ROS_INFO_STREAM("Monitoring returned: " << ret);
00223 
00224         bool plannerError = false;
00225         bool plannerTimeout = false;
00226         if(ret != 0 && ret != 1) {
00227             ROS_WARN_STREAM("Something in the symbolic planner/monitoring failed! (Signal " << ret << ")\n");
00228             plannerError = true;
00229             if(ret == 137) {
00230                 plannerTimeout = true;
00231                 ROS_WARN_STREAM("Planner timeout!" << std::endl);
00232             } else {
00233                 ROS_WARN_STREAM("Planner failure: " << ret << std::endl);
00234                 if(failOnPlannerError) {
00235                     ROS_FATAL("Aborting run\n");
00236                     exit(1);
00237                 }
00238             }
00239         }
00240 
00241         // check if plan.monitored exists for extra checking
00242         struct stat st;
00243         bool planWritten = false;
00244         if(stat(curMonitoredPlan.c_str(), &st) == 0) {
00245             planWritten = true;
00246         }
00247 
00248         if(ret == 0) {          // 0 = monitoring success
00249             if(!planWritten) {
00250                 ROS_WARN("Monitoring succeeded, but no monitored plan written.");
00251             }
00252 
00253             if(plannerTimeout)
00254                 return PR_SUCCESS_TIMEOUT;
00255             return PR_SUCCESS;
00256         } else if(ret == 1) {   // 1 = monitoring fail -> i.e. app(init, plan) doesnt reach goal
00257             if(planWritten) {
00258                 ROS_WARN("Monitoring didn't succeed, but a monitored plan was written.");
00259             }
00260 
00261             if(plannerTimeout)
00262                 return PR_FAILURE_TIMEOUT;
00263             return PR_FAILURE_UNREACHABLE;
00264         }
00265         
00266         return PR_FAILURE_OTHER;
00267     }
00268 };
00269 


tfd_modules
Author(s): Maintained by Christian Dornhege (see AUTHORS file).
autogenerated on Mon Oct 6 2014 07:52:06