/*!
Start the planner
Connects the signals needed to monitor the progress of the planner.
Then start the planner running.
*/
void GraspPlanningTask::startPlanner(){
QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete()));
QObject::connect(mPlanner, SIGNAL(loopUpdate()), this, SLOT(plannerLoopUpdate()));
QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(plannerUpdated()));
if (!mPlanner->resetPlanner()) {
DBGA("Grasp Planning Task: failed to reset planner");
mStatus = ERROR;
return ;
}
//load all already known grasps so that we avoid them in current searches
// mLastSolution = mPlanner->getListSize();
DBGA("Planner started");
mPlanner->startPlanner();
mStatus = RUNNING;
}
void BCIService::init(BCIControlWindow *bciControlWindow)
{
ROS_INFO("Initing BCI Service");
bciRenderArea = bciControlWindow->bciWorldView->renderArea;
QPushButton * slowButton = new QPushButton("Flex Medium (Next)");
QPushButton * fastButton = new QPushButton("Flex Hard (Select)");
slowButton->setDefault(true);
fastButton->setDefault(true);
QDialogButtonBox *cursorControlBox = new QDialogButtonBox(Qt::Vertical);
cursorControlBox->setCaption(QString("Cursor Control Box"));
cursorControlBox->addButton(slowButton, QDialogButtonBox::ActionRole);
cursorControlBox->addButton(fastButton, QDialogButtonBox::ActionRole);
cursorControlBox->setWindowFlags(Qt::WindowStaysOnTopHint);
cursorControlBox->resize(QSize(200,100));
cursorControlBox->show();
QObject::connect(slowButton, SIGNAL(clicked()), this, SLOT(updateControlSceneState1()));
QObject::connect(fastButton, SIGNAL(clicked()), this, SLOT(updateControlSceneState2()));
endOfExperimentFormat = new QFormLayout();
successFailure = new QComboBox();
successFailure->insertItem("Success");
successFailure->insertItem("Failure");
finalComments = new QTextEdit();
finishedButton = new QPushButton("FINISHED");
endOfExperimentFormat->addRow(tr("&Success or failure: "), successFailure);
endOfExperimentFormat->addRow(tr("&Comments: "), finalComments);
endOfExperimentFormat->addRow(tr(""), finishedButton);
endOfExperiment = new QGroupBox();
endOfExperiment->setLayout(endOfExperimentFormat);
QObject::connect(finishedButton, SIGNAL(clicked()), this, SLOT(onFinishedFinalLog()));
endOfExperiment->show();
timer = new QTimer;
QObject::connect(timer, SIGNAL(timeout()), this, SLOT(updateControlScene()));
timer->start(1000 / 30);
QObject::connect(this, SIGNAL(goToStateLow()), this, SLOT(updateControlSceneState0()));
QObject::connect(this, SIGNAL(goToStateMedium()), this, SLOT(updateControlSceneState1()));
QObject::connect(this, SIGNAL(goToStateHigh()), this, SLOT(updateControlSceneState2()));
SoAnnotation *hudSeparator = new SoAnnotation;
hudSeparator->renderCaching = SoSeparator::OFF;
hudSeparator->setName("hud");
graspitCore->getWorld()->getIVRoot()->addChild(hudSeparator);
pcam = new SoOrthographicCamera;
pcam->position = SbVec3f(0, 0, 10);
pcam->nearDistance = 0.1;
pcam->farDistance = 11;
hudSeparator->addChild(pcam);
SoLightModel * hudLightModel = new SoLightModel;
hudLightModel->model=SoLightModel::BASE_COLOR;
hudSeparator->addChild(hudLightModel);
SoCallback * disableZTestNode = new SoCallback();
disableZTestNode->setCallback(ui_tools::disableZCulling);
hudSeparator->addChild(disableZTestNode);
csm = new ControllerSceneManager(hudSeparator);
BCIStateMachine *bciStateMachine = new BCIStateMachine(bciControlWindow,this);
connect(this, SIGNAL(plannerUpdated()), bciControlWindow, SLOT(redraw()));
connect(GraspManager::getInstance(), SIGNAL(render()), bciControlWindow, SLOT(redraw()));
bciStateMachine->start();
ROS_INFO("Finished initing BCI Service");
}
