00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "graspit_dbase_tasks/dbTaskDispatcher.h"
00027
00028 #include <sstream>
00029 #include <cassert>
00030
00031 #include "DBPlanner/ros_database_manager.h"
00032 #include "graspitGUI.h"
00033 #include "graspit_db_model.h"
00034 #include "debug.h"
00035
00036 #include "graspit_dbase_tasks/dbTask.h"
00037
00038 namespace graspit_dbase_tasks {
00039
00040 DBTaskDispatcher::DBTaskDispatcher() : mDBMgr(NULL) , mCurrentTask(NULL), mStatus(READY)
00041 {
00042
00043 mMaxTasks = -1;
00044 mCompletedTasks = 0;
00045 }
00046
00047 DBTaskDispatcher::~DBTaskDispatcher()
00048 {
00049 if (mCurrentTask) {
00050
00051 DBGA("Dispatcher: deleting current task on cleanup");
00052 delete mCurrentTask;
00053 }
00054 delete mDBMgr;
00055 }
00056
00057 int DBTaskDispatcher::connect(std::string host, int port, std::string username,
00058 std::string password, std::string database)
00059 {
00060 delete mDBMgr;
00061 std::ostringstream port_str;
00062 port_str << port;
00063
00064
00065 mDBMgr = new db_planner::RosDatabaseManager(host, port_str.str(), username, password, database, NULL, NULL);
00066
00067 GeomGraspitDBModelAllocator* allocator = new GeomGraspitDBModelAllocator(mDBMgr);
00068 mDBMgr->SetModelAllocator(allocator);
00069
00070 if (!mDBMgr->isConnected()) {
00071 DBGA("DBase operator: Connection failed");
00072 delete mDBMgr; mDBMgr = NULL;
00073 return -1;
00074 }
00075 return 0;
00076 }
00077
00078 int DBTaskDispatcher::init(int argc, char **argv)
00079 {
00080
00081 return connect("wgs36",5432,"willow","willow","household_objects");
00082 }
00083
00092 void DBTaskDispatcher::startNewTask()
00093 {
00094 assert(!mCurrentTask);
00095
00096 if (mMaxTasks >= 0 && mCompletedTasks >= mMaxTasks) {
00097 mStatus = DONE;
00098 return;
00099 }
00100 db_planner::TaskRecord rec;
00101 if (!mDBMgr->AcquireNextTask(&rec)) {
00102 DBGA("Dispatcher: error reading next task");
00103 mStatus = ERROR;
00104 return;
00105 }
00106
00107 if (rec.taskType.empty()) {
00108 DBGA("Dispatcher: no tasks to be executed");
00109 mStatus = NO_TASK;
00110 return;
00111 }
00112
00113 std::map<std::string,DBTaskCreator*>::iterator it = mTaskCreators.find(rec.taskType);
00114 if (it==mTaskCreators.end())
00115 {
00116 DBGA("Dispatcher: no creator available for task type: " << rec.taskType);
00117 mStatus = ERROR;
00118 return;
00119 }
00120
00121 mCurrentTask = (*(it->second))(this, mDBMgr,rec);
00122 if (!mCurrentTask) {
00123 DBGA("Dispatcher: could not start task of type: " << rec.taskType);
00124 mStatus = ERROR;
00125 return;
00126 }
00127
00128 mCurrentTask->start();
00129 }
00130
00142 void DBTaskDispatcher::checkCurrentTask()
00143 {
00144 assert(mCurrentTask);
00145 switch (mCurrentTask->getStatus()) {
00146 case DBTask::RUNNING:
00147 DBGP("Task running");
00148 mStatus = RUNNING;
00149 mCurrentTask->mainLoop();
00150 break;
00151 case DBTask::ERROR:
00152 DBGP("Task error");
00153 mStatus = READY;
00154 if (!mDBMgr->SetTaskStatus(mCurrentTask->getRecord().taskId,"ERROR")) {
00155 DBGA("Dispatcher: error marking completed task");
00156 mStatus = ERROR;
00157 }
00158 delete mCurrentTask; mCurrentTask = NULL;
00159 break;
00160 case DBTask::DONE:
00161 DBGP("Task done");
00162 mStatus = READY;
00163 mCompletedTasks++;
00164 if (!mDBMgr->SetTaskStatus(mCurrentTask->getRecord().taskId,"COMPLETED")) {
00165 DBGA("Dispatcher: error marking completed task");
00166 mStatus = ERROR;
00167 }
00168 delete mCurrentTask; mCurrentTask = NULL;
00169 break;
00170 default:
00171 DBGA("Dispatcher: Unknown task state");
00172 mStatus = ERROR;
00173 }
00174 }
00175
00181 int DBTaskDispatcher::mainLoop()
00182 {
00183 if(mCurrentTask) {
00184 checkCurrentTask();
00185 }
00186 if (mStatus == READY) {
00187 startNewTask();
00188 }
00189 switch(mStatus) {
00190 case DONE:
00191 graspItGUI->exitMainLoop();
00192 return 1;
00193 case ERROR:
00194 graspItGUI->exitMainLoop();
00195 return 1;
00196 case NO_TASK:
00197
00198 return 0;
00199 case RUNNING:
00200 break;
00201 case READY:
00202 break;
00203 }
00204 return 0;
00205 }
00206
00207 }