ModelGenerationPanel.cpp
Go to the documentation of this file.
00001 
00012 // RAIL Pick and Place Tools
00013 #include "rail_pick_and_place_tools/ModelGenerationPanel.h"
00014 
00015 // ROS
00016 #include <pluginlib/class_list_macros.h>
00017 
00018 // Qt
00019 #include <QGridLayout>
00020 #include <QMessageBox>
00021 
00022 using namespace std;
00023 using namespace rail::pick_and_place;
00024 
00025 ModelGenerationPanel::ModelGenerationPanel(QWidget *parent)
00026     : rviz::Panel(parent), generate_models_ac_("/model_generator/generate_models", true),
00027       retrieve_grasp_ac_("/rail_grasp_retriever/retrieve_grasp", true),
00028       retrieve_grasp_model_ac_("/rail_grasp_model_retriever/retrieve_grasp_model", true)
00029 {
00030   // set defaults
00031   int port = graspdb::Client::DEFAULT_PORT;
00032   string host("127.0.0.1");
00033   string user("ros");
00034   string password("");
00035   string db("graspdb");
00036 
00037   // grab any parameters we need
00038   node_.getParam("/graspdb/host", host);
00039   node_.getParam("/graspdb/port", port);
00040   node_.getParam("/graspdb/user", user);
00041   node_.getParam("/graspdb/password", password);
00042   node_.getParam("/graspdb/db", db);
00043 
00044   // connect to the grasp database
00045   graspdb_ = new graspdb::Client(host, port, user, password, db);
00046   bool okay = graspdb_->connect();
00047 
00048   if (!okay)
00049   {
00050     ROS_WARN("Could not connect to grasp database.");
00051   }
00052 
00053   // list of current objects
00054   QHBoxLayout *objects_layout = new QHBoxLayout();
00055   QLabel *object_name_label = new QLabel("Object:");
00056   object_name_label->setAlignment(Qt::AlignRight);
00057   object_list_ = new QComboBox();
00058   objects_layout->addWidget(object_name_label);
00059   objects_layout->addWidget(object_list_);
00060   objects_layout->setAlignment(Qt::AlignCenter);
00061 
00062   // refresh and selection buttons
00063   QHBoxLayout *refresh_and_selections_layout = new QHBoxLayout();
00064   refresh_button_ = new QPushButton("Refresh");
00065   select_all_button_ = new QPushButton("Select All");
00066   deselect_all_button_ = new QPushButton("Deselect All");
00067   refresh_and_selections_layout->addWidget(refresh_button_);
00068   refresh_and_selections_layout->addWidget(select_all_button_);
00069   refresh_and_selections_layout->addWidget(deselect_all_button_);
00070 
00071   // delete button
00072   delete_button_ = new QPushButton("Delete");
00073   delete_button_->setEnabled(false);
00074 
00075   // grasp demonstration and model lists
00076   models_list_ = new QListWidget();
00077 
00078   // model generation options
00079   QHBoxLayout *generation_layout = new QHBoxLayout();
00080   // max model size options
00081   QLabel *model_size_label = new QLabel("Max Grasps Per Model:");
00082   model_size_label->setAlignment(Qt::AlignRight);
00083   model_size_spin_box_ = new QSpinBox();
00084   model_size_spin_box_->setRange(1, 99);
00085   model_size_spin_box_->setSingleStep(1);
00086   model_size_spin_box_->setValue(6);
00087   // model generation button
00088   generate_models_button_ = new QPushButton("Generate Models");
00089   generation_layout->addWidget(model_size_label);
00090   generation_layout->addWidget(model_size_spin_box_);
00091   generation_layout->addWidget(generate_models_button_);
00092 
00093   // action client feedback
00094   model_generation_status_ = new QLabel("Ready to generate models.");
00095 
00096   // build the final layout
00097   QVBoxLayout *layout = new QVBoxLayout();
00098   layout->addLayout(objects_layout);
00099   layout->addLayout(refresh_and_selections_layout);
00100   layout->addWidget(delete_button_);
00101   layout->addWidget(models_list_);
00102   layout->addLayout(generation_layout);
00103   layout->addWidget(model_generation_status_);
00104 
00105   // connect event callbacks
00106   QObject::connect(object_list_, SIGNAL(currentIndexChanged(
00107       const QString &)), this, SLOT(populateModelsList(
00108       const QString &)));
00109   QObject::connect(refresh_button_, SIGNAL(clicked()), this, SLOT(refresh()));
00110   QObject::connect(select_all_button_, SIGNAL(clicked()), this, SLOT(selectAll()));
00111   QObject::connect(deselect_all_button_, SIGNAL(clicked()), this, SLOT(deselectAll()));
00112   QObject::connect(delete_button_, SIGNAL(clicked()), this, SLOT(deleteModel()));
00113   QObject::connect(models_list_, SIGNAL(itemSelectionChanged()), this, SLOT(modelSelectionChanged()));
00114   QObject::connect(generate_models_button_, SIGNAL(clicked()), this, SLOT(executeGenerateModels()));
00115 
00116   // update with the initial state
00117   this->refresh();
00118 
00119   // set the final layout
00120   this->setLayout(layout);
00121 }
00122 
00123 ModelGenerationPanel::~ModelGenerationPanel()
00124 {
00125   // cleanup
00126   graspdb_->disconnect();
00127   delete graspdb_;
00128 }
00129 
00130 void ModelGenerationPanel::refresh()
00131 {
00132   // disable the button as we work
00133   refresh_button_->setEnabled(false);
00134 
00135   // clear the current objects
00136   object_list_->clear();
00137   // load from both the demonstration and model lists
00138   vector<string> object_names;
00139   vector<string> model_names;
00140   graspdb_->getUniqueGraspDemonstrationObjectNames(object_names);
00141   graspdb_->getUniqueGraspModelObjectNames(model_names);
00142   // combine the lists
00143   object_names.insert(object_names.end(), model_names.begin(), model_names.end());
00144   // sort the names
00145   sort(object_names.begin(), object_names.end());
00146   // make the lise unique
00147   object_names.erase(unique(object_names.begin(), object_names.end()), object_names.end());
00148   // add each item name
00149   for (size_t i = 0; i < object_names.size(); i++)
00150   {
00151     object_list_->addItem(object_names[i].c_str());
00152   }
00153 
00154   // re-enable the button
00155   refresh_button_->setEnabled(true);
00156 }
00157 
00158 void ModelGenerationPanel::selectAll()
00159 {
00160   // disable the button as we work
00161   select_all_button_->setEnabled(false);
00162 
00163   // simply go through the current list
00164   for (size_t i = 0; i < models_list_->count(); i++)
00165   {
00166     if (models_list_->item(i)->flags() & Qt::ItemIsUserCheckable)
00167     {
00168       models_list_->item(i)->setCheckState(Qt::Checked);
00169     }
00170   }
00171 
00172   // re-enable the button
00173   select_all_button_->setEnabled(true);
00174 }
00175 
00176 void ModelGenerationPanel::deselectAll()
00177 {
00178   // disable the button as we work
00179   deselect_all_button_->setEnabled(false);
00180 
00181   // simply go through the current list
00182   for (size_t i = 0; i < models_list_->count(); i++)
00183   {
00184     if (models_list_->item(i)->flags() & Qt::ItemIsUserCheckable)
00185     {
00186       models_list_->item(i)->setCheckState(Qt::Unchecked);
00187     }
00188   }
00189 
00190   // re-enable the button
00191   deselect_all_button_->setEnabled(true);
00192 }
00193 
00194 void ModelGenerationPanel::modelSelectionChanged()
00195 {
00196   // check if there is an item selected
00197   if (models_list_->currentItem() != NULL && models_list_->currentItem()->flags() & Qt::ItemIsUserCheckable)
00198   {
00199     // grab the current item
00200     string selected_item = models_list_->currentItem()->text().toStdString();
00201     // extract the ID
00202     int id = atoi(selected_item.substr(selected_item.find(' ')).c_str());
00203 
00204     // enable the delete button
00205     string delete_text = "Delete " + selected_item;
00206     delete_button_->setText(delete_text.c_str());
00207     delete_button_->setEnabled(true);
00208 
00209     // make calls to visualize the model or grasp
00210     if (selected_item[0] == 'G' && retrieve_grasp_ac_.isServerConnected())
00211     {
00212       rail_pick_and_place_msgs::RetrieveGraspDemonstrationGoal goal;
00213       goal.id = id;
00214       retrieve_grasp_ac_.sendGoal(goal);
00215     } else if (retrieve_grasp_model_ac_.isServerConnected())
00216     {
00217       rail_pick_and_place_msgs::RetrieveGraspModelGoal goal;
00218       goal.id = id;
00219       retrieve_grasp_model_ac_.sendGoal(goal);
00220     }
00221   } else
00222   {
00223     // disable the delete button
00224     delete_button_->setText("Delete");
00225     delete_button_->setEnabled(false);
00226   }
00227 }
00228 
00229 void ModelGenerationPanel::deleteModel()
00230 {
00231   if (models_list_->currentItem() != NULL)
00232   {
00233     // grab the current item
00234     string selected_item = models_list_->currentItem()->text().toStdString();
00235     // extract the ID
00236     int id = atoi(selected_item.substr(selected_item.find(' ')).c_str());
00237 
00238     // confirmation dialog
00239     string delete_text = "Are you sure you want to delete " + selected_item + "?";
00240     QMessageBox::StandardButton confirm = QMessageBox::question(this, "Delete?", delete_text.c_str(),
00241         QMessageBox::Yes | QMessageBox::No);
00242     if (confirm == QMessageBox::Yes)
00243     {
00244       // check for a grasp or a model
00245       if (selected_item[0] == 'G')
00246       {
00247         graspdb_->deleteGraspDemonstration(id);
00248       } else
00249       {
00250         graspdb_->deleteGraspModel(id);
00251       }
00252       delete models_list_->currentItem();
00253     }
00254   }
00255 }
00256 
00257 void ModelGenerationPanel::populateModelsList(const QString &text)
00258 {
00259   // check if an object exists
00260   if (object_list_->count() > 0)
00261   {
00262     // clear the current list
00263     models_list_->clear();
00264 
00265     // load grasps/models
00266     vector<graspdb::GraspDemonstration> demonstrations;
00267     vector<graspdb::GraspModel> models;
00268     graspdb_->loadGraspDemonstrationsByObjectName(text.toStdString(), demonstrations);
00269     graspdb_->loadGraspModelsByObjectName(text.toStdString(), models);
00270 
00271     // first add grasp demonstrations
00272     if (demonstrations.size() > 0)
00273     {
00274       // header
00275       QListWidgetItem *grasps_label = new QListWidgetItem("--Grasp Demonstrations--", models_list_);
00276       grasps_label->setTextAlignment(Qt::AlignCenter);
00277       // makes it so the user can't select this label
00278       grasps_label->setFlags(Qt::ItemIsEnabled);
00279       // add each demonstration
00280       for (size_t i = 0; i < demonstrations.size(); i++)
00281       {
00282         stringstream ss;
00283         ss << "Grasp " << demonstrations[i].getID();
00284         QListWidgetItem *item = new QListWidgetItem(ss.str().c_str(), models_list_);
00285         item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
00286         item->setCheckState(Qt::Unchecked);
00287       }
00288     }
00289 
00290     if (models.size() > 0)
00291     {
00292       QListWidgetItem *models_label = new QListWidgetItem("--Object Models--", models_list_);
00293       models_label->setTextAlignment(Qt::AlignCenter);
00294       // makes it so the user can't select this label
00295       models_label->setFlags(Qt::ItemIsEnabled);
00296       // add each model
00297       for (size_t i = 0; i < models.size(); i++)
00298       {
00299         stringstream ss;
00300         ss << "Model " << models[i].getID();
00301         QListWidgetItem *item = new QListWidgetItem(ss.str().c_str(), models_list_);
00302         item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
00303         item->setCheckState(Qt::Unchecked);
00304       }
00305     }
00306   }
00307 }
00308 
00309 void ModelGenerationPanel::executeGenerateModels()
00310 {
00311   // disable the button first
00312   generate_models_button_->setEnabled(false);
00313 
00314   if (!generate_models_ac_.isServerConnected())
00315   {
00316     model_generation_status_->setText("Model generation action server not found!");
00317     // make sure to re-enable the button
00318     generate_models_button_->setEnabled(true);
00319   } else
00320   {
00321     // grab the selected models
00322     rail_pick_and_place_msgs::GenerateModelsGoal goal;
00323     for (size_t i = 0; i < models_list_->count(); i++)
00324     {
00325       if (models_list_->item(i)->checkState() == Qt::Checked)
00326       {
00327         // grab the current item
00328         string selected_item = models_list_->item(i)->text().toStdString();
00329         // extract the ID
00330         int id = atoi(selected_item.substr(selected_item.find(' ')).c_str());
00331 
00332         if (selected_item[0] == 'G')
00333         {
00334           goal.grasp_demonstration_ids.push_back(id);
00335         } else
00336         {
00337           goal.grasp_model_ids.push_back(id);
00338         }
00339       }
00340     }
00341 
00342     // check the size
00343     if (goal.grasp_demonstration_ids.size() + goal.grasp_model_ids.size() == 0)
00344     {
00345       model_generation_status_->setText("No grasps or models selected.");
00346       // make sure to re-enable the button
00347       generate_models_button_->setEnabled(true);
00348     } else
00349     {
00350       // get the max model size
00351       goal.max_model_size = model_size_spin_box_->value();
00352       // send the goal asynchronously
00353       generate_models_ac_.sendGoal(goal, boost::bind(&ModelGenerationPanel::doneCallback, this, _1, _2),
00354           actionlib::SimpleActionClient<rail_pick_and_place_msgs::GenerateModelsAction>::SimpleActiveCallback(),
00355           boost::bind(&ModelGenerationPanel::feedbackCallback, this, _1));
00356     }
00357   }
00358 }
00359 
00360 void ModelGenerationPanel::doneCallback(const actionlib::SimpleClientGoalState &state,
00361     const rail_pick_and_place_msgs::GenerateModelsResultConstPtr &result)
00362 {
00363   // check if the action was successful
00364   if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00365   {
00366     // check how models were generated
00367     if (result->new_model_ids.size() > 0)
00368     {
00369       // built the final status string
00370       stringstream ss;
00371       ss << result->new_model_ids.size() << " model(s) successfully stored with ID(s) [";
00372       // update the model list while getting the IDs
00373       for (size_t i = 0; i < result->new_model_ids.size(); i++)
00374       {
00375         // add to the status message
00376         ss << result->new_model_ids[i];
00377         if ((int) i <= ((int) result->new_model_ids.size()) - 2)
00378         {
00379           ss << ", ";
00380         }
00381 
00382         // add to the models list
00383         stringstream ss2;
00384         ss2 << "Model " << result->new_model_ids[i];
00385         QListWidgetItem *item = new QListWidgetItem(ss2.str().c_str(), models_list_);
00386         item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
00387         item->setCheckState(Qt::Unchecked);
00388       }
00389       ss << "].";
00390       model_generation_status_->setText(ss.str().c_str());
00391     } else
00392     {
00393       model_generation_status_->setText("No valid models generated.");
00394     }
00395   } else
00396   {
00397     // state text should represent what went wrong
00398     model_generation_status_->setText(state.getText().c_str());
00399   }
00400 
00401   // re-enable the button
00402   generate_models_button_->setEnabled(true);
00403 }
00404 
00405 void ModelGenerationPanel::feedbackCallback(const rail_pick_and_place_msgs::GenerateModelsFeedbackConstPtr &feedback)
00406 {
00407   // simply set the status to the current message
00408   model_generation_status_->setText(feedback->message.c_str());
00409 }
00410 
00411 void ModelGenerationPanel::save(rviz::Config config) const
00412 {
00413   // first call the super class
00414   rviz::Panel::save(config);
00415   // save the model config
00416   config.mapSetValue("MaxModelSize", model_size_spin_box_->value());
00417 }
00418 
00419 void ModelGenerationPanel::load(const rviz::Config &config)
00420 {
00421   // first call the super class
00422   rviz::Panel::load(config);
00423   // load the model config
00424   int max_model_size;
00425   if (config.mapGetInt("MaxModelSize", &max_model_size))
00426   {
00427     model_size_spin_box_->setValue(max_model_size);
00428   }
00429 }
00430 
00431 // tell pluginlib about this class (must outside of any namespace scope)
00432 PLUGINLIB_EXPORT_CLASS(rail::pick_and_place::ModelGenerationPanel, rviz::Panel)


rail_pick_and_place_tools
Author(s): Russell Toris , David Kent
autogenerated on Thu Jun 6 2019 19:44:16