GraspCollectionPanel.cpp
Go to the documentation of this file.
00001 
00012 // RAIL Pick and Place Tools
00013 #include <rail_pick_and_place_tools/GraspCollectionPanel.h>
00014 
00015 // ROS
00016 #include <pluginlib/class_list_macros.h>
00017 
00018 // Qt
00019 #include <QBoxLayout>
00020 
00021 using namespace std;
00022 using namespace rail::pick_and_place;
00023 
00024 GraspCollectionPanel::GraspCollectionPanel(QWidget *parent)
00025     : rviz::Panel(parent), grasp_and_store_ac_("/rail_grasp_collection/grasp_and_store", true)
00026 {
00027   // grasp collection options
00028   QHBoxLayout *first_row_layout = new QHBoxLayout();
00029   QLabel *options_label = new QLabel("Options:");
00030   options_label->setAlignment(Qt::AlignRight);
00031   lift_box_ = new QCheckBox("Lift Object");
00032   verify_box_ = new QCheckBox("Verify Grasp");
00033   first_row_layout->addWidget(lift_box_, 0, Qt::AlignLeft);
00034   first_row_layout->addWidget(verify_box_, 0, Qt::AlignLeft);
00035   first_row_layout->addStretch();
00036 
00037   // grasp collection input
00038   QHBoxLayout *second_row_layout = new QHBoxLayout();
00039   QLabel *name_label = new QLabel("Object name:");
00040   name_label->setAlignment(Qt::AlignRight);
00041   name_input_ = new QLineEdit();
00042   grasp_and_store_button_ = new QPushButton("Grasp");
00043   second_row_layout->addWidget(name_input_);
00044   second_row_layout->addWidget(grasp_and_store_button_);
00045 
00046   // organizational layout
00047   QGridLayout *grid_layout = new QGridLayout();
00048   grid_layout->addWidget(options_label, 0, 0);
00049   grid_layout->addWidget(name_label, 1, 0);
00050   grid_layout->addLayout(first_row_layout, 0, 1);
00051   grid_layout->addLayout(second_row_layout, 1, 1);
00052 
00053   // grasp collection feedback
00054   grasp_and_store_status_ = new QLabel("Ready to collect grasp data.");
00055   grasp_and_store_status_->setAlignment(Qt::AlignCenter);
00056 
00057   // build final layout
00058   QVBoxLayout *layout = new QVBoxLayout();
00059   layout->addLayout(grid_layout);
00060   layout->addWidget(grasp_and_store_status_);
00061 
00062   // connect the grasp button
00063   QObject::connect(grasp_and_store_button_, SIGNAL(clicked()), this, SLOT(executeGraspAndStore()));
00064 
00065   // set the final layout
00066   this->setLayout(layout);
00067 }
00068 
00069 void GraspCollectionPanel::executeGraspAndStore()
00070 {
00071   // disable the button first
00072   grasp_and_store_button_->setEnabled(false);
00073 
00074   // check if the action server exists
00075   if (!grasp_and_store_ac_.isServerConnected())
00076   {
00077     grasp_and_store_status_->setText("Grasp and store action server not found!");
00078     // make sure to re-enable the button
00079     grasp_and_store_button_->setEnabled(true);
00080   } else
00081   {
00082     // set options for the goal from the GUI
00083     rail_pick_and_place_msgs::GraspAndStoreGoal goal;
00084     goal.lift = lift_box_->isChecked();
00085     goal.verify = verify_box_->isChecked();
00086     goal.object_name = name_input_->text().toStdString();
00087     // send the goal asynchronously
00088     grasp_and_store_ac_.sendGoal(goal, boost::bind(&GraspCollectionPanel::doneCallback, this, _1, _2),
00089         actionlib::SimpleActionClient<rail_pick_and_place_msgs::GraspAndStoreAction>::SimpleActiveCallback(),
00090         boost::bind(&GraspCollectionPanel::feedbackCallback, this, _1));
00091   }
00092 }
00093 
00094 void GraspCollectionPanel::doneCallback(const actionlib::SimpleClientGoalState &state,
00095     const rail_pick_and_place_msgs::GraspAndStoreResultConstPtr &result)
00096 {
00097   // check if the action was successful
00098   if (state == actionlib::SimpleClientGoalState::SUCCEEDED && result->success)
00099   {
00100     // built the final status string
00101     stringstream ss;
00102     ss << "Grasp demonstration successfully stored with ID " << result->id << ".";
00103     grasp_and_store_status_->setText(ss.str().c_str());
00104   }
00105   else
00106   {
00107     // state text should represent what went wrong
00108     grasp_and_store_status_->setText(state.getText().c_str());
00109   }
00110 
00111   // re-enable the button
00112   grasp_and_store_button_->setEnabled(true);
00113 }
00114 
00115 void GraspCollectionPanel::feedbackCallback(const rail_pick_and_place_msgs::GraspAndStoreFeedbackConstPtr &feedback)
00116 {
00117   // simply set the status to the current message
00118   grasp_and_store_status_->setText(feedback->message.c_str());
00119 }
00120 
00121 void GraspCollectionPanel::save(rviz::Config config) const
00122 {
00123   // first call the super class
00124   rviz::Panel::save(config);
00125 
00126   // save the state of the check boxes
00127   config.mapSetValue("LiftEnabled", lift_box_->isChecked());
00128   config.mapSetValue("VerifyEnabled", verify_box_->isChecked());
00129 }
00130 
00131 void GraspCollectionPanel::load(const rviz::Config &config)
00132 {
00133   // first call the super class
00134   rviz::Panel::load(config);
00135 
00136   // check the state of each check box
00137   bool flag;
00138   lift_box_->setChecked(config.mapGetBool("LiftEnabled", &flag) && flag);
00139   verify_box_->setChecked(config.mapGetBool("VerifyEnabled", &flag) && flag);
00140 }
00141 
00142 // tell pluginlib about this class (must outside of any namespace scope)
00143 PLUGINLIB_EXPORT_CLASS(rail::pick_and_place::GraspCollectionPanel, rviz::Panel)


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