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
00030 #include <Q3GroupBox>
00031 #include <Inventor/Qt/SoQt.h>
00032 #include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
00033 #include <Inventor/sensors/SoIdleSensor.h>
00034 #include <QLayout>
00035
00036 #ifdef Q_WS_X11
00037 #include <unistd.h>
00038 #endif
00039
00040 #include "graspitGUI.h"
00041 #include "mainWindow.h"
00042 #include "ivmgr.h"
00043 #include "world.h"
00044 #include "mytools.h"
00045 #include "SoComplexShape.h"
00046 #include "SoArrow.h"
00047 #include "SoTorquePointer.h"
00048 #include "plugin.h"
00049 #include "debug.h"
00050
00051 #ifdef CGDB_ENABLED
00052 #include "dbase_grasp.h"
00053 #include "taskDispatcher.h"
00054 #endif
00055
00056 bool GraspItGUI::initialized = false;
00057 int GraspItGUI::initResult = SUCCESS;
00058
00059
00060 extern int optind,optopt,opterr;
00061 extern char *optarg;
00062
00064
00065 #ifdef GRASPITDBG
00066 FILE *debugfile;
00067 #endif
00068
00070 GraspItGUI *graspItGUI = 0;
00071
00073
00084 GraspItGUI::GraspItGUI(int argc,char **argv) : mDispatch(NULL)
00085 {
00086 if (!initialized) {
00087 mainWindow = new MainWindow;
00088 SoQt::init(mainWindow->mWindow);
00089
00090
00091 SoComplexShape::initClass();
00092 SoArrow::initClass();
00093 SoTorquePointer::initClass();
00094
00095 ivmgr = new IVmgr((QWidget *)mainWindow->mUI->viewerHolder,"myivmgr");
00096
00097
00098
00099
00100 ivmgr->getViewer()->getWidget()->setFocusPolicy(Qt::StrongFocus);
00101
00102 initialized = true;
00103 graspItGUI = this;
00104 mExitCode = 0;
00105 initResult = processArgs(argc,argv);
00106 }
00107 }
00108
00112 GraspItGUI::~GraspItGUI()
00113 {
00114
00115
00116
00117
00118
00119
00120
00121 stopAllPlugins();
00122 for (size_t i=0; i<mPluginCreators.size(); i++) {
00123 delete mPluginCreators[i];
00124 }
00125
00126 #ifdef CGDB_ENABLED
00127 if (mDispatch) {
00128 delete mDispatch;
00129 }
00130 #endif
00131 delete ivmgr;
00132 delete mainWindow;
00133 }
00134
00142 int
00143 GraspItGUI::processArgs(int argc, char** argv)
00144 {
00145 QString filename;
00146 int errflag=0;
00147 errflag = errflag;
00148 QString graspitRoot = QString(getenv("GRASPIT"));
00149 if (graspitRoot.isNull() ) {
00150 std::cerr << "Please set the GRASPIT environment variable to the root directory of graspIt." << std::endl;
00151 return FAILURE;
00152 }
00153
00154
00155 for (int i=1; i<argc; i++) {
00156 QString arg(argv[i]);
00157 if (arg.section(',',0,0)=="plugin") {
00158 QString libName = arg.section(',',1,1);
00159 std::cout << "Processing arguments \n ";
00160 PluginCreator* creator = PluginCreator::loadFromLibrary(libName.toStdString());
00161 if (creator) {
00162 mPluginCreators.push_back(creator);
00163 } else {
00164 DBGA("Failed to load plugin: " << libName.latin1());
00165 }
00166 }
00167 }
00168
00169
00170 mPluginSensor = new SoIdleSensor(GraspItGUI::sensorCB, (void*)this);
00171 for (size_t i=0; i<mPluginCreators.size(); i++) {
00172 std::cout << "plugin creator autostart " << mPluginCreators[i]->autoStart() << std::endl;
00173 if (mPluginCreators[i]->autoStart()) {
00174 startPlugin(mPluginCreators[i], mPluginCreators[i]->defaultArgs());
00175 }
00176 }
00177
00178 if(argc > 1){
00179 #ifdef CGDB_ENABLED
00180 if(!strcmp(argv[1],"dbase")){
00181 DBaseBatchPlanner *dbp = new DBaseBatchPlanner(graspItGUI->getIVmgr(), this);
00182 dbp->processArguments(argc, argv);
00183 dbp->startPlanner();
00184 }
00185 if (!strcmp(argv[1],"db_dispatch")) {
00186 mDispatch = new TaskDispatcher();
00187 if (mDispatch->connect("wgs36",5432,"willow","willow","household_objects")) {
00188 std::cerr << "DBase dispatch failed to connect to database\n";
00189 return TERMINAL_FAILURE;
00190 }
00191 std::cerr << "DBase dispatch connected to database\n";
00192 mDispatch->mainLoop();
00193 if (mDispatch->getStatus() != TaskDispatcher::RUNNING) {
00194 mExitCode = mDispatch->getStatus();
00195 return TERMINAL_FAILURE;
00196 }
00197 }
00198 #endif
00199 }
00200
00201 #ifdef Q_WS_X11
00202 char c;
00203 while((c=getopt(argc, argv, "r:w:o:b:")) != EOF) {
00204 switch(c) {
00205 case 'r':
00206 filename = graspitRoot + QString("/models/robots/")+
00207 QString(optarg) + QString("/") + QString(optarg) + QString(".cfg");
00208 if (ivmgr->getWorld()->importRobot(filename)==NULL)
00209 ++errflag;
00210 break;
00211 case 'w':
00212 filename = graspitRoot + QString("/worlds/")+ QString(optarg) +
00213 QString(".wld");
00214 if (ivmgr->getWorld()->load(filename)==FAILURE)
00215 ++errflag;
00216 else
00217 mainWindow->mUI->worldBox->setTitle(filename);
00218 break;
00219 case 'o':
00220 filename = graspitRoot + QString("/models/objects/")+ QString(optarg) +
00221 QString(".iv");
00222 if (!ivmgr->getWorld()->importBody("GraspableBody",filename))
00223 ++errflag;
00224 break;
00225 case 'b':
00226 filename = graspitRoot + QString("/models/obstacles/")+ QString(optarg) +
00227 QString(".iv");
00228 if (!ivmgr->getWorld()->importBody("Body",filename))
00229 ++errflag;
00230 break;
00231 default:
00232 ++errflag;
00233 break;
00234 }
00235 }
00236 if (errflag) {
00237 std::cerr << "Usage: graspit [-w worldname] [-r robotname] [-o objectname] [-b obstaclename]" << std::endl;
00238 return FAILURE;
00239 }
00240 #endif
00241 return SUCCESS;
00242 }
00243
00247 void
00248 GraspItGUI::startMainLoop()
00249 {
00250 SoQt::show(mainWindow->mWindow);
00251 mainWindow->setMainWorld(ivmgr->getWorld());
00252 mainWindow->mWindow->resize(QSize(1070,937));
00253 SoQt::mainLoop();
00254 }
00255
00259 void
00260 GraspItGUI::exitMainLoop()
00261 {
00262 SoQt::exitMainLoop();
00263 }
00264
00265 bool
00266 GraspItGUI::terminalFailure() const
00267 {
00268 return initResult == TERMINAL_FAILURE;
00269 }
00270
00271 void
00272 GraspItGUI::sensorCB(void*, SoSensor*)
00273 {
00274 graspItGUI->processPlugins();
00275 }
00276
00277 void
00278 GraspItGUI::startPlugin(PluginCreator* creator, std::string args)
00279 {
00280 Plugin *plugin = creator->createPlugin(args);
00281 if (plugin) mActivePlugins.push_back( std::pair<Plugin*, std::string>(plugin, creator->type()) );
00282 if (!mActivePlugins.empty()) {
00283 mPluginSensor->schedule();
00284 }
00285 }
00286
00287 void GraspItGUI::processPlugins()
00288 {
00289 std::list< std::pair<Plugin*, std::string> >::iterator it = mActivePlugins.begin();
00290 while (it != mActivePlugins.end()) {
00291 if (it->first->mainLoop() == SUCCESS) {
00292 it++;
00293 } else{
00294 delete it->first;
00295 it = mActivePlugins.erase(it);
00296 }
00297 }
00298 if (!mActivePlugins.empty()) {
00299 mPluginSensor->schedule();
00300 }
00301 }
00302
00303 void GraspItGUI::stopPlugin(Plugin *plugin)
00304 {
00305 std::list< std::pair<Plugin*, std::string> >::iterator it;
00306 for (it = mActivePlugins.begin(); it!=mActivePlugins.end(); it++) {
00307 if (it->first == plugin) {
00308 delete it->first;
00309 mActivePlugins.erase(it);
00310 return;
00311 }
00312 }
00313 DBGA("Stop plugin: plugin not found");
00314 }
00315
00316 void GraspItGUI::stopAllPlugins()
00317 {
00318 std::list< std::pair<Plugin*, std::string> >::iterator it = mActivePlugins.begin();
00319 while (it != mActivePlugins.end()) {
00320 delete it->first;
00321 it = mActivePlugins.erase(it);
00322 }
00323 }
