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
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
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
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();
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
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
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
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();
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
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
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
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();
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
00209 try {
00210
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
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
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
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
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
00272 if(!loadPlanner(nhPriv))
00273 return false;
00274 ROS_INFO("Loaded planner.");
00275
00276
00277 if(!loadStateCreators(nhPriv))
00278 return false;
00279 ROS_INFO("Loaded state creators.");
00280
00281
00282 if(!loadGoalCreators(nhPriv))
00283 return false;
00284 ROS_INFO("Loaded goal creators.");
00285
00286
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)
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
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
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