00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include <vector>
00012 #include <string>
00013 #include <fstream>
00014
00015 #include <QWidget>
00016 #include <QPushButton>
00017 #include <QVBoxLayout>
00018 #include <QHBoxLayout>
00019 #include <QString>
00020 #include <QFileDialog>
00021 #include <QFileInfo>
00022 #include <QInputDialog>
00023 #include <QLineEdit>
00024 #include <QSlider>
00025 #include <QLabel>
00026 #include <QGroupBox>
00027 #include <QComboBox>
00028 #include <QHeaderView>
00029 #include <QMessageBox>
00030 #include <QSortFilterProxyModel>
00031
00032 #include <or_msgs/OrLearnCommand.h>
00033 #include "Modules/ORLearningModule.h"
00034
00035
00036
00037
00038
00039 #include "ObjectImagesControl.h"
00040
00041 #define THIS ObjectImagesControl
00042
00043 THIS::THIS ( ros::NodeHandle *nh, QWidget *parent ) : QWidget ( parent )
00044 {
00045 m_Ready = false;
00046
00047 QGridLayout* mainLayout = new QGridLayout();
00048
00049 m_ImageTable = new QTableWidget();
00050 m_ImageTable->setColumnCount ( 2 );
00051 m_ImageTable->setRowCount ( 0 );
00052 QStringList headerList;
00053 headerList << "#" << "Name";
00054 m_ImageTable->setHorizontalHeaderLabels ( headerList );
00055 m_ImageTable->horizontalHeader()->setResizeMode ( 0, QHeaderView::Interactive );
00056 m_ImageTable->horizontalHeader()->setResizeMode ( 1, QHeaderView::Stretch );
00057 m_ImageTable->verticalHeader()->hide();
00058
00059
00060 mainLayout->addWidget ( m_ImageTable, 1, 0, 1, 3 );
00061
00062 QHBoxLayout* creationLayout = new QHBoxLayout( );
00063 m_ObjectNameField = new QLineEdit ( "" );
00064 m_ObjectNameField->setMinimumWidth ( 80 );
00065 creationLayout->addWidget ( new QLabel ( "Name:" ) );
00066 creationLayout->addWidget ( m_ObjectNameField );
00067
00068 m_TypeComboBox = new QComboBox();
00069 m_TypeComboBox->addItem ( "Object" );
00070 m_TypeComboBox->addItem ( "Face" );
00071 creationLayout->addWidget ( m_TypeComboBox );
00072 m_saveObjectButton = new QPushButton ( QIcon ( "icons/save.png" ), "Save" );
00073 creationLayout->addWidget ( m_saveObjectButton );
00074 mainLayout->addLayout ( creationLayout, 0, 0, 1, 3 );
00075
00076 m_removeImageButton = new QPushButton ( "Remove Image" );
00077 mainLayout->addWidget ( m_removeImageButton, 3, 0 );
00078
00079 m_resetImagesButton = new QPushButton ( "Reset" );
00080 mainLayout->addWidget ( m_resetImagesButton, 3, 1 );
00081
00082 QPushButton* loadObjectButton = new QPushButton ( "Load Object" );
00083 mainLayout->addWidget ( loadObjectButton, 3, 2 );
00084
00085 mainLayout->addWidget ( new QLabel ( "Image name:" ), 4, 0 );
00086 m_ImageNameField = new QLineEdit ( "" );
00087 m_ImageNameField->setMinimumWidth ( 80 );
00088 mainLayout->addWidget ( m_ImageNameField, 4, 1 );
00089
00090 QPushButton* saveImageButton = new QPushButton ( "Add Image" );
00091 mainLayout->addWidget ( saveImageButton, 4, 2 );
00092
00093 setLayout ( mainLayout );
00094
00095 connect ( saveImageButton, SIGNAL ( clicked() ), this, SLOT ( saveImage() ) );
00096 connect ( loadObjectButton, SIGNAL ( clicked() ), this, SLOT ( loadObject() ) );
00097 connect ( m_removeImageButton, SIGNAL ( clicked() ), this, SLOT ( removeImage() ) );
00098
00099 connect ( m_saveObjectButton, SIGNAL ( clicked() ), this, SLOT ( saveObject() ) );
00100 connect ( m_ObjectNameField, SIGNAL ( textEdited ( QString ) ), this, SLOT ( objectNameFieldEdited ( QString ) ) );
00101 connect ( m_ObjectNameField, SIGNAL ( returnPressed() ), this, SLOT ( saveObject() ) );
00102 connect ( m_TypeComboBox, SIGNAL ( activated( QString ) ), this, SLOT ( setObjectType( QString ) ) );
00103 connect ( m_ImageTable, SIGNAL ( cellClicked ( int, int ) ), this, SLOT ( rowSelected ( int, int ) ) );
00104
00105 m_LastImageFolder = "objectProperties";
00106
00107 m_saveObjectButton->setEnabled ( false );
00108 m_removeImageButton->setEnabled ( false );
00109
00110 m_SelectedRow = 99999;
00111
00112 setObjectType( "Object" );
00113
00114 m_ORLearnCommandPublisher = nh->advertise<or_msgs::OrLearnCommand>("/or/learn_commands", 10);
00115 m_Ready = true;
00116 }
00117
00118 THIS::~THIS() {}
00119
00120 void THIS::setObjectType ( QString type )
00121 {
00122 if(m_Ready)
00123 {
00124 or_msgs::OrLearnCommand learnCommandMsg;
00125 learnCommandMsg.command = ORLearningModule::SetObjectType;
00126 learnCommandMsg.string_value = type.toStdString();
00127 m_ORLearnCommandPublisher.publish(learnCommandMsg);
00128 }
00129 }
00130
00131 void THIS::saveImage()
00132 {
00133 std::string name = ( m_ImageNameField->text() ).toStdString();
00134
00135 if(m_Ready)
00136 {
00137 or_msgs::OrLearnCommand learnCommandMsg;
00138 learnCommandMsg.command = ORLearningModule::SaveImage;
00139 learnCommandMsg.string_value = name;
00140 m_ORLearnCommandPublisher.publish(learnCommandMsg);
00141 }
00142
00143
00144 m_SelectedRow = m_ImageTable->rowCount();
00145 }
00146
00147 void THIS::removeImage()
00148 {
00149 if ( m_SelectedRow < m_ImageTable->rowCount() )
00150 {
00151 if(m_Ready)
00152 {
00153 or_msgs::OrLearnCommand learnCommandMsg;
00154 learnCommandMsg.command = ORLearningModule::DeleteImage;
00155 learnCommandMsg.int_value = m_SelectedRow;
00156 m_ORLearnCommandPublisher.publish(learnCommandMsg);
00157 }
00158 }
00159 }
00160
00161 void THIS::saveObject()
00162 {
00163 std::string objName = ( m_ObjectNameField->text() ).toStdString();
00164 std::string objType = m_TypeComboBox->currentText().toStdString();
00165
00166
00167
00168 if(m_Ready)
00169 {
00170 or_msgs::OrLearnCommand learnCommandMsg;
00171 learnCommandMsg.command = ORLearningModule::SetObjectType;
00172 learnCommandMsg.string_value = objType;
00173 m_ORLearnCommandPublisher.publish(learnCommandMsg);
00174
00175 or_msgs::OrLearnCommand learnCommandMsg2;
00176 learnCommandMsg2.command = ORLearningModule::SaveObject;
00177 learnCommandMsg2.string_value = objName;
00178 m_ORLearnCommandPublisher.publish(learnCommandMsg2);
00179 }
00180
00181 m_ImageTable->setRowCount ( 0 );
00182 m_ObjectNameField->clear();
00183 m_saveObjectButton->setEnabled ( false );
00184 }
00185
00186 void THIS::objectNameFieldEdited ( QString text )
00187 {
00188 m_saveObjectButton->setEnabled ( text.length() > 0 && m_ImageTable->rowCount() > 0 );
00189
00190 }
00191
00192 void THIS::keyReleaseEvent(QKeyEvent * e){
00193 if(e->key() == Qt::Key_Up)
00194 {
00195 if(m_SelectedRow > 0)
00196 {
00197 m_SelectedRow--;
00198 updateTable();
00199 }
00200 }
00201 if(e->key() == Qt::Key_Down)
00202 {
00203 if(m_SelectedRow < m_ImageTable->rowCount()-1)
00204 {
00205 m_SelectedRow++;
00206 updateTable();
00207 }
00208 }
00209 }
00210
00211 void THIS::rowSelected ( int row, int col )
00212 {
00213 m_SelectedRow = row;
00214 updateTable();
00215 }
00216
00217 void THIS::updateTable()
00218 {
00219 QColor rowColor1 ( 235,235,235 );
00220 QColor rowColor2 ( 180,180,180 );
00221 for ( int i = 0; i < m_ImageTable->rowCount(); i++ )
00222 {
00223 if ( i == m_SelectedRow )
00224 {
00225 m_ImageTable->item ( i,0 )->setBackgroundColor ( rowColor2 );
00226 m_ImageTable->item ( i,1 )->setBackgroundColor ( rowColor2 );
00227 }
00228 else
00229 {
00230 m_ImageTable->item ( i,0 )->setBackgroundColor ( rowColor1 );
00231 m_ImageTable->item ( i,1 )->setBackgroundColor ( rowColor1 );
00232 }
00233 }
00234
00235 m_removeImageButton->setEnabled( m_SelectedRow < m_ImageTable->rowCount() );
00236 m_saveObjectButton->setEnabled( ( m_ObjectNameField->displayText().length() > 0 ) && ( m_ImageTable->rowCount() > 0 ) );
00237
00238
00239 if ( m_SelectedRow < m_ImageTable->rowCount() )
00240 {
00241 if(m_Ready)
00242 {
00243 or_msgs::OrLearnCommand learnCommandMsg;
00244 learnCommandMsg.command = ORLearningModule::DisplayImage;
00245 learnCommandMsg.int_value = m_SelectedRow;
00246 m_ORLearnCommandPublisher.publish(learnCommandMsg);
00247 }
00248 }
00249 else
00250 {
00251 m_SelectedRow = 99999;
00252 }
00253 }
00254
00255 void THIS::loadObject( )
00256 {
00257 QStringList files = QFileDialog::getOpenFileNames ( this, tr ( "Select Object" ), m_LastImageFolder, "Object Properties (*.objprop)\nAll files (*)" );
00258 if( files.size()>0)
00259 {
00260 QFileInfo fileInfo( files[0] );
00261 QString base = fileInfo.baseName();
00262 QString absolutePath = fileInfo.absoluteFilePath();
00263
00264 if(m_Ready)
00265 {
00266 ROS_INFO_STREAM("base: " << base.toStdString());
00267 ROS_INFO_STREAM("absolutePath: " << absolutePath.toStdString());
00268
00269 or_msgs::OrLearnCommand learnCommandMsg;
00270 learnCommandMsg.command = ORLearningModule::LoadObject;
00271 learnCommandMsg.string_value = absolutePath.toStdString();
00272 m_ORLearnCommandPublisher.publish(learnCommandMsg);
00273 }
00274
00275 m_ObjectNameField->setText( base );
00276 }
00277 }
00278
00279
00280 void THIS::updateImageTable(std::vector<std::string> &imageNames, std::string &objType)
00281 {
00282 ROS_INFO_STREAM("Updating table, adding item type: " << objType );
00283 m_ImageTable->setRowCount ( 0 );
00284
00285 for ( unsigned i = 0; i < imageNames.size(); i++ )
00286 {
00287 int indexRow = m_ImageTable->rowCount();
00288
00289
00290 m_ImageTable->insertRow ( indexRow );
00291 std::ostringstream s;
00292 s << i;
00293 QTableWidgetItem* item = new QTableWidgetItem ( QString( s.str().c_str() ) );
00294 item->setTextAlignment ( Qt::AlignCenter );
00295 item->setFlags ( Qt::ItemIsSelectable );
00296 m_ImageTable->setItem ( indexRow, 0, item );
00297
00298
00299 item = new QTableWidgetItem ( QString ( imageNames[i].c_str() ) );
00300 item->setTextAlignment ( Qt::AlignCenter );
00301 item->setFlags ( Qt::ItemIsSelectable );
00302 m_ImageTable->setItem ( indexRow, 1, item );
00303 }
00304
00305 updateTable();
00306
00307 m_TypeComboBox->setCurrentIndex( m_TypeComboBox->findText( objType.c_str() ) );
00308
00309 }
00310
00311 #undef THIS