ContinualPlanningMonitorWindow.cpp
Go to the documentation of this file.
00001 #include <QMessageBox>
00002 #include <QInputDialog>
00003 #include <QAction>
00004 #include <QList>
00005 #include <boost/thread.hpp>
00006 #include "ContinualPlanningMonitorWindow.h"
00007 #include "continual_planning_executive/TemporalAction.h"
00008 
00009 extern bool g_Quit;
00010 
00011 ContinualPlanningMonitorWindow::ContinualPlanningMonitorWindow()
00012 {
00013     setupUi(this);
00014 
00015     stateEstimationGrp->setProperty("status", "inactive");
00016     monitoringGrp->setProperty("status", "inactive");
00017     planningGrp->setProperty("status", "inactive");
00018     executionGrp->setProperty("status", "inactive");
00019     resultGrp->setProperty("status", "inactive");
00020     restyle();
00021 
00022     ros::NodeHandle nh;
00023     _subStatus = nh.subscribe("continual_planning_status", 10,
00024             &ContinualPlanningMonitorWindow::statusCallback, this);
00025 
00026     currentPlanList->setContextMenuPolicy(Qt::CustomContextMenu);
00027     connect(currentPlanList, SIGNAL(customContextMenuRequested(const QPoint &)),
00028             this, SLOT(currentPlanList_contextMenu(const QPoint &)));
00029     lastPlanList->setContextMenuPolicy(Qt::CustomContextMenu);
00030     connect(lastPlanList, SIGNAL(customContextMenuRequested(const QPoint &)),
00031             this, SLOT(lastPlanList_contextMenu(const QPoint &)));
00032 
00033     connect(&_executeActionThread, SIGNAL(actionExecutionFailed(bool, QString, QString)),
00034             this, SLOT(notifyUser(bool, QString, QString)));
00035     connect(&_continualPlanningControlThread, SIGNAL(controlCommandSet(bool, QString, QString)),
00036             this, SLOT(notifyUser(bool, QString, QString)));
00037 
00038     _signalMapper = new QSignalMapper(this);
00039     _settings = new QSettings("gki", "ContinualPlanningMonitor");
00040     _nCustomActions = 0;
00041 
00042     int size = _settings->beginReadArray("custom_actions");
00043     for(int i = 0; i < size; ++i) {
00044         _settings->setArrayIndex(i);
00045         QString actionTxt = _settings->value("action").toString();
00046 
00047         // check last is the exec action, then add sep
00048         if(menuExecution->actions().size() > 0) {
00049             if(menuExecution->actions().back() == actionExecute_Action) {
00050                 menuExecution->addSeparator();
00051             }
00052         }
00053 
00054         QAction* act = menuExecution->addAction(actionTxt);
00055 
00056         _signalMapper->setMapping(act, actionTxt);
00057         connect(act, SIGNAL(triggered()),
00058                 _signalMapper, SLOT (map()));
00059         connect(_signalMapper, SIGNAL(mapped(QString)),
00060                 this, SLOT(executeAction(QString)));
00061 
00062         _nCustomActions++;
00063     }
00064     _settings->endArray();
00065 }
00066 
00067 ContinualPlanningMonitorWindow::~ContinualPlanningMonitorWindow()
00068 {
00069 }
00070 
00071 void ContinualPlanningMonitorWindow::on_actionExit_activated()
00072 {
00073     g_Quit = true;
00074 }
00075 
00076 void ContinualPlanningMonitorWindow::on_actionReset_activated()
00077 {
00078     stateEstimationGrp->setProperty("status", "inactive");
00079     monitoringGrp->setProperty("status", "inactive");
00080     planningGrp->setProperty("status", "inactive");
00081     executionGrp->setProperty("status", "inactive");
00082     resultGrp->setProperty("status", "inactive");
00083     restyle();
00084 
00085     stateTxt->document()->setPlainText("");
00086     lastPlanList->clear();
00087     currentPlanList->clear();
00088     currentActionTxt->setText("");
00089     goalReachedTxt->setText("");
00090 }
00091 
00092 void ContinualPlanningMonitorWindow::on_actionRun_activated()
00093 {
00094     _continualPlanningControlThread.setContinualPlanningControl(
00095             continual_planning_executive::SetContinualPlanningControl::Request::RUN);
00096 }
00097 
00098 void ContinualPlanningMonitorWindow::on_actionPause_activated()
00099 {
00100     _continualPlanningControlThread.setContinualPlanningControl(
00101             continual_planning_executive::SetContinualPlanningControl::Request::PAUSE);
00102 }
00103 
00104 void ContinualPlanningMonitorWindow::on_actionStep_activated()
00105 {
00106     _continualPlanningControlThread.setContinualPlanningControl(
00107             continual_planning_executive::SetContinualPlanningControl::Request::STEP);
00108 }
00109 
00110 void ContinualPlanningMonitorWindow::on_actionExecute_Action_activated()
00111 {
00112     QString actionTxt = queryActionText("");
00113 
00114     executeAction(actionTxt);
00115 }
00116 
00117 void ContinualPlanningMonitorWindow::on_actionAdd_Action_activated()
00118 {
00119     QString actionTxt = queryActionText("");
00120     if(actionTxt.length() <= 0)
00121         return;
00122 
00123     // check last is the exec action, then add sep
00124     if(menuExecution->actions().size() > 0) {
00125         if(menuExecution->actions().back() == actionExecute_Action) {
00126             menuExecution->addSeparator();
00127         }
00128     }
00129 
00130     QAction* act = menuExecution->addAction(actionTxt);
00131 
00132     _signalMapper->setMapping(act, actionTxt);
00133     connect(act, SIGNAL(triggered()),
00134             _signalMapper, SLOT (map()));
00135     connect(_signalMapper, SIGNAL(mapped(QString)),
00136             this, SLOT(executeAction(QString)));
00137 
00138     _settings->beginWriteArray("custom_actions");
00139     _settings->setArrayIndex(_nCustomActions);
00140     _settings->setValue("action", actionTxt);
00141     _nCustomActions++;
00142     _settings->endArray();
00143 }
00144 
00145 void ContinualPlanningMonitorWindow::executeAction(QString actionTxt)
00146 {
00147     _executeActionThread.executeAction(actionTxt);
00148 }
00149 
00150 void ContinualPlanningMonitorWindow::on_actionForce_Replanning_activated()
00151 {
00152     _continualPlanningControlThread.setContinualPlanningControl(
00153             continual_planning_executive::SetContinualPlanningControl::Request::FORCE_REPLANNING);
00154 }
00155 
00156 void ContinualPlanningMonitorWindow::on_actionReestimate_State_activated()
00157 {
00158     _continualPlanningControlThread.setContinualPlanningControl(
00159             continual_planning_executive::SetContinualPlanningControl::Request::REESTIMATE_STATE);
00160 }
00161 
00162 QString ContinualPlanningMonitorWindow::getActionDescription(QString action)
00163 {
00164     QString ret = action;
00165     int openParen = ret.indexOf("(");
00166     if(openParen > 0)
00167         ret = ret.mid(openParen + 1);   // skip everything before (
00168 
00169     int closeParen = ret.indexOf(")");
00170     if(closeParen > 0)
00171         ret = ret.mid(0, closeParen);   // skip everything after (
00172 
00173     return ret;
00174 }
00175 
00176 void ContinualPlanningMonitorWindow::statusCallback(
00177         const continual_planning_executive::ContinualPlanningStatus & status)
00178 {
00179     QGroupBox* grp = NULL;
00180     QTextDocument* doc = NULL;
00181     QLineEdit* lineEdit = NULL;
00182     QListWidget* list = NULL;
00183     switch(status.component) {
00184         case continual_planning_executive::ContinualPlanningStatus::STATE_ESTIMATION:
00185             grp = stateEstimationGrp;
00186             doc = stateTxt->document();
00187             break;
00188         case continual_planning_executive::ContinualPlanningStatus::MONITORING:
00189             grp = monitoringGrp;
00190             // no txt to update
00191             break;
00192         case continual_planning_executive::ContinualPlanningStatus::PLANNING:
00193             grp = planningGrp;
00194             list = lastPlanList;
00195             break;
00196         case continual_planning_executive::ContinualPlanningStatus::EXECUTION:
00197             grp = executionGrp;
00198             lineEdit = currentActionTxt;
00199             break;
00200         case continual_planning_executive::ContinualPlanningStatus::CURRENT_PLAN:
00201             // do not set status for CURRENT_PLAN
00202             list = currentPlanList;
00203             break;
00204         case continual_planning_executive::ContinualPlanningStatus::CONTINUAL_PLANNING_FINISHED:
00205             grp = resultGrp;
00206             lineEdit = goalReachedTxt;
00207             break;
00208         default:
00209             ROS_ERROR("Received status with unkown component: %d", status.component);
00210             return;
00211     }
00212     if(grp != NULL) {
00213         switch(status.status) {
00214             case continual_planning_executive::ContinualPlanningStatus::INACTIVE:
00215                 grp->setProperty("status", "inactive");
00216                 break;
00217             case continual_planning_executive::ContinualPlanningStatus::ACTIVE:
00218                 grp->setProperty("status", "active");
00219                 break;
00220             case continual_planning_executive::ContinualPlanningStatus::SUCCESS:
00221                 grp->setProperty("status", "succeeded");
00222                 break;
00223             case continual_planning_executive::ContinualPlanningStatus::FAILURE:
00224                 grp->setProperty("status", "failed");
00225                 break;
00226             default:
00227                 ROS_ERROR("Received status with unknown status: %d", status.status);
00228         }
00229     }
00230 
00231     // one can send '-' to signal that the description should not be updated
00232     // i.e. kept at the before state
00233     if(status.description != "-") {
00234         if(doc != NULL)
00235             doc->setPlainText(status.description.c_str());
00236         if(lineEdit != NULL)
00237             lineEdit->setText(status.description.c_str());
00238         if(list != NULL) {
00239             list->clear();
00240             QString desc = status.description.c_str();
00241             QStringList parts = desc.split('\n');
00242             foreach(QString s, parts) {
00243                 list->addItem(s);
00244             }
00245         }
00246         // execution can also update the current action in the currentPlanList
00247         if(status.component == continual_planning_executive::ContinualPlanningStatus::EXECUTION) {
00248             currentPlanList->setCurrentRow(-1);
00249             int itemRow = -1;
00250             for(int i = 0; i < currentPlanList->count(); i++) {
00251                 QListWidgetItem* item = currentPlanList->item(i);
00252                 if(item == NULL)
00253                     continue;
00254                 if(item->text() == status.description.c_str()) {
00255                     itemRow = i;
00256                     break;
00257                 }
00258             }
00259             currentPlanList->setCurrentRow(itemRow);
00260 
00261             lastPlanList->setCurrentRow(-1);
00262             itemRow = -1;
00263             // in currentPlan this was a full match, in lastPlan the timestamps might be different, so ignore those
00264             QString match = getActionDescription(status.description.c_str());
00265             for(int i = 0; i < lastPlanList->count(); i++) {
00266                 QListWidgetItem* item = lastPlanList->item(i);
00267                 if(item == NULL)
00268                     continue;
00269                 if(getActionDescription(item->text()) == match) {
00270                     itemRow = i;
00271                     break;
00272                 }
00273             }
00274             lastPlanList->setCurrentRow(itemRow);
00275             if(itemRow < 0)
00276                 ROS_WARN("Found no matching item for execution in lastPlan: %s.", status.description.c_str());
00277         }
00278     }
00279 
00280     restyle();
00281 }
00282 
00283 void ContinualPlanningMonitorWindow::lastPlanList_contextMenu(const QPoint & pos)
00284 {
00285     QListWidgetItem* item = lastPlanList->itemAt(pos);
00286     QPoint globalPos = lastPlanList->mapToGlobal(pos);
00287 
00288     executeActionDirectly_contextMenu(item, globalPos);
00289 }
00290 
00291 void ContinualPlanningMonitorWindow::currentPlanList_contextMenu(const QPoint & pos)
00292 {
00293     QListWidgetItem* item = currentPlanList->itemAt(pos);
00294     QPoint globalPos = currentPlanList->mapToGlobal(pos);
00295 
00296     executeActionDirectly_contextMenu(item, globalPos);
00297 }
00298 
00299 void ContinualPlanningMonitorWindow::executeActionDirectly_contextMenu(QListWidgetItem* item,
00300         const QPoint & globalPos)
00301 {
00302     QMenu myMenu;
00303     QAction* executeActionDirectlyAction = myMenu.addAction("Execute Action Directly");
00304     QAction* executeAction = myMenu.addAction("Execute Action ...");
00305 
00306     QAction* selectedItem = myMenu.exec(globalPos);
00307     if(selectedItem == NULL)
00308         return;
00309 
00310     if(selectedItem == executeAction || selectedItem == executeActionDirectlyAction) {
00311         QString actionTxt = "";
00312         if(item != NULL) {
00313             actionTxt = getActionDescription(item->text()).trimmed();
00314         }
00315 
00316         if(selectedItem == executeActionDirectlyAction) { // directly = don't ask/edit
00317             if(actionTxt.length() <= 0)
00318                 return;
00319         } else {
00320             actionTxt = queryActionText(actionTxt);
00321         }
00322 
00323         _executeActionThread.executeAction(actionTxt);
00324     }
00325 }
00326 
00327 QString ContinualPlanningMonitorWindow::queryActionText(QString actionTxt)
00328 {
00329     // the whole point of doing this manually is to be able to resize the dialog.
00330     QInputDialog inp(this);
00331     inp.setInputMode(QInputDialog::TextInput);
00332     inp.setWindowTitle("Execute Action Directly");
00333     inp.setLabelText("Action:");
00334     inp.setTextValue(actionTxt);
00335     QSize s = inp.size();
00336     s.setWidth(s.width() + 200);
00337     inp.resize(s);
00338     if(inp.exec() == QDialog::Accepted)
00339         actionTxt = inp.textValue().trimmed();
00340     else
00341         actionTxt = "";
00342     return actionTxt;
00343 }
00344 
00345 void ContinualPlanningMonitorWindow::notifyUser(bool success, QString title, QString message)
00346 {
00347     if(success)
00348         QMessageBox::information(this, title, message);
00349     else
00350         QMessageBox::critical(this, title, message);
00351 }
00352 
00353 void ContinualPlanningMonitorWindow::restyle()
00354 {
00355     style()->unpolish(stateEstimationGrp);
00356     style()->polish(stateEstimationGrp);
00357     style()->unpolish(monitoringGrp);
00358     style()->polish(monitoringGrp);
00359     style()->unpolish(planningGrp);
00360     style()->polish(planningGrp);
00361     style()->unpolish(executionGrp);
00362     style()->polish(executionGrp);
00363     style()->unpolish(resultGrp);
00364     style()->polish(resultGrp);
00365 }
00366 
00367 ExecuteActionThread::ExecuteActionThread()
00368 {
00369     ros::NodeHandle nh;
00370 
00371     _serviceExecuteActionDirectly =
00372         nh.serviceClient<continual_planning_executive::ExecuteActionDirectly>("execute_action_directly");
00373 }
00374 
00375 void ExecuteActionThread::executeAction(QString actionTxt)
00376 {
00377     if(isRunning() && actionTxt != _actionTxt) {
00378         Q_EMIT actionExecutionFailed(false, "Execute action",
00379                 "Execute action failed as another action is still running.");
00380         return;
00381     }
00382     // thread isn't running, and as we are here in the main thread
00383     // nobody can start it.
00384 
00385     if(actionTxt.length() <= 0)
00386         return;
00387 
00388     continual_planning_executive::TemporalAction temporalAction;
00389     QStringList parts = actionTxt.split(" ", QString::SkipEmptyParts);
00390     if(parts.size() < 1) {
00391         ROS_ERROR("Empty parts for action: %s", qPrintable(actionTxt));
00392         return;
00393     }
00394     temporalAction.name = qPrintable(parts.at(0));
00395     for(int i = 1; i < parts.size(); i++) {
00396         temporalAction.parameters.push_back(qPrintable(parts.at(i)));
00397     }
00398     temporalAction.start_time = 0;  // make it executable now
00399     temporalAction.duration = 1;
00400     _srv.request.action = temporalAction;
00401     _actionTxt = actionTxt;
00402 
00403     // set cur srv
00404     start(LowPriority);
00405 }
00406 
00407 void ExecuteActionThread::run()
00408 {
00409     // Put this service call in a background thread to reenable
00410     // GUI updates from the service call
00411     if(!_serviceExecuteActionDirectly.call(_srv)) {
00412         Q_EMIT actionExecutionFailed(false, "Execute Action",
00413                 QString("Executing action %1 failed.").arg(_actionTxt));
00414     } else {
00415         // Send an OK is too annoying?
00416         //Q_EMIT actionExecutionFailed(true, "Execute Action",
00417           //      QString("Executed action %1.").arg(_actionTxt));
00418     }
00419 }
00420 
00421 ContinualPlanningControlThread::ContinualPlanningControlThread()
00422 {
00423     ros::NodeHandle nh;
00424 
00425     _serviceContinualPlanningControl =
00426         nh.serviceClient<continual_planning_executive::SetContinualPlanningControl>
00427         ("set_continual_planning_control");
00428 }
00429 
00430 void ContinualPlanningControlThread::setContinualPlanningControl(int command)
00431 {
00432     if(isRunning()) {
00433         Q_EMIT controlCommandSet(false, "Set ContinualPlanningControl",
00434                 "Set ContinualPlanningControl failed as another request is still running.");
00435         return;
00436     }
00437     // thread isn't running, and as we are here in the main thread
00438     // nobody can start it.
00439 
00440     _srv.request.command = command;
00441 
00442     start(LowPriority);
00443 }
00444 
00445 void ContinualPlanningControlThread::run()
00446 {
00447     // Put this service call in a background thread to reenable
00448     // GUI updates from the run
00449     if(!_serviceContinualPlanningControl.call(_srv)) {
00450         Q_EMIT controlCommandSet(false, "Set ContinualPlanningControl",
00451                 QString("Setting ContinualPlanningControl to %1 failed.").arg(_srv.request.command));
00452     } else {
00453         // don't send for RUN, etc. we'll see that because it's running now
00454         if(_srv.response.command == continual_planning_executive::SetContinualPlanningControl::Request::PAUSE)
00455             Q_EMIT controlCommandSet(true, "Set ContinualPlanningControl",
00456                     "ContinualPlanningControl paused successfully.");
00457     }
00458 }
00459 


continual_planning_monitor
Author(s): Christian Dornhege
autogenerated on Mon Oct 6 2014 07:52:02