main.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <ros/ros.h>
00003 #include <vector>
00004 #include <deque>
00005 #include <string>
00006 #include <sstream>
00007 #include <signal.h>
00008 
00009 #include "continual_planning_executive/symbolicState.h"
00010 #include "continual_planning_executive/stateCreator.h"
00011 #include "continual_planning_executive/goalCreator.h"
00012 #include "continual_planning_executive/plannerInterface.h"
00013 #include "continual_planning_executive/SetContinualPlanningControl.h"
00014 #include "continual_planning_executive/ExecuteActionDirectly.h"
00015 #include "planExecutor.h"
00016 #include "continualPlanning.h"
00017 #include <pluginlib/class_loader.h>
00018 
00019 static ContinualPlanning* s_ContinualPlanning = NULL;
00020 
00021 static pluginlib::ClassLoader<continual_planning_executive::PlannerInterface>* s_PlannerLoader = NULL;
00022 static pluginlib::ClassLoader<continual_planning_executive::StateCreator>* s_StateCreatorLoader = NULL;
00023 static pluginlib::ClassLoader<continual_planning_executive::GoalCreator>* s_GoalCreatorLoader = NULL;
00024 static pluginlib::ClassLoader<continual_planning_executive::ActionExecutorInterface>* s_ActionExecutorLoader = NULL;
00025 
00026 static int s_ContinualPlanningMode = continual_planning_executive::SetContinualPlanningControl::Request::RUN;
00027 
00028 std::deque<std::string> splitString(const std::string & s, const char* delim)
00029 {
00030     std::deque<std::string> elems;
00031     std::string::size_type lastPos = 0;
00032     std::string::size_type pos     = 0;
00033 
00034     do {
00035         pos = s.find_first_of(delim, lastPos);
00036         elems.push_back(s.substr(lastPos, pos - lastPos));
00037         lastPos = pos + 1;
00038     } while(std::string::npos != pos);
00039 
00040     return elems;
00041 }
00042 
00043 bool loadStateCreators(ros::NodeHandle & nh)
00044 {
00045     try {
00046         // create here with new as it can't go out of scope
00047         s_StateCreatorLoader 
00048             = new pluginlib::ClassLoader<continual_planning_executive::StateCreator>
00049             ("continual_planning_executive", "continual_planning_executive::StateCreator");
00050     } catch(pluginlib::PluginlibException & ex) {
00051         // possible reason for failure: no known plugins
00052         ROS_ERROR("Could not instantiate class loader for continual_planning_executive::StateCreator - are there plugins registered? Error: %s", ex.what());
00053         return false;
00054     }
00055 
00056     XmlRpc::XmlRpcValue xmlRpc;
00057     if(!nh.getParam("state_creators", xmlRpc)) {
00058         ROS_ERROR("No state_creators defined.");
00059         return false;
00060     }
00061     if(xmlRpc.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00062         ROS_ERROR("state_creators param should be a list.");
00063         return false;
00064     }
00065     if(xmlRpc.size() == 0) {
00066         ROS_ERROR("state_creators list is empty.");
00067         return false;
00068     }
00069     for(int i = 0; i < xmlRpc.size(); i++) {
00070         if(xmlRpc[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
00071             ROS_ERROR("state_creators entry %d is not of type string.", i);
00072             return false;
00073         }
00074         // This should be name + params
00075         std::deque<std::string> state_creator_entry = splitString(xmlRpc[i], " ");
00076         ROS_ASSERT(state_creator_entry.size() >= 1);
00077 
00078         std::string state_creator_name = state_creator_entry.at(0);
00079         state_creator_entry.pop_front();  // no only params left
00080 
00081         ROS_INFO("Loading state creator %s", state_creator_name.c_str());
00082         try {
00083             continual_planning_executive::StateCreator* sc = s_StateCreatorLoader->createClassInstance(state_creator_name);
00084             sc->initialize(state_creator_entry);
00085             s_ContinualPlanning->_stateCreators.push_back(sc);
00086         } catch(pluginlib::PluginlibException & ex) {
00087             ROS_ERROR("Failed to load StateCreator instance for: %s. Error: %s.",
00088                     state_creator_name.c_str(), ex.what());
00089             return false;
00090         }
00091     }
00092 
00093     return !s_ContinualPlanning->_stateCreators.empty();
00094 }
00095 
00096 bool loadGoalCreators(ros::NodeHandle & nh)
00097 {
00098     try {
00099         // create here with new as it can't go out of scope
00100         s_GoalCreatorLoader
00101             = new pluginlib::ClassLoader<continual_planning_executive::GoalCreator>
00102             ("continual_planning_executive", "continual_planning_executive::GoalCreator");
00103     } catch(pluginlib::PluginlibException & ex) {
00104         // possible reason for failure: no known plugins
00105         ROS_ERROR("Could not instantiate class loader for continual_planning_executive::GoalCreator - are there plugins registered? Error: %s", ex.what());
00106         return false;
00107     }
00108 
00109     XmlRpc::XmlRpcValue xmlRpc;
00110     if(!nh.getParam("goal_creators", xmlRpc)) {
00111         ROS_ERROR("No goal_creators defined.");
00112         return false;
00113     } 
00114     if(xmlRpc.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00115         ROS_ERROR("goal_creators param should be a list.");
00116         return false;
00117     }
00118     if(xmlRpc.size() == 0) {
00119         ROS_ERROR("goal_creators list is empty.");
00120         return false;
00121     }
00122     for(int i = 0; i < xmlRpc.size(); i++) {
00123         if(xmlRpc[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
00124             ROS_ERROR("goal_creators entry %d is not of type string.", i);
00125             return false;
00126         }
00127         // This should be name + params
00128         std::deque<std::string> goal_creator_entry = splitString(xmlRpc[i], " ");
00129         ROS_ASSERT(goal_creator_entry.size() >= 1);
00130 
00131         std::string goal_creator_name = goal_creator_entry.at(0);
00132         goal_creator_entry.pop_front();  // no only params left
00133 
00134         ROS_INFO("Loading goal creator %s", goal_creator_name.c_str());
00135         try {
00136             continual_planning_executive::GoalCreator* gc = s_GoalCreatorLoader->createClassInstance(goal_creator_name);
00137             gc->initialize(goal_creator_entry);
00138             if(!gc->fillStateAndGoal(s_ContinualPlanning->_currentState, s_ContinualPlanning->_goal)) {
00139                 ROS_ERROR("Filling state and goal failed for goal_creator %s.", goal_creator_name.c_str());
00140                 return false;
00141             }
00142         } catch(pluginlib::PluginlibException & ex) {
00143             ROS_ERROR("Failed to load GoalCreator instance for: %s. Error: %s.",
00144                     goal_creator_name.c_str(), ex.what());
00145             return false;
00146         }
00147     }
00148 
00149     ROS_INFO_STREAM("Goal initialized to:\n" << s_ContinualPlanning->_goal);
00150     return true;
00151 }
00152 
00153 bool loadActionExecutors(ros::NodeHandle & nh)
00154 {
00155     try {
00156         // create here with new as it can't go out of scope
00157         s_ActionExecutorLoader
00158             = new pluginlib::ClassLoader<continual_planning_executive::ActionExecutorInterface>
00159             ("continual_planning_executive", "continual_planning_executive::ActionExecutorInterface");
00160     } catch(pluginlib::PluginlibException & ex) {
00161         // possible reason for failure: no known plugins
00162         ROS_ERROR("Could not instantiate class loader for continual_planning_executive::ActionExecutorInterface - are there plugins registered? Error: %s", ex.what());
00163         return false;
00164     }
00165 
00166     XmlRpc::XmlRpcValue xmlRpc;
00167     if(!nh.getParam("action_executors", xmlRpc)) {
00168         ROS_ERROR("No action_executors defined.");
00169         return false;
00170     } 
00171     if(xmlRpc.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00172         ROS_ERROR("action_executors param should be a list.");
00173         return false;
00174     }
00175     if(xmlRpc.size() == 0) {
00176         ROS_ERROR("action_executors list is empty.");
00177         return false;
00178     }
00179     for(int i = 0; i < xmlRpc.size(); i++) {
00180         if(xmlRpc[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
00181             ROS_ERROR("action_executors entry %d is not of type string.", i);
00182             return false;
00183         }
00184         // This should be name + params
00185         std::deque<std::string> action_executor_entry = splitString(xmlRpc[i], " ");
00186         ROS_ASSERT(action_executor_entry.size() >= 1);
00187 
00188         std::string action_executor_name = action_executor_entry.at(0);
00189         action_executor_entry.pop_front();  // no only params left
00190 
00191         ROS_INFO("Loading action_executor %s", action_executor_name.c_str());
00192         try {
00193             continual_planning_executive::ActionExecutorInterface* ae
00194                 = s_ActionExecutorLoader->createClassInstance(action_executor_name);
00195             ae->initialize(action_executor_entry);
00196             s_ContinualPlanning->_planExecutor.addActionExecutor(ae);
00197         } catch(pluginlib::PluginlibException & ex) {
00198             ROS_ERROR("Failed to load ActionExecutor instance for: %s. Error: %s.",
00199                     action_executor_name.c_str(), ex.what());
00200             return false;
00201         }
00202     }
00203     return true;
00204 }
00205 
00206 bool loadPlanner(ros::NodeHandle & nh)
00207 {
00208     // load planner
00209     try {
00210         // create here with new as it can't go out of scope
00211         s_PlannerLoader
00212             = new pluginlib::ClassLoader<continual_planning_executive::PlannerInterface>
00213             ("continual_planning_executive", "continual_planning_executive::PlannerInterface");
00214     } catch(pluginlib::PluginlibException & ex) {
00215         // possible reason for failure: no known plugins
00216         ROS_ERROR("Could not instantiate class loader for continual_planning_executive::PlannerInterface - are there plugins registered? Error: %s", ex.what());
00217         return false;
00218     }
00219 
00220     std::string planner_name;
00221     if(!nh.getParam("planner", planner_name)) {
00222         ROS_ERROR("No planner defined!");
00223         return false;
00224     }
00225     ROS_INFO("Loading planner %s", planner_name.c_str());
00226     try {
00227         s_ContinualPlanning->_planner = s_PlannerLoader->createClassInstance(planner_name);
00228     } catch(pluginlib::PluginlibException & ex) {
00229         ROS_ERROR("Failed to load Planner instance for: %s. Error: %s.",
00230                 planner_name.c_str(), ex.what());
00231         return false;
00232     }
00233 
00234     // load planner options
00235     std::vector<std::string> plannerOptions;
00236     XmlRpc::XmlRpcValue xmlRpc;
00237     if(!nh.getParam("planner_options", xmlRpc)) {
00238         ROS_INFO("No planner_options defined.");
00239     } else {
00240         if(xmlRpc.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00241             ROS_ERROR("planner_options param should be a list.");
00242             return false;
00243         }
00244         for(int i = 0; i < xmlRpc.size(); i++) {
00245             if(xmlRpc[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
00246                 ROS_ERROR("planner_options entry %d is not of type string.", i);
00247                 return false;
00248             }
00249             plannerOptions.push_back(xmlRpc[i]);
00250             ROS_DEBUG("Adding planner option \"%s\"", plannerOptions.back().c_str());
00251         }
00252     } 
00253 
00254     // get domain
00255     std::string domainFile;
00256     if(!nh.getParam("domain_file", domainFile)) {
00257         ROS_ERROR("Could not get ~domain_file parameter.");
00258         return false;
00259     }
00260 
00261     // init planner
00262     s_ContinualPlanning->_planner->initialize(domainFile, plannerOptions);
00263 
00264     return s_ContinualPlanning->_planner != NULL;
00265 }
00266 
00267 bool init()
00268 {
00269     ros::NodeHandle nhPriv("~");
00270 
00271     // planner
00272     if(!loadPlanner(nhPriv))
00273         return false;
00274     ROS_INFO("Loaded planner.");
00275 
00276     // state creators
00277     if(!loadStateCreators(nhPriv))
00278         return false;
00279     ROS_INFO("Loaded state creators.");
00280 
00281     // goal
00282     if(!loadGoalCreators(nhPriv))
00283         return false;
00284     ROS_INFO("Loaded goal creators.");
00285 
00286     // actions
00287     if(!loadActionExecutors(nhPriv))
00288         return false;
00289     ROS_INFO("Loaded action executors.");
00290 
00291     return true;
00292 }
00293 
00294 bool setControlHandler(continual_planning_executive::SetContinualPlanningControl::Request & req,
00295         continual_planning_executive::SetContinualPlanningControl::Response & resp)
00296 {
00297     switch(req.command) {
00298         case continual_planning_executive::SetContinualPlanningControl::Request::RUN:
00299         case continual_planning_executive::SetContinualPlanningControl::Request::PAUSE:
00300         case continual_planning_executive::SetContinualPlanningControl::Request::STEP:
00301             if(s_ContinualPlanningMode != req.command) {
00302                 ROS_INFO("Setting ContinualPlanningMode to %d", req.command);
00303             }
00304             s_ContinualPlanningMode = req.command;
00305             resp.command = s_ContinualPlanningMode;
00306             break;
00307         case continual_planning_executive::SetContinualPlanningControl::Request::FORCE_REPLANNING:
00308             s_ContinualPlanning->forceReplanning();
00309             resp.command = req.command;
00310             break;
00311         case continual_planning_executive::SetContinualPlanningControl::Request::REESTIMATE_STATE:
00312             if(!s_ContinualPlanning->estimateCurrentState()) {
00313                 ROS_WARN("SetContinualPlanningControl: State estimation failed.");
00314                 return false;
00315             }
00316             resp.command = req.command;
00317             break;
00318         default:
00319             ROS_ERROR("Invalid command in continual_planning_executive::SetContinualPlanningControl: %d",
00320                     req.command);
00321             return false;
00322     }
00323     return true;
00324 }
00325 
00326 bool executeActionDirectlyHandler(continual_planning_executive::ExecuteActionDirectly::Request & req,
00327         continual_planning_executive::ExecuteActionDirectly::Response & resp)
00328 {
00329     if(s_ContinualPlanningMode == continual_planning_executive::SetContinualPlanningControl::Request::RUN) {
00330         ROS_WARN("Recevied executeActionDirectly request during RUN - ignoring, PAUSE first.");
00331         return false;
00332     }
00333 
00334     DurativeAction a(req.action);
00335 
00336     return s_ContinualPlanning->executeActionDirectly(a, true);
00337 }
00338 
00339 void signal_handler(int signal)
00340 {
00341     if(signal != SIGINT) {
00342         raise(signal);
00343         return;
00344     }
00345 
00346     ROS_INFO("SIGINT received - canceling all running actions.");
00347     s_ContinualPlanning->_planExecutor.cancelAllActions();
00348 
00349     ROS_INFO("shutting down...");
00350     ros::shutdown();
00351 }
00352 
00354 
00362 bool parseOptions(int argc, char** argv, DurativeAction & a)
00363 {
00364     if(argc < 2)
00365         return false;
00366 
00367     a.name = argv[1];
00368     for(int i = 2; i < argc; i++)
00369         a.parameters.push_back(argv[i]);
00370     return true;
00371 }
00372 
00373 int main(int argc, char** argv)
00374 {
00375     ROS_INFO("Continual Planning Executive started.");
00376 
00377     unsigned int initOps = ros::init_options::NoSigintHandler;
00378     if(argc > 1)        // this is usually not the "main" continual_planning_executive, but a debug node
00379         initOps |= ros::init_options::AnonymousName;
00380     ros::init(argc, argv, "continual_planning_executive", initOps);
00381     signal(SIGINT, signal_handler);
00382 
00383     ros::NodeHandle nh;
00384 
00385     DurativeAction debugAction;
00386     bool executeDebug = parseOptions(argc, argv, debugAction);
00387 
00388     // clear module param cache
00389     if(!executeDebug)
00390         nh.deleteParam("tfd_modules/module_cache/putdown");
00391 
00392     s_ContinualPlanning = new ContinualPlanning();
00393 
00394     if(!init()) {
00395         ROS_FATAL("Init failed.");
00396         return 1;
00397     }
00398 
00399     if(executeDebug) {
00400         bool execOK = s_ContinualPlanning->executeActionDirectly(debugAction, false);
00401         return execOK ? 0 : 1;
00402     }
00403 
00404     ros::ServiceServer serviceContinualPlanningMode =
00405         nh.advertiseService("set_continual_planning_control", setControlHandler);
00406     ros::ServiceServer serviceExecuteActionDirectly =
00407         nh.advertiseService("execute_action_directly", executeActionDirectlyHandler);
00408 
00409     ros::NodeHandle nhPriv("~");
00410     bool paused = false;
00411     nhPriv.param("start_paused", paused, false);
00412     if(paused)
00413         s_ContinualPlanningMode = continual_planning_executive::SetContinualPlanningControl::Request::PAUSE;
00414 
00415     ros::Rate loopSleep(5);
00416     ContinualPlanning::ContinualPlanningState cpState = ContinualPlanning::Running;
00417     while(ros::ok()) {
00418         ros::spinOnce();
00419 
00420         if(s_ContinualPlanningMode == continual_planning_executive::SetContinualPlanningControl::Request::RUN
00421             || s_ContinualPlanningMode == continual_planning_executive::SetContinualPlanningControl::Request::STEP) {
00422             cpState = s_ContinualPlanning->loop();
00423             if(cpState != ContinualPlanning::Running) {
00424                 break;
00425             }
00426             // STEP means RUN once.
00427             if(s_ContinualPlanningMode == continual_planning_executive::SetContinualPlanningControl::Request::STEP)
00428                 s_ContinualPlanningMode = continual_planning_executive::SetContinualPlanningControl::Request::PAUSE;
00429         }
00430 
00431         loopSleep.sleep();
00432     }
00433 
00434     if(s_ContinualPlanning->isGoalFulfilled() || cpState == ContinualPlanning::FinishedAtGoal) {
00435         std::stringstream ss2;
00436         ss2 << "\n\nContinual planning ended.\n";
00437         if(s_ContinualPlanning->isGoalFulfilled())
00438             ss2 << "GOAL REACHED by agent!\n";
00439         if(cpState == ContinualPlanning::FinishedAtGoal)
00440             ss2 << "ContinualPlanningState: FinishedAtGoal!\n";
00441         ss2 << "\n";
00442         if(ros::ok())
00443             ROS_INFO("%s", ss2.str().c_str());
00444         else
00445             printf("%s", ss2.str().c_str());
00446     } else {
00447         if(ros::ok())
00448             ROS_ERROR("\n\nContinual planning ended.\nGOAL was NOT REACHED.\n\n");
00449         else
00450             printf("\n\nContinual planning ended.\nGOAL was NOT REACHED.\n\n\n");
00451     }
00452 
00453     return 0;
00454 }
00455 


continual_planning_executive
Author(s): Christian Dornhege
autogenerated on Mon Oct 6 2014 07:51:56