any_plan_executor.cpp
Go to the documentation of this file.
00001 
00002 #include "msgs_utils.h"
00003 #include "RemoteReasoner.h"
00004 #include "StaticFacts.h"
00005 
00006 #include "actasp/action_utils.h"
00007 #include "actasp/executors/ReplanningActionExecutor.h"
00008 #include "actasp/planners/AnyPlan.h"
00009 #include "actasp/ExecutionObserver.h"
00010 #include "actasp/PlanningObserver.h"
00011 #include "actasp/AnswerSet.h"
00012 #include <actasp/reasoners/Clingo4_2.h>
00013 
00014 
00015 #include "bwi_kr_execution/ExecutePlanAction.h"
00016 
00017 #include "actions/ActionFactory.h"
00018 #include "actions/LogicalNavigation.h"
00019 
00020 #include <actionlib/server/simple_action_server.h>
00021 
00022 #include <ros/ros.h>
00023 #include <ros/package.h>
00024 #include <ros/console.h>
00025 
00026 #include <boost/filesystem.hpp>
00027 
00028 #include <string>
00029 
00030 const int MAX_N = 20;
00031 const std::string queryDirectory("/tmp/bwi_action_execution/");
00032 
00033 
00034 using namespace std;
00035 using namespace bwi_krexec;
00036 using namespace actasp;
00037 
00038 typedef actionlib::SimpleActionServer<bwi_kr_execution::ExecutePlanAction> Server;
00039 
00040 
00041 ActionExecutor *executor;
00042 
00043 struct PrintFluent {
00044   
00045   PrintFluent(ostream& stream) : stream(stream) {}
00046   
00047   string operator()(const AspFluent& fluent) {
00048     stream << fluent.toString() << " ";
00049   }
00050   
00051   ostream &stream;
00052   
00053 };
00054 
00055 struct Observer : public ExecutionObserver, public PlanningObserver {
00056   
00057   void actionStarted(const AspFluent& action) throw() {
00058     ROS_INFO_STREAM("Starting execution: " << action.toString());
00059   }
00060   
00061   void actionTerminated(const AspFluent& action) throw() {
00062     ROS_INFO_STREAM("Terminating execution: " << action.toString());
00063   }
00064   
00065   
00066   void planChanged(const AnswerSet& newPlan) throw() {
00067    stringstream planStream;
00068    
00069    ROS_INFO_STREAM("plan size: " << newPlan.getFluents().size());
00070    
00071    copy(newPlan.getFluents().begin(),newPlan.getFluents().end(),ostream_iterator<string>(planStream," "));
00072    
00073    ROS_INFO_STREAM(planStream.str());
00074   }
00075   
00076   void goalChanged(std::vector<actasp::AspRule> newGoalRules) throw() {}
00077   
00078   void policyChanged(PartialPolicy* policy) throw() {}
00079   
00080   
00081 };
00082 
00083 void executePlan(const bwi_kr_execution::ExecutePlanGoalConstPtr& plan, Server* as) {
00084 
00085   vector<AspRule> goalRules;
00086 
00087   transform(plan->aspGoal.begin(),plan->aspGoal.end(),back_inserter(goalRules),TranslateRule());
00088 
00089   executor->setGoal(goalRules);
00090 
00091   ros::Rate loop(10);
00092 
00093   while (!executor->goalReached() && !executor->failed() && ros::ok()) {
00094 
00095     if (!as->isPreemptRequested()) {
00096       executor->executeActionStep();
00097     }
00098     else {
00099       if(as->isNewGoalAvailable()) {
00100         goalRules.clear();
00101         const bwi_kr_execution::ExecutePlanGoalConstPtr& newGoal = as->acceptNewGoal();
00102         transform(newGoal->aspGoal.begin(),newGoal->aspGoal.end(),back_inserter(goalRules),TranslateRule());
00103         executor->setGoal(goalRules);
00104       }
00105     }
00106          loop.sleep();
00107   }
00108 
00109 
00110   if (executor->goalReached()) {
00111     ROS_INFO("Execution succeded");
00112     as->setSucceeded();
00113   }
00114   else {
00115     ROS_INFO("Execution failed");
00116     as->setAborted();
00117   }
00118 }
00119 
00120 int main(int argc, char**argv) {
00121   ros::init(argc, argv, "action_executor");
00122   ros::NodeHandle n;
00123 
00124 //   if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
00125 //     ros::console::notifyLoggerLevelsChanged();
00126 //   }
00127   
00128   ros::NodeHandle privateNode("~");
00129   string domainDirectory;
00130   n.param<std::string>("bwi_kr_execution/domain_directory", domainDirectory, ros::package::getPath("bwi_kr_execution")+"/domain/");
00131   
00132   if(domainDirectory.at(domainDirectory.size()-1) != '/')
00133     domainDirectory += '/';
00134 
00135 //  create initial state
00136   LogicalNavigation setInitialState("noop");
00137   setInitialState.run();
00138 
00139 
00140   bool simulating;
00141   privateNode.param<bool>("simulation",simulating,false);
00142   ActionFactory::setSimulation(simulating); 
00143   
00144   boost::filesystem::create_directories(queryDirectory);
00145 
00146   FilteringQueryGenerator *generator = new Clingo4_2("n",queryDirectory,domainDirectory,actionMapToSet(ActionFactory::actions()));
00147   AspKR *reasoner = new RemoteReasoner(generator, MAX_N,actionMapToSet(ActionFactory::actions()));
00148   StaticFacts::retrieveStaticFacts(reasoner, domainDirectory);
00149   Planner *planner = new AnyPlan(reasoner,1.);
00150   
00151   //need a pointer to the specific type for the observer
00152   ReplanningActionExecutor *replanner = new ReplanningActionExecutor(reasoner,planner,ActionFactory::actions());
00153   executor = replanner;
00154   
00155   Observer observer;
00156   executor->addExecutionObserver(&observer);
00157   replanner->addPlanningObserver(&observer);
00158 
00159   Server server(privateNode, "execute_plan", boost::bind(&executePlan, _1, &server), false);
00160   server.start();
00161 
00162   ros::spin();
00163 
00164   
00165   return 0;
00166 }


bwi_kr_execution
Author(s): Matteo Leonetti, Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:36