MetricTrainingPanel.cpp
Go to the documentation of this file.
00001 
00011 // RAIL Pick and Place Tools
00012 #include "rail_pick_and_place_tools/MetricTrainingPanel.h"
00013 
00014 // ROS
00015 #include <pluginlib/class_list_macros.h>
00016 
00017 // Qt
00018 #include <QBoxLayout>
00019 
00020 using namespace std;
00021 using namespace rail::pick_and_place;
00022 
00023 MetricTrainingPanel::MetricTrainingPanel(QWidget *parent)
00024     : rviz::Panel(parent), train_metrics_ac_("/metric_trainer/train_metrics", true),
00025       as_(node_, "/metric_trainer/get_yes_no_feedback", boost::bind(&MetricTrainingPanel::getYesNoFeedbackCallback,
00026           this, _1), false)
00027 {
00028   responded_ = false;
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   as_.start();
00054 
00055   // list of current objects
00056   QHBoxLayout *objects_layout = new QHBoxLayout();
00057   QLabel *object_name_label = new QLabel("Object:");
00058   object_name_label->setAlignment(Qt::AlignRight);
00059   object_list_ = new QComboBox();
00060   objects_layout->addWidget(object_name_label);
00061   objects_layout->addWidget(object_list_);
00062   objects_layout->setAlignment(Qt::AlignCenter);
00063 
00064   // refresh and train buttons
00065   QHBoxLayout *buttons_layout = new QHBoxLayout();
00066   refresh_button_ = new QPushButton("Refresh");
00067   train_metrics_button_ = new QPushButton("Begin Training");
00068   buttons_layout->addWidget(refresh_button_);
00069   buttons_layout->addWidget(train_metrics_button_);
00070   buttons_layout->setAlignment(Qt::AlignCenter);
00071 
00072   // service feedback
00073   train_metrics_status_ = new QLabel("Ready to train.");
00074   train_metrics_status_->setAlignment(Qt::AlignCenter);
00075 
00076   // yes/no response buttons
00077   QHBoxLayout *response_layout = new QHBoxLayout();
00078   QLabel *response_label = new QLabel("Valid Registration?");
00079   object_name_label->setAlignment(Qt::AlignRight);
00080   yes_button_ = new QPushButton("Yes");
00081   no_button_ = new QPushButton("No");
00082   // initially disabled (waits for response request)
00083   yes_button_->setEnabled(false);
00084   no_button_->setEnabled(false);
00085   response_layout->addWidget(response_label);
00086   response_layout->addWidget(yes_button_);
00087   response_layout->addWidget(no_button_);
00088   response_layout->setAlignment(Qt::AlignCenter);
00089 
00090   // build the final layout
00091   QVBoxLayout *layout = new QVBoxLayout();
00092   layout->addLayout(objects_layout);
00093   layout->addLayout(buttons_layout);
00094   layout->addWidget(train_metrics_status_);
00095   layout->addLayout(response_layout);
00096 
00097   // connect the buttons
00098   QObject::connect(refresh_button_, SIGNAL(clicked()), this, SLOT(refresh()));
00099   QObject::connect(train_metrics_button_, SIGNAL(clicked()), this, SLOT(executeTrainMetrics()));
00100   QObject::connect(yes_button_, SIGNAL(clicked()), this, SLOT(setYesFeedback()));
00101   QObject::connect(no_button_, SIGNAL(clicked()), this, SLOT(setNoFeedback()));
00102 
00103   // update with the initial state
00104   this->refresh();
00105 
00106   // set the final layout
00107   this->setLayout(layout);
00108 }
00109 
00110 MetricTrainingPanel::~MetricTrainingPanel()
00111 {
00112   // cleanup
00113   as_.shutdown();
00114   graspdb_->disconnect();
00115   delete graspdb_;
00116 }
00117 
00118 void MetricTrainingPanel::getYesNoFeedbackCallback(const rail_pick_and_place_msgs::GetYesNoFeedbackGoalConstPtr &goal)
00119 {
00120   // enable the buttons
00121   yes_button_->setEnabled(true);
00122   no_button_->setEnabled(true);
00123 
00124   // wait for a response from the user
00125   ros::Rate rate(10);
00126   while (ros::ok())
00127   {
00128     // lock the flag
00129     {
00130       boost::mutex::scoped_lock lock(mutex_);
00131       if (responded_)
00132       {
00133         // reset the flag
00134         responded_ = false;
00135 
00136         // send the response
00137         rail_pick_and_place_msgs::GetYesNoFeedbackResult result;
00138         result.yes = yes_;
00139         as_.setSucceeded(result);
00140 
00141         // disable the buttons
00142         yes_button_->setEnabled(false);
00143         no_button_->setEnabled(false);
00144         return;
00145       }
00146     }
00147     rate.sleep();
00148   }
00149 }
00150 
00151 void MetricTrainingPanel::setYesFeedback()
00152 {
00153   // lock the flag
00154   boost::mutex::scoped_lock lock(mutex_);
00155   yes_ = true;
00156   responded_ = true;
00157 }
00158 
00159 void MetricTrainingPanel::setNoFeedback()
00160 {
00161   // lock the flag
00162   boost::mutex::scoped_lock lock(mutex_);
00163   yes_ = false;
00164   responded_ = true;
00165 }
00166 
00167 void MetricTrainingPanel::refresh()
00168 {
00169   // disable the button as we work
00170   refresh_button_->setEnabled(false);
00171 
00172   // clear the current objects
00173   object_list_->clear();
00174   // load from both the demonstration and model lists
00175   vector<string> object_names;
00176   graspdb_->getUniqueGraspDemonstrationObjectNames(object_names);
00177   // sort the names
00178   sort(object_names.begin(), object_names.end());
00179   // add each item name
00180   for (size_t i = 0; i < object_names.size(); i++)
00181   {
00182     object_list_->addItem(object_names[i].c_str());
00183   }
00184 
00185   // re-enable the button
00186   refresh_button_->setEnabled(true);
00187 }
00188 
00189 void MetricTrainingPanel::executeTrainMetrics()
00190 {
00191   // disable the button first
00192   train_metrics_button_->setEnabled(false);
00193 
00194   if (!train_metrics_ac_.isServerConnected())
00195   {
00196     train_metrics_status_->setText("Train metrics action server not found!");
00197     // make sure to re-enable the button
00198     train_metrics_button_->setEnabled(true);
00199   } else
00200   {
00201     // set options for the goal from the GUI
00202     rail_pick_and_place_msgs::TrainMetricsGoal goal;
00203     goal.object_name = object_list_->currentText().toStdString();
00204     // send the goal asynchronously
00205     train_metrics_ac_.sendGoal(goal, boost::bind(&MetricTrainingPanel::doneCallback, this, _1, _2),
00206         actionlib::SimpleActionClient<rail_pick_and_place_msgs::TrainMetricsAction>::SimpleActiveCallback(),
00207         boost::bind(&MetricTrainingPanel::feedbackCallback, this, _1));
00208   }
00209 }
00210 
00211 void MetricTrainingPanel::doneCallback(const actionlib::SimpleClientGoalState &state,
00212     const rail_pick_and_place_msgs::TrainMetricsResultConstPtr &result)
00213 {
00214   // check if the action was successful
00215   if (state == actionlib::SimpleClientGoalState::SUCCEEDED && result->success)
00216   {
00217     train_metrics_status_->setText("Metric training finished and saved.");
00218   }
00219   else
00220   {
00221     // state text should represent what went wrong
00222     train_metrics_status_->setText(state.getText().c_str());
00223   }
00224 
00225   // re-enable the button
00226   train_metrics_button_->setEnabled(true);
00227 }
00228 
00229 void MetricTrainingPanel::feedbackCallback(const rail_pick_and_place_msgs::TrainMetricsFeedbackConstPtr &feedback)
00230 {
00231   // simply set the status to the current message
00232   train_metrics_status_->setText(feedback->message.c_str());
00233 }
00234 
00235 void MetricTrainingPanel::save(rviz::Config config) const
00236 {
00237   // simply call the super class
00238   rviz::Panel::save(config);
00239 }
00240 
00241 void MetricTrainingPanel::load(const rviz::Config &config)
00242 {
00243   // simply call the super class
00244   rviz::Panel::load(config);
00245 }
00246 
00247 // tell pluginlib about this class (must outside of any namespace scope)
00248 PLUGINLIB_EXPORT_CLASS(rail::pick_and_place::MetricTrainingPanel, rviz::Panel)


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