robot_poses_widget.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman */
00036 
00037 // SA
00038 #include "robot_poses_widget.h"
00039 #include <moveit_msgs/JointLimits.h>
00040 // Qt
00041 #include <QFormLayout>
00042 #include <QMessageBox>
00043 #include <QDoubleValidator>
00044 #include <QApplication>
00045 
00046 #include <moveit/robot_state/conversions.h>
00047 #include <moveit_msgs/DisplayRobotState.h>
00048 
00049 namespace moveit_setup_assistant
00050 {
00051 // ******************************************************************************************
00052 // Outer User Interface for MoveIt Configuration Assistant
00053 // ******************************************************************************************
00054 RobotPosesWidget::RobotPosesWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
00055   : SetupScreenWidget(parent), config_data_(config_data)
00056 {
00057   // Set pointer to null so later we can tell if we need to delete it
00058   joint_list_layout_ = NULL;
00059 
00060   // Basic widget container
00061   QVBoxLayout* layout = new QVBoxLayout();
00062 
00063   // Top Header Area ------------------------------------------------
00064 
00065   HeaderWidget* header =
00066       new HeaderWidget("Robot Poses", "Create poses for the robot. Poses are defined as sets of joint values for "
00067                                       "particular planning groups. This is useful for things like <i>folded arms</i>.",
00068                        this);
00069   layout->addWidget(header);
00070 
00071   // Create contents screens ---------------------------------------
00072   pose_list_widget_ = createContentsWidget();
00073   pose_edit_widget_ = createEditWidget();
00074 
00075   // Create stacked layout -----------------------------------------
00076   stacked_layout_ = new QStackedLayout(this);
00077   stacked_layout_->addWidget(pose_list_widget_);  // screen index 0
00078   stacked_layout_->addWidget(pose_edit_widget_);  // screen index 1
00079 
00080   // Create Widget wrapper for layout
00081   QWidget* stacked_layout_widget = new QWidget(this);
00082   stacked_layout_widget->setLayout(stacked_layout_);
00083 
00084   layout->addWidget(stacked_layout_widget);
00085 
00086   // Finish Layout --------------------------------------------------
00087   this->setLayout(layout);
00088 
00089   // Create joint publisher -----------------------------------------
00090   ros::NodeHandle nh;
00091 
00092   // Create scene publisher for later use
00093   pub_robot_state_ = nh.advertise<moveit_msgs::DisplayRobotState>(MOVEIT_ROBOT_STATE, 1);
00094 
00095   // Set the planning scene
00096   config_data_->getPlanningScene()->setName("MoveIt Planning Scene");
00097 
00098   // Collision Detection initializtion -------------------------------
00099 
00100   // Setup the request
00101   request.contacts = true;
00102   request.max_contacts = 1;
00103   request.max_contacts_per_pair = 1;
00104   request.verbose = false;
00105 }
00106 
00107 // ******************************************************************************************
00108 // Create the main content widget
00109 // ******************************************************************************************
00110 QWidget* RobotPosesWidget::createContentsWidget()
00111 {
00112   // Main widget
00113   QWidget* content_widget = new QWidget(this);
00114 
00115   // Basic widget container
00116   QVBoxLayout* layout = new QVBoxLayout(this);
00117 
00118   // Table ------------ ------------------------------------------------
00119 
00120   data_table_ = new QTableWidget(this);
00121   data_table_->setColumnCount(2);
00122   data_table_->setSortingEnabled(true);
00123   data_table_->setSelectionBehavior(QAbstractItemView::SelectRows);
00124   connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int)));
00125   connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int)));
00126   layout->addWidget(data_table_);
00127 
00128   // Set header labels
00129   QStringList header_list;
00130   header_list.append("Pose Name");
00131   header_list.append("Group Name");
00132   data_table_->setHorizontalHeaderLabels(header_list);
00133 
00134   // Bottom Buttons --------------------------------------------------
00135 
00136   QHBoxLayout* controls_layout = new QHBoxLayout();
00137 
00138   // Set Default Button
00139   QPushButton* btn_default = new QPushButton("&Show Default Pose", this);
00140   btn_default->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00141   btn_default->setMaximumWidth(300);
00142   connect(btn_default, SIGNAL(clicked()), this, SLOT(showDefaultPose()));
00143   controls_layout->addWidget(btn_default);
00144   controls_layout->setAlignment(btn_default, Qt::AlignLeft);
00145 
00146   // Set play button
00147   QPushButton* btn_play = new QPushButton("&MoveIt!", this);
00148   btn_play->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00149   btn_play->setMaximumWidth(300);
00150   connect(btn_play, SIGNAL(clicked()), this, SLOT(playPoses()));
00151   controls_layout->addWidget(btn_play);
00152   controls_layout->setAlignment(btn_play, Qt::AlignLeft);
00153 
00154   // Spacer
00155   QWidget* spacer = new QWidget(this);
00156   spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00157   controls_layout->addWidget(spacer);
00158 
00159   // Edit Selected Button
00160   btn_edit_ = new QPushButton("&Edit Selected", this);
00161   btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00162   btn_edit_->setMaximumWidth(300);
00163   btn_edit_->hide();  // show once we know if there are existing poses
00164   connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
00165   controls_layout->addWidget(btn_edit_);
00166   controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
00167 
00168   // Delete
00169   btn_delete_ = new QPushButton("&Delete Selected", this);
00170   connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteSelected()));
00171   controls_layout->addWidget(btn_delete_);
00172   controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
00173 
00174   // Add Group Button
00175   QPushButton* btn_add = new QPushButton("&Add Pose", this);
00176   btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00177   btn_add->setMaximumWidth(300);
00178   connect(btn_add, SIGNAL(clicked()), this, SLOT(showNewScreen()));
00179   controls_layout->addWidget(btn_add);
00180   controls_layout->setAlignment(btn_add, Qt::AlignRight);
00181 
00182   // Add layout
00183   layout->addLayout(controls_layout);
00184 
00185   // Set layout -----------------------------------------------------
00186   content_widget->setLayout(layout);
00187 
00188   return content_widget;
00189 }
00190 
00191 // ******************************************************************************************
00192 // Create the edit widget
00193 // ******************************************************************************************
00194 QWidget* RobotPosesWidget::createEditWidget()
00195 {
00196   // Main widget
00197   QWidget* edit_widget = new QWidget(this);
00198   // Layout
00199   QVBoxLayout* layout = new QVBoxLayout();
00200 
00201   // 2 columns -------------------------------------------------------
00202 
00203   QHBoxLayout* columns_layout = new QHBoxLayout();
00204   QVBoxLayout* column1 = new QVBoxLayout();
00205   column2_ = new QVBoxLayout();
00206 
00207   // Column 1 --------------------------------------------------------
00208 
00209   // Simple form -------------------------------------------
00210   QFormLayout* form_layout = new QFormLayout();
00211   // form_layout->setContentsMargins( 0, 15, 0, 15 );
00212   form_layout->setRowWrapPolicy(QFormLayout::WrapAllRows);
00213 
00214   // Name input
00215   pose_name_field_ = new QLineEdit(this);
00216   // pose_name_field_->setMaximumWidth( 300 );
00217   form_layout->addRow("Pose Name:", pose_name_field_);
00218 
00219   // Group name input
00220   group_name_field_ = new QComboBox(this);
00221   group_name_field_->setEditable(false);
00222   // Connect the signal for changes to the drop down box
00223   connect(group_name_field_, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(loadJointSliders(const QString&)));
00224   // group_name_field_->setMaximumWidth( 300 );
00225   form_layout->addRow("Planning Group:", group_name_field_);
00226 
00227   // Indicator that robot is in collision or not
00228   collision_warning_ = new QLabel("<font color='red'><b>Robot in Collision State</b></font>", this);
00229   collision_warning_->setTextFormat(Qt::RichText);
00230   collision_warning_->hide();  // show later
00231   form_layout->addRow(" ", collision_warning_);
00232 
00233   column1->addLayout(form_layout);
00234   columns_layout->addLayout(column1);
00235 
00236   // Column 2 --------------------------------------------------------
00237 
00238   // Box to hold joint sliders
00239   joint_list_widget_ = new QWidget(this);
00240 
00241   // Create scroll area
00242   scroll_area_ = new QScrollArea(this);
00243   // scroll_area_->setBackgroundRole(QPalette::Dark);
00244   scroll_area_->setWidget(joint_list_widget_);
00245 
00246   column2_->addWidget(scroll_area_);
00247 
00248   columns_layout->addLayout(column2_);
00249 
00250   // Set columns in main layout
00251   layout->addLayout(columns_layout);
00252 
00253   // Bottom Buttons --------------------------------------------------
00254 
00255   QHBoxLayout* controls_layout = new QHBoxLayout();
00256   controls_layout->setContentsMargins(0, 25, 0, 15);
00257 
00258   // Spacer
00259   QWidget* spacer = new QWidget(this);
00260   spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00261   controls_layout->addWidget(spacer);
00262 
00263   // Save
00264   QPushButton* btn_save_ = new QPushButton("&Save", this);
00265   btn_save_->setMaximumWidth(200);
00266   connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing()));
00267   controls_layout->addWidget(btn_save_);
00268   controls_layout->setAlignment(btn_save_, Qt::AlignRight);
00269 
00270   // Cancel
00271   QPushButton* btn_cancel_ = new QPushButton("&Cancel", this);
00272   btn_cancel_->setMaximumWidth(200);
00273   connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing()));
00274   controls_layout->addWidget(btn_cancel_);
00275   controls_layout->setAlignment(btn_cancel_, Qt::AlignRight);
00276 
00277   // Add layout
00278   layout->addLayout(controls_layout);
00279 
00280   // Set layout -----------------------------------------------------
00281   edit_widget->setLayout(layout);
00282 
00283   return edit_widget;
00284 }
00285 
00286 // ******************************************************************************************
00287 // Show edit screen for creating a new pose
00288 // ******************************************************************************************
00289 void RobotPosesWidget::showNewScreen()
00290 {
00291   // Switch to screen - do this before clearEditText()
00292   stacked_layout_->setCurrentIndex(1);
00293 
00294   // Remember that this is a new pose
00295   current_edit_pose_.clear();
00296 
00297   // Manually send the load joint sliders signal
00298   if (!group_name_field_->currentText().isEmpty())
00299     loadJointSliders(group_name_field_->currentText());
00300 
00301   // Clear previous data
00302   pose_name_field_->setText("");
00303 
00304   // Announce that this widget is in modal mode
00305   Q_EMIT isModal(true);
00306 }
00307 
00308 // ******************************************************************************************
00309 // Edit whatever element is selected
00310 // ******************************************************************************************
00311 void RobotPosesWidget::editDoubleClicked(int row, int column)
00312 {
00313   // We'll just base the edit on the selection highlight
00314   editSelected();
00315 }
00316 
00317 // ******************************************************************************************
00318 // Preview whatever element is selected
00319 // ******************************************************************************************
00320 void RobotPosesWidget::previewClicked(int row, int column)
00321 {
00322   // Get list of all selected items
00323   QList<QTableWidgetItem*> selected = data_table_->selectedItems();
00324 
00325   // Check that an element was selected
00326   if (!selected.size())
00327     return;
00328 
00329   // Find the selected in datastructure
00330   srdf::Model::GroupState* pose = findPoseByName(selected[0]->text().toStdString());
00331 
00332   showPose(pose);
00333 }
00334 
00335 // ******************************************************************************************
00336 // Show the robot in the current pose
00337 // ******************************************************************************************
00338 void RobotPosesWidget::showPose(srdf::Model::GroupState* pose)
00339 {
00340   // Set pose joint values by adding them to the local joint state map
00341   for (std::map<std::string, std::vector<double> >::const_iterator value_it = pose->joint_values_.begin();
00342        value_it != pose->joint_values_.end(); ++value_it)
00343   {
00344     // Only copy the first joint value // TODO: add capability for multi-DOF joints?
00345     joint_state_map_[value_it->first] = value_it->second[0];
00346   }
00347 
00348   // Update the joints
00349   publishJoints();
00350 
00351   // Unhighlight all links
00352   Q_EMIT unhighlightAll();
00353 
00354   // Highlight group
00355   Q_EMIT highlightGroup(pose->group_);
00356 }
00357 
00358 // ******************************************************************************************
00359 // Show the robot in its default joint positions
00360 // ******************************************************************************************
00361 void RobotPosesWidget::showDefaultPose()
00362 {
00363   // Get list of all joints for the robot
00364   std::vector<const robot_model::JointModel*> joint_models = config_data_->getRobotModel()->getJointModels();
00365 
00366   // Iterate through the joints
00367   for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models.begin();
00368        joint_it < joint_models.end(); ++joint_it)
00369   {
00370     // Check that this joint only represents 1 variable.
00371     if ((*joint_it)->getVariableCount() == 1)
00372     {
00373       double init_value;
00374 
00375       // get the first joint value in its vector
00376       (*joint_it)->getVariableDefaultPositions(&init_value);
00377 
00378       // Change joint's value in joint_state_map to the default
00379       joint_state_map_[(*joint_it)->getName()] = init_value;
00380     }
00381   }
00382 
00383   // Update the joints
00384   publishJoints();
00385 
00386   // Unhighlight all links
00387   Q_EMIT unhighlightAll();
00388 }
00389 
00390 // ******************************************************************************************
00391 // Play through the poses
00392 // ******************************************************************************************
00393 void RobotPosesWidget::playPoses()
00394 {
00395   // Loop through each pose and play them
00396   for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
00397        pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
00398   {
00399     ROS_INFO_STREAM("Showing pose " << pose_it->name_);
00400     showPose(&(*pose_it));
00401     ros::Duration(0.05).sleep();
00402     QApplication::processEvents();
00403     ros::Duration(0.45).sleep();
00404   }
00405 }
00406 
00407 // ******************************************************************************************
00408 // Edit whatever element is selected
00409 // ******************************************************************************************
00410 void RobotPosesWidget::editSelected()
00411 {
00412   // Get list of all selected items
00413   QList<QTableWidgetItem*> selected = data_table_->selectedItems();
00414 
00415   // Check that an element was selected
00416   if (!selected.size())
00417     return;
00418 
00419   // Get selected name and edit it
00420   edit(selected[0]->text().toStdString());
00421 }
00422 
00423 // ******************************************************************************************
00424 // Edit pose
00425 // ******************************************************************************************
00426 void RobotPosesWidget::edit(const std::string& name)
00427 {
00428   // Remember what we are editing
00429   current_edit_pose_ = name;
00430 
00431   // Find the selected in datastruture
00432   srdf::Model::GroupState* pose = findPoseByName(name);
00433 
00434   // Set pose name
00435   pose_name_field_->setText(pose->name_.c_str());
00436 
00437   // Set pose joint values by adding them to the local joint state map
00438   for (std::map<std::string, std::vector<double> >::const_iterator value_it = pose->joint_values_.begin();
00439        value_it != pose->joint_values_.end(); ++value_it)
00440   {
00441     // Only copy the first joint value // TODO: add capability for multi-DOF joints?
00442     joint_state_map_[value_it->first] = value_it->second[0];
00443   }
00444 
00445   // Update robot model in rviz
00446   publishJoints();
00447 
00448   // Set group:
00449   int index = group_name_field_->findText(pose->group_.c_str());
00450   if (index == -1)
00451   {
00452     QMessageBox::critical(this, "Error Loading", "Unable to find group name in drop down box");
00453     return;
00454   }
00455 
00456   // Load joint sliders
00457   group_name_field_->setCurrentIndex(index);
00458 
00459   // Switch to screen - do this before setCurrentIndex
00460   stacked_layout_->setCurrentIndex(1);
00461 
00462   // Announce that this widget is in modal mode
00463   Q_EMIT isModal(true);
00464 
00465   // Manually send the load joint sliders signal
00466   loadJointSliders(QString(pose->group_.c_str()));
00467 }
00468 
00469 // ******************************************************************************************
00470 // Populate the combo dropdown box with avail group names
00471 // ******************************************************************************************
00472 void RobotPosesWidget::loadGroupsComboBox()
00473 {
00474   // Remove all old groups
00475   group_name_field_->clear();
00476 
00477   // Add all group names to combo box
00478   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
00479        group_it != config_data_->srdf_->groups_.end(); ++group_it)
00480   {
00481     group_name_field_->addItem(group_it->name_.c_str());
00482   }
00483 }
00484 
00485 // ******************************************************************************************
00486 // Load the joint sliders based on group selected from combo box
00487 // ******************************************************************************************
00488 void RobotPosesWidget::loadJointSliders(const QString& selected)
00489 {
00490   // Ignore this event if the combo box is empty. This occurs when clearing the combo box and reloading with the
00491   // newest groups. Also ignore if we are not on the edit screen
00492   if (!group_name_field_->count() || selected.isEmpty() || stacked_layout_->currentIndex() == 0)
00493     return;
00494 
00495   // Get group name from input
00496   const std::string group_name = selected.toStdString();
00497 
00498   // Check that joint model exist
00499   if (!config_data_->getRobotModel()->hasJointModelGroup(group_name))
00500   {
00501     QMessageBox::critical(this, "Error Loading", QString("Unable to find joint model group for group: ")
00502                                                      .append(group_name.c_str())
00503                                                      .append(" Are you sure this group has associated joints/links?"));
00504     return;
00505   }
00506 
00507   // Delete old sliders from joint_list_layout_ if this has been loaded before
00508   if (joint_list_layout_)
00509   {
00510     delete joint_list_layout_;
00511     qDeleteAll(joint_list_widget_->children());
00512   }
00513 
00514   // Create layout again
00515   joint_list_layout_ = new QVBoxLayout();
00516   joint_list_widget_->setLayout(joint_list_layout_);
00517   //  joint_list_widget_->setSizePolicy( QSizePolicy::Expanding, QSizePolicy::Expanding );
00518   joint_list_widget_->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Ignored);
00519   joint_list_widget_->setMinimumSize(50, 50);  // w, h
00520 
00521   // Get list of associated joints
00522   const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name);
00523   joint_models_ = joint_model_group->getJointModels();
00524 
00525   // Iterate through the joints
00526   int num_joints = 0;
00527   for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models_.begin();
00528        joint_it < joint_models_.end(); ++joint_it)
00529   {
00530     // Check that this joint only represents 1 variable.
00531     if ((*joint_it)->getVariableCount() == 1)
00532     {
00533       double init_value;
00534 
00535       // Decide what this joint's initial value is
00536       if (joint_state_map_.find((*joint_it)->getName()) == joint_state_map_.end())
00537       {
00538         // the joint state map does not yet have an entry for this joint
00539 
00540         // get the first joint value in its vector
00541         (*joint_it)->getVariableDefaultPositions(&init_value);
00542       }
00543       else  // there is already a value in the map
00544       {
00545         init_value = joint_state_map_[(*joint_it)->getName()];
00546       }
00547 
00548       // For each joint in group add slider
00549       SliderWidget* sw = new SliderWidget(this, *joint_it, init_value);
00550       joint_list_layout_->addWidget(sw);
00551 
00552       // Connect value change event
00553       connect(sw, SIGNAL(jointValueChanged(const std::string&, double)), this,
00554               SLOT(updateRobotModel(const std::string&, double)));
00555 
00556       ++num_joints;
00557     }
00558   }
00559 
00560   // Copy the width of column 2 and manually calculate height from number of joints
00561   joint_list_widget_->resize(300, num_joints * 70);  // w, h
00562 
00563   // Update the robot model in Rviz with newly selected joint values
00564   publishJoints();
00565 
00566   // Highlight the planning group
00567   Q_EMIT unhighlightAll();
00568   Q_EMIT highlightGroup(group_name);
00569 }
00570 
00571 // ******************************************************************************************
00572 // Find the associated data by name
00573 // ******************************************************************************************
00574 srdf::Model::GroupState* RobotPosesWidget::findPoseByName(const std::string& name)
00575 {
00576   // Find the group state we are editing based on the pose name
00577   srdf::Model::GroupState* searched_group = NULL;  // used for holding our search results
00578 
00579   for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
00580        pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
00581   {
00582     if (pose_it->name_ == name)  // string match
00583     {
00584       searched_group = &(*pose_it);  // convert to pointer from iterator
00585       break;                         // we are done searching
00586     }
00587   }
00588 
00589   // Check if pose was found
00590   if (searched_group == NULL)  // not found
00591   {
00592     QMessageBox::critical(this, "Error Saving", "An internal error has occured while saving. Quitting.");
00593     QApplication::quit();
00594   }
00595 
00596   return searched_group;
00597 }
00598 
00599 // ******************************************************************************************
00600 // Delete currently editing item
00601 // ******************************************************************************************
00602 void RobotPosesWidget::deleteSelected()
00603 {
00604   // Get list of all selected items
00605   QList<QTableWidgetItem*> selected = data_table_->selectedItems();
00606 
00607   // Check that an element was selected
00608   if (!selected.size())
00609     return;
00610 
00611   // Get selected name and edit it
00612   current_edit_pose_ = selected[0]->text().toStdString();
00613 
00614   // Confirm user wants to delete group
00615   if (QMessageBox::question(
00616           this, "Confirm Pose Deletion",
00617           QString("Are you sure you want to delete the pose '").append(current_edit_pose_.c_str()).append("'?"),
00618           QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
00619   {
00620     return;
00621   }
00622 
00623   // Delete pose from vector
00624   for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
00625        pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
00626   {
00627     // check if this is the group we want to delete
00628     if (pose_it->name_ == current_edit_pose_)  // string match
00629     {
00630       config_data_->srdf_->group_states_.erase(pose_it);
00631       break;
00632     }
00633   }
00634 
00635   // Reload main screen table
00636   loadDataTable();
00637   config_data_->changes |= MoveItConfigData::POSES;
00638 }
00639 
00640 // ******************************************************************************************
00641 // Save editing changes
00642 // ******************************************************************************************
00643 void RobotPosesWidget::doneEditing()
00644 {
00645   // Get a reference to the supplied strings
00646   const std::string pose_name = pose_name_field_->text().toStdString();
00647 
00648   // Used for editing existing groups
00649   srdf::Model::GroupState* searched_data = NULL;
00650 
00651   // Check that name field is not empty
00652   if (pose_name.empty())
00653   {
00654     QMessageBox::warning(this, "Error Saving", "A name must be given for the pose!");
00655     return;
00656   }
00657 
00658   // Check if this is an existing group
00659   if (!current_edit_pose_.empty())
00660   {
00661     // Find the group we are editing based on the goup name string
00662     searched_data = findPoseByName(current_edit_pose_);
00663   }
00664 
00665   // Check that the pose name is unique
00666   for (std::vector<srdf::Model::GroupState>::const_iterator data_it = config_data_->srdf_->group_states_.begin();
00667        data_it != config_data_->srdf_->group_states_.end(); ++data_it)
00668   {
00669     if (data_it->name_.compare(pose_name) == 0)  // the names are the same
00670     {
00671       // is this our existing pose? check if pose pointers are same
00672       if (&(*data_it) != searched_data)
00673       {
00674         QMessageBox::warning(this, "Error Saving", "A pose already exists with that name!");
00675         return;
00676       }
00677     }
00678   }
00679 
00680   // Check that a group was selected
00681   if (group_name_field_->currentText().isEmpty())
00682   {
00683     QMessageBox::warning(this, "Error Saving", "A planning group must be chosen!");
00684     return;
00685   }
00686 
00687   config_data_->changes |= MoveItConfigData::POSES;
00688 
00689   // Save the new pose name or create the new pose ----------------------------
00690   bool isNew = false;
00691 
00692   if (searched_data == NULL)  // create new
00693   {
00694     isNew = true;
00695 
00696     searched_data = new srdf::Model::GroupState();
00697   }
00698 
00699   // Copy name data ----------------------------------------------------
00700   searched_data->name_ = pose_name;
00701   searched_data->group_ = group_name_field_->currentText().toStdString();
00702 
00703   // Copy joint positions ----------------------------------------
00704 
00705   // Clear the old values
00706   searched_data->joint_values_.clear();
00707 
00708   // Iterate through the current group's joints and add to SRDF
00709   for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models_.begin();
00710        joint_it < joint_models_.end(); ++joint_it)
00711   {
00712     // Check that this joint only represents 1 variable.
00713     if ((*joint_it)->getVariableCount() == 1)
00714     {
00715       // Create vector for new joint values
00716       std::vector<double> joint_value;
00717       joint_value.push_back(joint_state_map_[(*joint_it)->getName()]);
00718 
00719       // Add joint vector to SRDF
00720       searched_data->joint_values_[(*joint_it)->getName()] = joint_value;
00721     }
00722   }
00723 
00724   // Insert new poses into group state vector --------------------------
00725   if (isNew)
00726   {
00727     config_data_->srdf_->group_states_.push_back(*searched_data);
00728   }
00729 
00730   // Finish up ------------------------------------------------------
00731 
00732   // Reload main screen table
00733   loadDataTable();
00734 
00735   // Switch to screen
00736   stacked_layout_->setCurrentIndex(0);
00737 
00738   // Announce that this widget is done with modal mode
00739   Q_EMIT isModal(false);
00740 }
00741 
00742 // ******************************************************************************************
00743 // Cancel changes
00744 // ******************************************************************************************
00745 void RobotPosesWidget::cancelEditing()
00746 {
00747   // Switch to screen
00748   stacked_layout_->setCurrentIndex(0);
00749 
00750   // Announce that this widget is done with modal mode
00751   Q_EMIT isModal(false);
00752 }
00753 
00754 // ******************************************************************************************
00755 // Load the robot poses into the table
00756 // ******************************************************************************************
00757 void RobotPosesWidget::loadDataTable()
00758 {
00759   // Disable Table
00760   data_table_->setUpdatesEnabled(false);  // prevent table from updating until we are completely done
00761   data_table_->setDisabled(true);         // make sure we disable it so that the cellChanged event is not called
00762   data_table_->clearContents();
00763 
00764   // Set size of datatable
00765   data_table_->setRowCount(config_data_->srdf_->group_states_.size());
00766 
00767   // Loop through every pose
00768   int row = 0;
00769   for (std::vector<srdf::Model::GroupState>::const_iterator data_it = config_data_->srdf_->group_states_.begin();
00770        data_it != config_data_->srdf_->group_states_.end(); ++data_it)
00771   {
00772     // Create row elements
00773     QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str());
00774     data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
00775     QTableWidgetItem* group_name = new QTableWidgetItem(data_it->group_.c_str());
00776     group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
00777 
00778     // Add to table
00779     data_table_->setItem(row, 0, data_name);
00780     data_table_->setItem(row, 1, group_name);
00781 
00782     // Increment counter
00783     ++row;
00784   }
00785 
00786   // Reenable
00787   data_table_->setUpdatesEnabled(true);  // prevent table from updating until we are completely done
00788   data_table_->setDisabled(false);       // make sure we disable it so that the cellChanged event is not called
00789 
00790   // Resize header
00791   data_table_->resizeColumnToContents(0);
00792   data_table_->resizeColumnToContents(1);
00793 
00794   // Show edit button if applicable
00795   if (config_data_->srdf_->group_states_.size())
00796     btn_edit_->show();
00797 }
00798 
00799 // ******************************************************************************************
00800 // Called when setup assistant navigation switches to this screen
00801 // ******************************************************************************************
00802 void RobotPosesWidget::focusGiven()
00803 {
00804   // Show the current poses screen
00805   stacked_layout_->setCurrentIndex(0);
00806 
00807   // Load the data to the tree
00808   loadDataTable();
00809 
00810   // Load the avail groups to the combo box
00811   loadGroupsComboBox();
00812 }
00813 
00814 // ******************************************************************************************
00815 // Call when one of the sliders has its value changed
00816 // ******************************************************************************************
00817 void RobotPosesWidget::updateRobotModel(const std::string& name, double value)
00818 {
00819   // Save the new value
00820   joint_state_map_[name] = value;
00821 
00822   // Update the robot model/rviz
00823   publishJoints();
00824 }
00825 
00826 // ******************************************************************************************
00827 // Publish the joint values in the joint_state_map_ to Rviz
00828 // ******************************************************************************************
00829 void RobotPosesWidget::publishJoints()
00830 {
00831   // Change the scene
00832   // scene.getCurrentState().setToDefaultValues();//set to default values of 0 OR half between low and high joint values
00833   // config_data_->getPlanningScene()->getCurrentState().setToRandomValues();
00834 
00835   // Set the joints based on the map
00836   config_data_->getPlanningScene()->getCurrentStateNonConst().setVariablePositions(joint_state_map_);
00837 
00838   // Create a planning scene message
00839   moveit_msgs::DisplayRobotState msg;
00840   robot_state::robotStateToRobotStateMsg(config_data_->getPlanningScene()->getCurrentState(), msg.state);
00841 
00842   // Publish!
00843   pub_robot_state_.publish(msg);
00844 
00845   // Prevent dirty collision body transforms
00846   config_data_->getPlanningScene()->getCurrentStateNonConst().update();
00847 
00848   // Decide if current state is in collision
00849   collision_detection::CollisionResult result;
00850   config_data_->getPlanningScene()->checkSelfCollision(
00851       request, result, config_data_->getPlanningScene()->getCurrentState(), config_data_->allowed_collision_matrix_);
00852   // Show result notification
00853   if (result.contacts.size())
00854   {
00855     collision_warning_->show();
00856   }
00857   else
00858   {
00859     collision_warning_->hide();
00860   }
00861 }
00862 
00863 // ******************************************************************************************
00864 // ******************************************************************************************
00865 // Slider Widget
00866 // ******************************************************************************************
00867 // ******************************************************************************************
00868 
00869 // ******************************************************************************************
00870 // Simple widget for adjusting joints of a robot
00871 // ******************************************************************************************
00872 SliderWidget::SliderWidget(QWidget* parent, const robot_model::JointModel* joint_model, double init_value)
00873   : QWidget(parent), joint_model_(joint_model)
00874 {
00875   // Create layouts
00876   QVBoxLayout* layout = new QVBoxLayout();
00877   QHBoxLayout* row2 = new QHBoxLayout();
00878 
00879   // Row 1
00880   joint_label_ = new QLabel(joint_model_->getName().c_str(), this);
00881   joint_label_->setContentsMargins(0, 0, 0, 0);
00882   layout->addWidget(joint_label_);
00883 
00884   // Row 2 -------------------------------------------------------------
00885   joint_slider_ = new QSlider(Qt::Horizontal, this);
00886   joint_slider_->setTickPosition(QSlider::TicksBelow);
00887   joint_slider_->setSingleStep(10);
00888   joint_slider_->setPageStep(500);
00889   joint_slider_->setTickInterval(1000);
00890   joint_slider_->setContentsMargins(0, 0, 0, 0);
00891   row2->addWidget(joint_slider_);
00892 
00893   // Joint Value Box ------------------------------------------------
00894   joint_value_ = new QLineEdit(this);
00895   joint_value_->setMaximumWidth(62);
00896   joint_value_->setContentsMargins(0, 0, 0, 0);
00897   connect(joint_value_, SIGNAL(editingFinished()), this, SLOT(changeJointSlider()));
00898   row2->addWidget(joint_value_);
00899 
00900   // Joint Limits ----------------------------------------------------
00901   const std::vector<moveit_msgs::JointLimits>& limits = joint_model_->getVariableBoundsMsg();
00902   if (limits.empty())
00903   {
00904     QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading the joints");
00905     return;
00906   }
00907 
00908   // Only use the first limit, because there is only 1 variable (as checked earlier)
00909   moveit_msgs::JointLimits joint_limit = limits[0];
00910   max_position_ = joint_limit.max_position;
00911   min_position_ = joint_limit.min_position;
00912 
00913   // Set the slider limits
00914   joint_slider_->setMaximum(max_position_ * 10000);
00915   joint_slider_->setMinimum(min_position_ * 10000);
00916 
00917   // Connect slider to joint value box
00918   connect(joint_slider_, SIGNAL(valueChanged(int)), this, SLOT(changeJointValue(int)));
00919 
00920   // Initial joint values -------------------------------------------
00921   int value = init_value * 10000;           // scale double to integer for slider use
00922   joint_slider_->setSliderPosition(value);  // set slider value
00923   changeJointValue(value);                  // show in textbox
00924 
00925   // Finish GUI ----------------------------------------
00926   layout->addLayout(row2);
00927 
00928   this->setContentsMargins(0, 0, 0, 0);
00929   // this->setSizePolicy( QSizePolicy::Expanding, QSizePolicy::Preferred );
00930   this->setGeometry(QRect(110, 80, 120, 80));
00931   this->setLayout(layout);
00932 
00933   // Declare std::string as metatype so we can use it in a signal
00934   qRegisterMetaType<std::string>("std::string");
00935 }
00936 
00937 // ******************************************************************************************
00938 // Deconstructor
00939 // ******************************************************************************************
00940 SliderWidget::~SliderWidget()
00941 {
00942 }
00943 
00944 // ******************************************************************************************
00945 // Called when the joint value slider is changed
00946 // ******************************************************************************************
00947 void SliderWidget::changeJointValue(int value)
00948 {
00949   // Get joint value
00950   const double double_value = double(value) / 10000;
00951 
00952   // Set textbox
00953   joint_value_->setText(QString("%1").arg(double_value, 0, 'f', 4));
00954 
00955   // Send event to parent widget
00956   Q_EMIT jointValueChanged(joint_model_->getName(), double_value);
00957 }
00958 
00959 // ******************************************************************************************
00960 // Called when the joint value box is changed
00961 // ******************************************************************************************
00962 void SliderWidget::changeJointSlider()
00963 {
00964   // Get joint value
00965   double value = joint_value_->text().toDouble();
00966 
00967   if (min_position_ > value || value > max_position_)
00968   {
00969     value = (min_position_ > value) ? min_position_ : max_position_;
00970     joint_value_->setText(QString("%1").arg(value, 0, 'f', 4));
00971   }
00972 
00973   // We assume it converts to double because of the validator
00974   joint_slider_->setSliderPosition(value * 10000);
00975 
00976   // Send event to parent widget
00977   Q_EMIT jointValueChanged(joint_model_->getName(), value);
00978 }
00979 
00980 }  // namespace


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Wed Jun 19 2019 19:25:13