planning_groups_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 // ******************************************************************************************
00038 /* DEVELOPER NOTES
00039 
00040    This widget has 6 subscreens, located in somewhat different places
00041    - Main screen, the tree view of all groups & subgroups - embedded in this file as a function
00042    - Add/Edit Group screen - located in group_edit_widget.cpp
00043    - Joint Collection Screen - implements the double_list_widget.cpp widget
00044    - Link Collection Screen - implements the double_list_widget.cpp widget
00045    - Kinematic Chain Screen - uses it own custom widget - kinematic_chain_widget.cpp
00046    - Subgroup Screen - implements the double_list_widget.cpp widget
00047 */
00048 // ******************************************************************************************
00049 
00050 #include "ros/ros.h"
00051 #include "header_widget.h"
00052 #include "planning_groups_widget.h"
00053 #include <boost/thread.hpp>
00054 #include <boost/lexical_cast.hpp>  // for checking convertion of string to double
00055 // Qt
00056 #include <QApplication>
00057 #include <QDebug>
00058 #include <QVBoxLayout>
00059 #include <QHBoxLayout>
00060 #include <QLabel>
00061 #include <QPushButton>
00062 #include <QMessageBox>
00063 #include <QString>
00064 #include <QLineEdit>
00065 #include <QTreeWidgetItem>
00066 #include <QHeaderView>
00067 // Cycle checking
00068 #include <boost/utility.hpp>
00069 #include <boost/graph/adjacency_list.hpp>
00070 #include <boost/graph/depth_first_search.hpp>
00071 #include <boost/graph/visitors.hpp>
00072 
00073 namespace moveit_setup_assistant
00074 {
00075 // Name of rviz topic in ROS
00076 static const std::string VIS_TOPIC_NAME = "planning_components_visualization";
00077 
00078 // Used for checking for cycles in a subgroup hierarchy
00079 struct cycle_detector : public boost::dfs_visitor<>
00080 {
00081   cycle_detector(bool& has_cycle) : m_has_cycle(has_cycle)
00082   {
00083   }
00084 
00085   template <class Edge, class Graph>
00086   void back_edge(Edge, Graph&)
00087   {
00088     m_has_cycle = true;
00089   }
00090 
00091 protected:
00092   bool& m_has_cycle;
00093 };
00094 
00095 // ******************************************************************************************
00096 // Constructor
00097 // ******************************************************************************************
00098 PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
00099   : SetupScreenWidget(parent), config_data_(config_data)
00100 {
00101   // Basic widget container
00102   QVBoxLayout* layout = new QVBoxLayout();
00103 
00104   // Top Label Area ------------------------------------------------
00105   HeaderWidget* header =
00106       new HeaderWidget("Planning Groups", "Create and edit planning groups for your robot based on joint collections, "
00107                                           "link collections, kinematic chains and subgroups.",
00108                        this);
00109   layout->addWidget(header);
00110 
00111   // Left Side ---------------------------------------------
00112 
00113   // Create left side widgets
00114   groups_tree_widget_ = createContentsWidget();  // included in this file
00115 
00116   // Joints edit widget
00117   joints_widget_ = new DoubleListWidget(this, config_data_, "Joint Collection", "Joint");
00118   connect(joints_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
00119   connect(joints_widget_, SIGNAL(doneEditing()), this, SLOT(saveJointsScreen()));
00120   connect(joints_widget_, SIGNAL(previewSelected(std::vector<std::string>)), this,
00121           SLOT(previewSelectedJoints(std::vector<std::string>)));
00122 
00123   // Links edit widget
00124   links_widget_ = new DoubleListWidget(this, config_data_, "Link Collection", "Link");
00125   connect(links_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
00126   connect(links_widget_, SIGNAL(doneEditing()), this, SLOT(saveLinksScreen()));
00127   connect(links_widget_, SIGNAL(previewSelected(std::vector<std::string>)), this,
00128           SLOT(previewSelectedLink(std::vector<std::string>)));
00129 
00130   // Chain Widget
00131   chain_widget_ = new KinematicChainWidget(this, config_data);
00132   connect(chain_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
00133   connect(chain_widget_, SIGNAL(doneEditing()), this, SLOT(saveChainScreen()));
00134   connect(chain_widget_, SIGNAL(unhighlightAll()), this, SIGNAL(unhighlightAll()));
00135   connect(chain_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00136           SIGNAL(highlightLink(const std::string&, const QColor&)));
00137 
00138   // Subgroups Widget
00139   subgroups_widget_ = new DoubleListWidget(this, config_data_, "Subgroup", "Subgroup");
00140   connect(subgroups_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
00141   connect(subgroups_widget_, SIGNAL(doneEditing()), this, SLOT(saveSubgroupsScreen()));
00142   connect(subgroups_widget_, SIGNAL(previewSelected(std::vector<std::string>)), this,
00143           SLOT(previewSelectedSubgroup(std::vector<std::string>)));
00144 
00145   // Group Edit Widget
00146   group_edit_widget_ = new GroupEditWidget(this, config_data_);
00147   connect(group_edit_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
00148   connect(group_edit_widget_, SIGNAL(deleteGroup()), this, SLOT(deleteGroup()));
00149   connect(group_edit_widget_, SIGNAL(save()), this, SLOT(saveGroupScreenEdit()));
00150   connect(group_edit_widget_, SIGNAL(saveJoints()), this, SLOT(saveGroupScreenJoints()));
00151   connect(group_edit_widget_, SIGNAL(saveLinks()), this, SLOT(saveGroupScreenLinks()));
00152   connect(group_edit_widget_, SIGNAL(saveChain()), this, SLOT(saveGroupScreenChain()));
00153   connect(group_edit_widget_, SIGNAL(saveSubgroups()), this, SLOT(saveGroupScreenSubgroups()));
00154 
00155   // Combine into stack
00156   stacked_layout_ = new QStackedLayout(this);
00157   stacked_layout_->addWidget(groups_tree_widget_);  // screen index 0
00158   stacked_layout_->addWidget(joints_widget_);       // screen index 1
00159   stacked_layout_->addWidget(links_widget_);        // screen index 2
00160   stacked_layout_->addWidget(chain_widget_);        // screen index 3
00161   stacked_layout_->addWidget(subgroups_widget_);    // screen index 4
00162   stacked_layout_->addWidget(group_edit_widget_);   // screen index 5
00163 
00164   showMainScreen();
00165 
00166   // Finish GUI -----------------------------------------------------------
00167 
00168   // Create Widget wrapper for layout
00169   QWidget* stacked_layout_widget = new QWidget(this);
00170   stacked_layout_widget->setLayout(stacked_layout_);
00171 
00172   layout->addWidget(stacked_layout_widget);
00173 
00174   setLayout(layout);
00175 
00176   // process the gui
00177   QApplication::processEvents();
00178 }
00179 
00180 // ******************************************************************************************
00181 // Create the main tree view widget
00182 // ******************************************************************************************
00183 QWidget* PlanningGroupsWidget::createContentsWidget()
00184 {
00185   // Main widget
00186   QWidget* content_widget = new QWidget(this);
00187 
00188   // Basic widget container
00189   QVBoxLayout* layout = new QVBoxLayout(this);
00190 
00191   // Tree Box ----------------------------------------------------------------------
00192 
00193   groups_tree_ = new QTreeWidget(this);
00194   groups_tree_->setHeaderLabel("Current Groups");
00195   connect(groups_tree_, SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)), this, SLOT(editSelected()));
00196   connect(groups_tree_, SIGNAL(itemClicked(QTreeWidgetItem*, int)), this, SLOT(previewSelected()));
00197   layout->addWidget(groups_tree_);
00198 
00199   // Bottom Controls -------------------------------------------------------------
00200 
00201   QHBoxLayout* controls_layout = new QHBoxLayout();
00202 
00203   // Expand/Contract controls
00204   QLabel* expand_controls = new QLabel(this);
00205   expand_controls->setText("<a href='expand'>Expand All</a> <a href='contract'>Collapse All</a>");
00206   connect(expand_controls, SIGNAL(linkActivated(const QString)), this, SLOT(alterTree(const QString)));
00207   controls_layout->addWidget(expand_controls);
00208 
00209   // Spacer
00210   QWidget* spacer = new QWidget(this);
00211   spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00212   controls_layout->addWidget(spacer);
00213 
00214   // Delete Selected Button
00215   btn_delete_ = new QPushButton("&Delete Selected", this);
00216   btn_delete_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00217   btn_delete_->setMaximumWidth(300);
00218   connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteGroup()));
00219   controls_layout->addWidget(btn_delete_);
00220   controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
00221 
00222   //  Edit Selected Button
00223   btn_edit_ = new QPushButton("&Edit Selected", this);
00224   btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00225   btn_edit_->setMaximumWidth(300);
00226   btn_edit_->hide();  // show once we know if there are existing groups
00227   connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
00228   controls_layout->addWidget(btn_edit_);
00229   controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
00230 
00231   // Add Group Button
00232   QPushButton* btn_add = new QPushButton("&Add Group", this);
00233   btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00234   btn_add->setMaximumWidth(300);
00235   connect(btn_add, SIGNAL(clicked()), this, SLOT(addGroup()));
00236   controls_layout->addWidget(btn_add);
00237   controls_layout->setAlignment(btn_add, Qt::AlignRight);
00238 
00239   // Add Controls to layout
00240   layout->addLayout(controls_layout);
00241 
00242   // Set layout
00243   content_widget->setLayout(layout);
00244 
00245   return content_widget;
00246 }
00247 
00248 // ******************************************************************************************
00249 // Displays data in the link_pairs_ data structure into a QtTableWidget
00250 // ******************************************************************************************
00251 void PlanningGroupsWidget::loadGroupsTree()
00252 {
00253   // Disable Tree
00254   groups_tree_->setUpdatesEnabled(false);  // prevent table from updating until we are completely done
00255   groups_tree_->setDisabled(true);         // make sure we disable it so that the cellChanged event is not called
00256   groups_tree_->clear();                   // reset the tree
00257 
00258   // Display all groups by looping through them
00259   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
00260        group_it != config_data_->srdf_->groups_.end(); ++group_it)
00261   {
00262     loadGroupsTreeRecursive(*group_it, NULL);
00263   }
00264 
00265   // Reenable Tree
00266   groups_tree_->setUpdatesEnabled(true);  // prevent table from updating until we are completely done
00267   groups_tree_->setDisabled(false);       // make sure we disable it so that the cellChanged event is not called
00268 
00269   // Show Edit button if there are things to edit
00270   if (!config_data_->srdf_->groups_.empty())
00271   {
00272     btn_edit_->show();
00273     btn_delete_->show();
00274   }
00275   else
00276   {
00277     btn_edit_->hide();
00278     btn_delete_->hide();
00279   }
00280 
00281   alterTree("expand");
00282 }
00283 
00284 // ******************************************************************************************
00285 // Recursively Adds Groups, and subgroups to groups...
00286 // ******************************************************************************************
00287 void PlanningGroupsWidget::loadGroupsTreeRecursive(srdf::Model::Group& group_it, QTreeWidgetItem* parent)
00288 {
00289   // Fonts for tree
00290   const QFont top_level_font("Arial", 11, QFont::Bold);
00291   const QFont type_font("Arial", 11, QFont::Normal, QFont::StyleItalic);
00292 
00293   QTreeWidgetItem* group;
00294 
00295   // Allow a subgroup to open into a whole new group
00296   if (parent == NULL)
00297   {
00298     group = new QTreeWidgetItem(groups_tree_);
00299     group->setText(0, group_it.name_.c_str());
00300     group->setFont(0, top_level_font);
00301     group->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, GROUP)));
00302     groups_tree_->addTopLevelItem(group);
00303   }
00304   else
00305   {
00306     group = new QTreeWidgetItem(parent);
00307     group->setText(0, group_it.name_.c_str());
00308     group->setFont(0, top_level_font);
00309     group->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, GROUP)));
00310     parent->addChild(group);
00311   }
00312 
00313   // Joints --------------------------------------------------------------
00314   QTreeWidgetItem* joints = new QTreeWidgetItem(group);
00315   joints->setText(0, "Joints");
00316   joints->setFont(0, type_font);
00317   joints->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, JOINT)));
00318   group->addChild(joints);
00319 
00320   // Retrieve pointer to the shared kinematic model
00321   const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
00322 
00323   // Loop through all aval. joints
00324   for (std::vector<std::string>::const_iterator joint_it = group_it.joints_.begin(); joint_it != group_it.joints_.end();
00325        ++joint_it)
00326   {
00327     QTreeWidgetItem* j = new QTreeWidgetItem(joints);
00328     j->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, JOINT)));
00329     std::string joint_name;
00330 
00331     // Get the type of joint this is
00332     const robot_model::JointModel* jm = model->getJointModel(*joint_it);
00333     if (jm)  // check if joint model was found
00334     {
00335       joint_name = *joint_it + " - " + jm->getTypeName();
00336     }
00337     else
00338     {
00339       joint_name = *joint_it;
00340     }
00341 
00342     // Add to tree
00343     j->setText(0, joint_name.c_str());
00344     joints->addChild(j);
00345   }
00346 
00347   // Links -------------------------------------------------------------
00348   QTreeWidgetItem* links = new QTreeWidgetItem(group);
00349   links->setText(0, "Links");
00350   links->setFont(0, type_font);
00351   links->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, LINK)));
00352   group->addChild(links);
00353 
00354   // Loop through all aval. links
00355   for (std::vector<std::string>::const_iterator joint_it = group_it.links_.begin(); joint_it != group_it.links_.end();
00356        ++joint_it)
00357   {
00358     QTreeWidgetItem* j = new QTreeWidgetItem(links);
00359     j->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, LINK)));
00360     j->setText(0, joint_it->c_str());
00361     links->addChild(j);
00362   }
00363 
00364   // Chains -------------------------------------------------------------
00365   QTreeWidgetItem* chains = new QTreeWidgetItem(group);
00366   chains->setText(0, "Chain");
00367   chains->setFont(0, type_font);
00368   chains->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, CHAIN)));
00369   group->addChild(chains);
00370 
00371   // Warn if there is more than 1 chain per group
00372   static bool warn_once = true;
00373   if (group_it.chains_.size() > 1 && warn_once)
00374   {
00375     warn_once = false;
00376     QMessageBox::warning(this, "Group with Multiple Kinematic Chains",
00377                          "Warning: this MoveIt Setup Assistant is only designed to handle one kinematic chain per "
00378                          "group. The loaded SRDF has more than one kinematic chain for a group. A possible loss of "
00379                          "data may occur.");
00380   }
00381 
00382   // Loop through all aval. chains
00383   for (std::vector<std::pair<std::string, std::string> >::const_iterator chain_it = group_it.chains_.begin();
00384        chain_it != group_it.chains_.end(); ++chain_it)
00385   {
00386     QTreeWidgetItem* j = new QTreeWidgetItem(chains);
00387     j->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, CHAIN)));
00388     j->setText(0, QString(chain_it->first.c_str()).append("  ->  ").append(chain_it->second.c_str()));
00389     chains->addChild(j);
00390   }
00391 
00392   // Subgroups -------------------------------------------------------------
00393   QTreeWidgetItem* subgroups = new QTreeWidgetItem(group);
00394   subgroups->setText(0, "Subgroups");
00395   subgroups->setFont(0, type_font);
00396   subgroups->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, SUBGROUP)));
00397   group->addChild(subgroups);
00398 
00399   // Loop through all aval. subgroups
00400   for (std::vector<std::string>::iterator subgroup_it = group_it.subgroups_.begin();
00401        subgroup_it != group_it.subgroups_.end(); ++subgroup_it)
00402   {
00403     // Find group with this subgroups' name
00404 
00405     srdf::Model::Group* searched_group = NULL;  // used for holding our search results
00406 
00407     for (std::vector<srdf::Model::Group>::iterator group2_it = config_data_->srdf_->groups_.begin();
00408          group2_it != config_data_->srdf_->groups_.end(); ++group2_it)
00409     {
00410       if (group2_it->name_ == *subgroup_it)  // this is the group we are looking for
00411       {
00412         searched_group = &(*group2_it);  // convert to pointer from iterator
00413         break;                           // we are done searching
00414       }
00415     }
00416 
00417     // Check if subgroup was found
00418     if (searched_group == NULL)  // not found
00419     {
00420       QMessageBox::critical(this, "Error Loading SRDF", QString("Subgroup '")
00421                                                             .append(subgroup_it->c_str())
00422                                                             .append("' of group '")
00423                                                             .append(group_it.name_.c_str())
00424                                                             .append("' not found. Your SRDF is invalid"));
00425       return;  // TODO: something better for error handling?
00426     }
00427 
00428     // subgroup found!
00429 
00430     // Recurse this function for each new group
00431     loadGroupsTreeRecursive(*searched_group, subgroups);
00432   }
00433 }
00434 
00435 // ******************************************************************************************
00436 // Highlight the group of whatever element is selected in the tree view
00437 // ******************************************************************************************
00438 void PlanningGroupsWidget::previewSelected()
00439 {
00440   QTreeWidgetItem* item = groups_tree_->currentItem();
00441 
00442   // Check that something was actually selected
00443   if (item == NULL)
00444     return;
00445 
00446   // Get the user custom properties of the currently selected row
00447   PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
00448 
00449   // Unhighlight all links
00450   Q_EMIT unhighlightAll();
00451 
00452   // Highlight the group
00453   Q_EMIT(highlightGroup(plan_group.group_->name_));
00454 }
00455 
00456 // ******************************************************************************************
00457 // Edit whatever element is selected in the tree view
00458 // ******************************************************************************************
00459 void PlanningGroupsWidget::editSelected()
00460 {
00461   QTreeWidgetItem* item = groups_tree_->currentItem();
00462 
00463   // Check that something was actually selected
00464   if (item == NULL)
00465     return;
00466 
00467   adding_new_group_ = false;
00468 
00469   // Get the user custom properties of the currently selected row
00470   PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
00471 
00472   if (plan_group.type_ == JOINT)
00473   {
00474     // Load the data
00475     loadJointsScreen(plan_group.group_);
00476 
00477     // Switch to screen
00478     changeScreen(1);  // 1 is index of joints
00479   }
00480   else if (plan_group.type_ == LINK)
00481   {
00482     // Load the data
00483     loadLinksScreen(plan_group.group_);
00484 
00485     // Switch to screen
00486     changeScreen(2);
00487   }
00488   else if (plan_group.type_ == CHAIN)
00489   {
00490     // Load the data
00491     loadChainScreen(plan_group.group_);
00492 
00493     // Switch to screen
00494     changeScreen(3);
00495   }
00496   else if (plan_group.type_ == SUBGROUP)
00497   {
00498     // Load the data
00499     loadSubgroupsScreen(plan_group.group_);
00500 
00501     // Switch to screen
00502     changeScreen(4);
00503   }
00504   else if (plan_group.type_ == GROUP)
00505   {
00506     // Load the data
00507     loadGroupScreen(plan_group.group_);
00508 
00509     // Switch to screen
00510     changeScreen(5);
00511   }
00512   else
00513   {
00514     QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading.");
00515   }
00516 }
00517 
00518 // ******************************************************************************************
00519 // Load the popup screen with correct data for joints
00520 // ******************************************************************************************
00521 void PlanningGroupsWidget::loadJointsScreen(srdf::Model::Group* this_group)
00522 {
00523   // Retrieve pointer to the shared kinematic model
00524   const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
00525 
00526   // Get the names of the all joints
00527   const std::vector<std::string>& joints = model->getJointModelNames();
00528 
00529   if (joints.size() == 0)
00530   {
00531     QMessageBox::critical(this, "Error Loading", "No joints found for robot model");
00532     return;
00533   }
00534 
00535   // Set the available joints (left box)
00536   joints_widget_->setAvailable(joints);
00537 
00538   // Set the selected joints (right box)
00539   joints_widget_->setSelected(this_group->joints_);
00540 
00541   // Set the title
00542   joints_widget_->title_->setText(
00543       QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Joint Collection"));
00544 
00545   // Remember what is currently being edited so we can later save changes
00546   current_edit_group_ = this_group->name_;
00547   current_edit_element_ = JOINT;
00548 }
00549 
00550 // ******************************************************************************************
00551 // Load the popup screen with correct data for links
00552 // ******************************************************************************************
00553 void PlanningGroupsWidget::loadLinksScreen(srdf::Model::Group* this_group)
00554 {
00555   // Retrieve pointer to the shared kinematic model
00556   const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
00557 
00558   // Get the names of the all links
00559   const std::vector<std::string>& links = model->getLinkModelNames();
00560 
00561   if (links.size() == 0)
00562   {
00563     QMessageBox::critical(this, "Error Loading", "No links found for robot model");
00564     return;
00565   }
00566 
00567   // Set the available links (left box)
00568   links_widget_->setAvailable(links);
00569 
00570   // Set the selected links (right box)
00571   links_widget_->setSelected(this_group->links_);
00572 
00573   // Set the title
00574   links_widget_->title_->setText(
00575       QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Link Collection"));
00576 
00577   // Remember what is currently being edited so we can later save changes
00578   current_edit_group_ = this_group->name_;
00579   current_edit_element_ = LINK;
00580 }
00581 
00582 // ******************************************************************************************
00583 // Load the popup screen with correct data for chains
00584 // ******************************************************************************************
00585 void PlanningGroupsWidget::loadChainScreen(srdf::Model::Group* this_group)
00586 {
00587   // Tell the kin chain widget to load up the chain from a kinematic model
00588   chain_widget_->setAvailable();
00589 
00590   // Make sure there isn't more than 1 chain pair
00591   if (this_group->chains_.size() > 1)
00592   {
00593     QMessageBox::warning(this, "Multiple Kinematic Chains", "Warning: This setup assistant is only designed to handle "
00594                                                             "one kinematic chain per group. The loaded SRDF has more "
00595                                                             "than one kinematic chain for a group. A possible loss of "
00596                                                             "data may occur.");
00597   }
00598 
00599   // Set the selected tip and base of chain if one exists
00600   if (this_group->chains_.size() > 0)
00601   {
00602     chain_widget_->setSelected(this_group->chains_[0].first, this_group->chains_[0].second);
00603   }
00604 
00605   // Set the title
00606   chain_widget_->title_->setText(
00607       QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Kinematic Chain"));
00608 
00609   // Remember what is currently being edited so we can later save changes
00610   current_edit_group_ = this_group->name_;
00611   current_edit_element_ = CHAIN;
00612 }
00613 
00614 // ******************************************************************************************
00615 // Load the popup screen with correct data for subgroups
00616 // ******************************************************************************************
00617 void PlanningGroupsWidget::loadSubgroupsScreen(srdf::Model::Group* this_group)
00618 {
00619   // Load all groups into the subgroup screen except the current group
00620   std::vector<std::string> subgroups;
00621 
00622   // Display all groups by looping through them
00623   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
00624        group_it != config_data_->srdf_->groups_.end(); ++group_it)
00625   {
00626     if (group_it->name_ != this_group->name_)  //  do not include current group
00627     {
00628       // add to available subgroups list
00629       subgroups.push_back(group_it->name_);
00630     }
00631   }
00632 
00633   // Set the available subgroups (left box)
00634   subgroups_widget_->setAvailable(subgroups);
00635 
00636   // Set the selected subgroups (right box)
00637   subgroups_widget_->setSelected(this_group->subgroups_);
00638 
00639   // Set the title
00640   subgroups_widget_->title_->setText(
00641       QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Subgroups"));
00642 
00643   // Remember what is currently being edited so we can later save changes
00644   current_edit_group_ = this_group->name_;
00645   current_edit_element_ = SUBGROUP;
00646 }
00647 
00648 // ******************************************************************************************
00649 // Load the popup screen with correct data for groups
00650 // ******************************************************************************************
00651 void PlanningGroupsWidget::loadGroupScreen(srdf::Model::Group* this_group)
00652 {
00653   // Load the avail kin solvers. This function only runs once
00654   group_edit_widget_->loadKinematicPlannersComboBox();
00655 
00656   if (this_group == NULL)  // this is a new screen
00657   {
00658     current_edit_group_.clear();  // provide a blank group name
00659     group_edit_widget_->title_->setText("Create New Planning Group");
00660     group_edit_widget_->btn_delete_->hide();
00661     group_edit_widget_->new_buttons_widget_->show();  // helps user choose next step
00662     group_edit_widget_->btn_save_->hide();            // this is only for edit mode
00663   }
00664   else  // load the group name into the widget
00665   {
00666     current_edit_group_ = this_group->name_;
00667     group_edit_widget_->title_->setText(
00668         QString("Edit Planning Group '").append(current_edit_group_.c_str()).append("'"));
00669     group_edit_widget_->btn_delete_->show();
00670     group_edit_widget_->new_buttons_widget_->hide();  // not necessary for existing groups
00671     group_edit_widget_->btn_save_->show();            // this is only for edit mode
00672   }
00673 
00674   // Set the data in the edit box
00675   group_edit_widget_->setSelected(current_edit_group_);
00676 
00677   // Remember what is currently being edited so we can later save changes
00678   current_edit_element_ = GROUP;
00679 }
00680 
00681 // ******************************************************************************************
00682 // Delete a group
00683 // ******************************************************************************************
00684 void PlanningGroupsWidget::deleteGroup()
00685 {
00686   std::string group = current_edit_group_;
00687   if (group.empty())
00688   {
00689     QTreeWidgetItem* item = groups_tree_->currentItem();
00690     // Check that something was actually selected
00691     if (item == NULL)
00692       return;
00693     // Get the user custom properties of the currently selected row
00694     PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
00695     if (plan_group.group_)
00696       group = plan_group.group_->name_;
00697   }
00698   else
00699     current_edit_group_.clear();
00700   if (group.empty())
00701     return;
00702 
00703   // Find the group we are editing based on the goup name string
00704   srdf::Model::Group* searched_group = config_data_->findGroupByName(group);
00705 
00706   // Confirm user wants to delete group
00707   if (QMessageBox::question(
00708           this, "Confirm Group Deletion",
00709           QString("Are you sure you want to delete the planning group '")
00710               .append(searched_group->name_.c_str())
00711               .append("'? This will also delete all references in subgroups, robot poses and end effectors."),
00712           QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
00713   {
00714     return;
00715   }
00716 
00717   // delete all robot poses that use that group's name
00718   bool haveConfirmedGroupStateDeletion = false;
00719   bool haveDeletedGroupState = true;
00720   while (haveDeletedGroupState)
00721   {
00722     haveDeletedGroupState = false;
00723     for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
00724          pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
00725     {
00726       // check if this group state depends on the currently being deleted group
00727       if (pose_it->group_ == searched_group->name_)
00728       {
00729         if (!haveConfirmedGroupStateDeletion)
00730         {
00731           haveConfirmedGroupStateDeletion = true;
00732 
00733           // confirm the user wants to delete group states
00734           if (QMessageBox::question(
00735                   this, "Confirm Group State Deletion",
00736                   QString("The group that is about to be deleted has robot poses (robot states) that depend on this "
00737                           "group. Are you sure you want to delete this group as well as all dependend robot poses?"),
00738                   QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
00739           {
00740             return;
00741           }
00742         }
00743         config_data_->changes |= MoveItConfigData::POSES;
00744 
00745         // the user has confirmed, now delete this group state
00746         config_data_->srdf_->group_states_.erase(pose_it);
00747         haveDeletedGroupState = true;
00748         break;  // you can only delete 1 item in vector before invalidating iterator
00749       }
00750     }
00751   }
00752 
00753   // delete all end effectors that use that group's name
00754   bool haveConfirmedEndEffectorDeletion = false;
00755   bool haveDeletedEndEffector = true;
00756   while (haveDeletedEndEffector)
00757   {
00758     haveDeletedEndEffector = false;
00759     for (std::vector<srdf::Model::EndEffector>::iterator effector_it = config_data_->srdf_->end_effectors_.begin();
00760          effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it)
00761     {
00762       // check if this group state depends on the currently being deleted group
00763       if (effector_it->component_group_ == searched_group->name_)
00764       {
00765         if (!haveConfirmedEndEffectorDeletion)
00766         {
00767           haveConfirmedEndEffectorDeletion = true;
00768 
00769           // confirm the user wants to delete group states
00770           if (QMessageBox::question(
00771                   this, "Confirm End Effector Deletion",
00772                   QString("The group that is about to be deleted has end effectors (grippers) that depend on this "
00773                           "group. Are you sure you want to delete this group as well as all dependend end effectors?"),
00774                   QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
00775           {
00776             return;
00777           }
00778         }
00779         config_data_->changes |= MoveItConfigData::END_EFFECTORS;
00780 
00781         // the user has confirmed, now delete this group state
00782         config_data_->srdf_->end_effectors_.erase(effector_it);
00783         haveDeletedEndEffector = true;
00784         break;  // you can only delete 1 item in vector before invalidating iterator
00785       }
00786     }
00787   }
00788 
00789   config_data_->changes |= MoveItConfigData::GROUPS;
00790 
00791   // delete actual group
00792   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
00793        group_it != config_data_->srdf_->groups_.end(); ++group_it)
00794   {
00795     // check if this is the group we want to delete
00796     if (group_it->name_ == group)  // string match
00797     {
00798       config_data_->srdf_->groups_.erase(group_it);
00799       break;
00800     }
00801   }
00802 
00803   // loop again to delete all subgroup references
00804   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
00805        group_it != config_data_->srdf_->groups_.end(); ++group_it)
00806   {
00807     // delete all subgroup references
00808     bool deleted_subgroup = true;
00809     while (deleted_subgroup)
00810     {
00811       deleted_subgroup = false;
00812 
00813       // check if the subgroups reference our deleted group
00814       for (std::vector<std::string>::iterator subgroup_it = group_it->subgroups_.begin();
00815            subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
00816       {
00817         // Check if that subgroup references the deletion group. if so, delete it
00818         if (subgroup_it->compare(group) == 0)  // same name
00819         {
00820           group_it->subgroups_.erase(subgroup_it);  // delete
00821           deleted_subgroup = true;
00822           break;
00823         }
00824       }
00825     }
00826   }
00827 
00828   // Switch to main screen
00829   showMainScreen();
00830 
00831   // Reload main screen table
00832   loadGroupsTree();
00833 
00834   // Update the kinematic model with changes
00835   config_data_->updateRobotModel();
00836 }
00837 
00838 // ******************************************************************************************
00839 // Create a new, empty group
00840 // ******************************************************************************************
00841 void PlanningGroupsWidget::addGroup()
00842 {
00843   adding_new_group_ = true;
00844 
00845   // Load the data
00846   loadGroupScreen(NULL);  // NULL indicates this is a new group, not an existing one
00847 
00848   // Switch to screen
00849   changeScreen(5);
00850 }
00851 
00852 // ******************************************************************************************
00853 // Call when joints edit sceen is done and needs to be saved
00854 // ******************************************************************************************
00855 void PlanningGroupsWidget::saveJointsScreen()
00856 {
00857   // Find the group we are editing based on the goup name string
00858   srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
00859 
00860   // clear the old data
00861   searched_group->joints_.clear();
00862 
00863   // copy the data
00864   for (int i = 0; i < joints_widget_->selected_data_table_->rowCount(); ++i)
00865   {
00866     searched_group->joints_.push_back(joints_widget_->selected_data_table_->item(i, 0)->text().toStdString());
00867   }
00868 
00869   // Switch to main screen
00870   showMainScreen();
00871 
00872   // Reload main screen table
00873   loadGroupsTree();
00874 
00875   // Update the kinematic model with changes
00876   config_data_->updateRobotModel();
00877   config_data_->changes |= MoveItConfigData::GROUP_CONTENTS;
00878 }
00879 
00880 // ******************************************************************************************
00881 // Call when links edit sceen is done and needs to be saved
00882 // ******************************************************************************************
00883 void PlanningGroupsWidget::saveLinksScreen()
00884 {
00885   // Find the group we are editing based on the goup name string
00886   srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
00887 
00888   // Find the group we are editing based on the goup name string
00889   // clear the old data
00890   searched_group->links_.clear();
00891 
00892   // copy the data
00893   for (int i = 0; i < links_widget_->selected_data_table_->rowCount(); ++i)
00894   {
00895     searched_group->links_.push_back(links_widget_->selected_data_table_->item(i, 0)->text().toStdString());
00896   }
00897 
00898   // Switch to main screen
00899   showMainScreen();
00900 
00901   // Reload main screen table
00902   loadGroupsTree();
00903 
00904   // Update the kinematic model with changes
00905   config_data_->updateRobotModel();
00906   config_data_->changes |= MoveItConfigData::GROUP_CONTENTS;
00907 }
00908 
00909 // ******************************************************************************************
00910 // Call when chains edit sceen is done and needs to be saved
00911 // ******************************************************************************************
00912 void PlanningGroupsWidget::saveChainScreen()
00913 {
00914   // Find the group we are editing based on the goup name string
00915   srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
00916 
00917   // Get a reference to the supplied strings
00918   const std::string& tip = chain_widget_->tip_link_field_->text().toStdString();
00919   const std::string& base = chain_widget_->base_link_field_->text().toStdString();
00920 
00921   // Check that box the tip and base, or neither, have text
00922   if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty()))
00923   {
00924     QMessageBox::warning(this, "Error Saving", "You must specify a link for both the base and tip, or leave both "
00925                                                "blank.");
00926     return;
00927   }
00928 
00929   // Check that both given links are valid links, unless they are both blank
00930   if (!tip.empty() && !base.empty())
00931   {
00932     // Check that they are not the same link
00933     if (tip.compare(base) == 0)  // they are same
00934     {
00935       QMessageBox::warning(this, "Error Saving", "Tip and base link cannot be the same link.");
00936       return;
00937     }
00938 
00939     bool found_tip = false;
00940     bool found_base = false;
00941     const std::vector<std::string>& links = config_data_->getRobotModel()->getLinkModelNames();
00942 
00943     for (std::vector<std::string>::const_iterator link_it = links.begin(); link_it != links.end(); ++link_it)
00944     {
00945       // Check if string matches either of user specefied links
00946       if (link_it->compare(tip) == 0)  // they are same
00947         found_tip = true;
00948       else if (link_it->compare(base) == 0)  // they are same
00949         found_base = true;
00950 
00951       // Check if we are done searching
00952       if (found_tip && found_base)
00953         break;
00954     }
00955 
00956     // Check if we found both links
00957     if (!found_tip || !found_base)
00958     {
00959       QMessageBox::warning(this, "Error Saving", "Tip or base link(s) were not found in kinematic chain.");
00960       return;
00961     }
00962   }
00963 
00964   // clear the old data
00965   searched_group->chains_.clear();
00966 
00967   // Save the data if there is data to save
00968   if (!tip.empty() && !base.empty())
00969   {
00970     searched_group->chains_.push_back(std::pair<std::string, std::string>(base, tip));
00971   }
00972 
00973   // Switch to main screen
00974   showMainScreen();
00975 
00976   // Reload main screen table
00977   loadGroupsTree();
00978 
00979   // Update the kinematic model with changes
00980   config_data_->updateRobotModel();
00981   config_data_->changes |= MoveItConfigData::GROUP_CONTENTS;
00982 }
00983 
00984 // ******************************************************************************************
00985 // Call when subgroups edit sceen is done and needs to be saved
00986 // ******************************************************************************************
00987 void PlanningGroupsWidget::saveSubgroupsScreen()
00988 {
00989   // Find the group we are editing based on the goup name string
00990   srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
00991 
00992   // Check for cycles -------------------------------
00993 
00994   // Create vector index of all nodes
00995   std::map<std::string, int> group_nodes;
00996 
00997   // Create vector of all nodes for use as id's
00998   int node_id = 0;
00999   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
01000        group_it != config_data_->srdf_->groups_.end(); ++group_it)
01001   {
01002     // Add string to vector
01003     group_nodes.insert(std::pair<std::string, int>(group_it->name_, node_id));
01004     ++node_id;
01005   }
01006 
01007   // Create the empty graph
01008   typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS> Graph;
01009   typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
01010   Graph g(group_nodes.size());
01011 
01012   // Traverse the group list again, this time inserting subgroups into graph
01013   int from_id = 0;  // track the outer node we are on to reduce searches performed
01014   for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
01015        group_it != config_data_->srdf_->groups_.end(); ++group_it)
01016   {
01017     // Check if group_it is same as current group
01018     if (group_it->name_ == searched_group->name_)  // yes, same group
01019     {
01020       // add new subgroup list from widget, not old one. this way we can check for new cycles
01021       for (int i = 0; i < subgroups_widget_->selected_data_table_->rowCount(); ++i)
01022       {
01023         // Get std::string of subgroup
01024         const std::string to_string = subgroups_widget_->selected_data_table_->item(i, 0)->text().toStdString();
01025 
01026         // convert subgroup string to associated id
01027         int to_id = group_nodes[to_string];
01028 
01029         // Add edge from from_id to to_id
01030         add_edge(from_id, to_id, g);
01031       }
01032     }
01033     else  // this group is not the group we are editing, so just add subgroups from memory
01034     {
01035       // add new subgroup list from widget, not old one. this way we can check for new cycles
01036       for (unsigned int i = 0; i < group_it->subgroups_.size(); ++i)
01037       {
01038         // Get std::string of subgroup
01039         const std::string to_string = group_it->subgroups_.at(i);
01040 
01041         // convert subgroup string to associated id
01042         int to_id = group_nodes[to_string];
01043 
01044         // Add edge from from_id to to_id
01045         add_edge(from_id, to_id, g);
01046       }
01047     }
01048 
01049     ++from_id;
01050   }
01051 
01052   // Check for cycles
01053   bool has_cycle = false;
01054   cycle_detector vis(has_cycle);
01055   boost::depth_first_search(g, visitor(vis));
01056 
01057   if (has_cycle)
01058   {
01059     // report to user the error
01060     QMessageBox::warning(this, "Error Saving", "Depth first search reveals a cycle in the subgroups");
01061     return;
01062   }
01063 
01064   // clear the old data
01065   searched_group->subgroups_.clear();
01066 
01067   // copy the data
01068   for (int i = 0; i < subgroups_widget_->selected_data_table_->rowCount(); ++i)
01069   {
01070     searched_group->subgroups_.push_back(subgroups_widget_->selected_data_table_->item(i, 0)->text().toStdString());
01071   }
01072 
01073   // Switch to main screen
01074   showMainScreen();
01075 
01076   // Reload main screen table
01077   loadGroupsTree();
01078 
01079   // Update the kinematic model with changes
01080   config_data_->updateRobotModel();
01081   config_data_->changes |= MoveItConfigData::GROUP_CONTENTS;
01082 }
01083 
01084 // ******************************************************************************************
01085 // Call when groups edit sceen is done and needs to be saved
01086 // ******************************************************************************************
01087 bool PlanningGroupsWidget::saveGroupScreen()
01088 {
01089   // Get a reference to the supplied strings
01090   const std::string& group_name = group_edit_widget_->group_name_field_->text().toStdString();
01091   const std::string& kinematics_solver = group_edit_widget_->kinematics_solver_field_->currentText().toStdString();
01092   const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString();
01093   const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString();
01094   const std::string& kinematics_attempts = group_edit_widget_->kinematics_attempts_field_->text().toStdString();
01095 
01096   // Used for editing existing groups
01097   srdf::Model::Group* searched_group = NULL;
01098 
01099   // Check that a valid group name has been given
01100   if (group_name.empty())
01101   {
01102     QMessageBox::warning(this, "Error Saving", "A name must be given for the group!");
01103     return false;
01104   }
01105 
01106   // Check if this is an existing group
01107   if (!current_edit_group_.empty())
01108   {
01109     // Find the group we are editing based on the goup name string
01110     searched_group = config_data_->findGroupByName(current_edit_group_);
01111   }
01112 
01113   // Check that the group name is unique
01114   for (std::vector<srdf::Model::Group>::const_iterator group_it = config_data_->srdf_->groups_.begin();
01115        group_it != config_data_->srdf_->groups_.end(); ++group_it)
01116   {
01117     if (group_it->name_.compare(group_name) == 0)  // the names are the same
01118     {
01119       // is this our existing group? check if group pointers are same
01120       if (&(*group_it) != searched_group)
01121       {
01122         QMessageBox::warning(this, "Error Saving", "A group already exists with that name!");
01123         return false;
01124       }
01125     }
01126   }
01127 
01128   // Check that the resolution is an double number
01129   double kinematics_resolution_double;
01130   try
01131   {
01132     kinematics_resolution_double = boost::lexical_cast<double>(kinematics_resolution);
01133   }
01134   catch (boost::bad_lexical_cast&)
01135   {
01136     QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics resolution to a double number.");
01137     return false;
01138   }
01139 
01140   // Check that the timeout is a double number
01141   double kinematics_timeout_double;
01142   try
01143   {
01144     kinematics_timeout_double = boost::lexical_cast<double>(kinematics_timeout);
01145   }
01146   catch (boost::bad_lexical_cast&)
01147   {
01148     QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics solver timeout to a double number.");
01149     return false;
01150   }
01151 
01152   // Check that the attempts is an int number
01153   int kinematics_attempts_int;
01154   try
01155   {
01156     kinematics_attempts_int = boost::lexical_cast<int>(kinematics_attempts);
01157   }
01158   catch (boost::bad_lexical_cast&)
01159   {
01160     QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics solver attempts to an int number.");
01161     return false;
01162   }
01163 
01164   // Check that all numbers are >0
01165   if (kinematics_resolution_double <= 0)
01166   {
01167     QMessageBox::warning(this, "Error Saving", "Kinematics solver search resolution must be greater than 0.");
01168     return false;
01169   }
01170   if (kinematics_timeout_double <= 0)
01171   {
01172     QMessageBox::warning(this, "Error Saving", "Kinematics solver search timeout must be greater than 0.");
01173     return false;
01174   }
01175   if (kinematics_attempts_int <= 0)
01176   {
01177     QMessageBox::warning(this, "Error Saving", "Kinematics solver attempts must be greater than 0.");
01178     return false;
01179   }
01180 
01181   adding_new_group_ = false;
01182 
01183   // Save the new group name or create the new group
01184   if (searched_group == NULL)  // create new
01185   {
01186     srdf::Model::Group new_group;
01187     new_group.name_ = group_name;
01188     config_data_->srdf_->groups_.push_back(new_group);
01189     adding_new_group_ = true;  // remember this group is new
01190     config_data_->changes |= MoveItConfigData::GROUPS;
01191   }
01192   else
01193   {
01194     // Remember old group name
01195     const std::string old_group_name = searched_group->name_;
01196 
01197     // Change group name
01198     searched_group->name_ = group_name;
01199 
01200     // Change all references to this group name in other subgroups
01201     // Loop through every group
01202     for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
01203          group_it != config_data_->srdf_->groups_.end(); ++group_it)
01204     {
01205       // Loop through every subgroup
01206       for (std::vector<std::string>::iterator subgroup_it = group_it->subgroups_.begin();
01207            subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
01208       {
01209         // Check if that subgroup references old group name. if so, update it
01210         if (subgroup_it->compare(old_group_name) == 0)  // same name
01211         {
01212           subgroup_it->assign(group_name);  // updated
01213           config_data_->changes |= MoveItConfigData::GROUP_CONTENTS;
01214         }
01215       }
01216     }
01217 
01218     // Change all references to this group name in the end effectors screen
01219     for (std::vector<srdf::Model::EndEffector>::iterator eef_it = config_data_->srdf_->end_effectors_.begin();
01220          eef_it != config_data_->srdf_->end_effectors_.end(); ++eef_it)
01221     {
01222       // Check if this eef's parent group references old group name. if so, update it
01223       if (eef_it->parent_group_.compare(old_group_name) == 0)  // same name
01224       {
01225         ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new parent group name "
01226                                                                   << group_name);
01227         eef_it->parent_group_ = group_name;  // updated
01228         config_data_->changes |= MoveItConfigData::END_EFFECTORS;
01229       }
01230 
01231       // Check if this eef's group references old group name. if so, update it
01232       if (eef_it->component_group_.compare(old_group_name) == 0)  // same name
01233       {
01234         ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new group name "
01235                                                                   << group_name);
01236         eef_it->component_group_ = group_name;  // updated
01237         config_data_->changes |= MoveItConfigData::END_EFFECTORS;
01238       }
01239     }
01240 
01241     // Change all references to this group name in the robot poses screen
01242     for (std::vector<srdf::Model::GroupState>::iterator state_it = config_data_->srdf_->group_states_.begin();
01243          state_it != config_data_->srdf_->group_states_.end(); ++state_it)
01244     {
01245       // Check if this eef's parent group references old group name. if so, update it
01246       if (state_it->group_.compare(old_group_name) == 0)  // same name
01247       {
01248         ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed group state group '" << state_it->group_
01249                                                                                 << "' to new parent group name "
01250                                                                                 << group_name);
01251         state_it->group_ = group_name;  // updated
01252         config_data_->changes |= MoveItConfigData::POSES;
01253       }
01254     }
01255 
01256     // Now update the robot model based on our changed to the SRDF
01257     config_data_->updateRobotModel();
01258   }
01259 
01260   // Save the group meta data
01261   config_data_->group_meta_data_[group_name].kinematics_solver_ = kinematics_solver;
01262   config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double;
01263   config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double;
01264   config_data_->group_meta_data_[group_name].kinematics_solver_attempts_ = kinematics_attempts_int;
01265   config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS;
01266 
01267   // Reload main screen table
01268   loadGroupsTree();
01269 
01270   // Update the current edit group so that we can proceed to the next screen, if user desires
01271   current_edit_group_ = group_name;
01272 
01273   return true;
01274 }
01275 
01276 // ******************************************************************************************
01277 // Call when a new group is created and ready to progress to next screen
01278 // ******************************************************************************************
01279 void PlanningGroupsWidget::saveGroupScreenEdit()
01280 {
01281   // Save the group
01282   if (!saveGroupScreen())
01283     return;
01284 
01285   // Switch to main screen
01286   showMainScreen();
01287 }
01288 
01289 // ******************************************************************************************
01290 // Call when a new group is created and ready to progress to next screen
01291 // ******************************************************************************************
01292 void PlanningGroupsWidget::saveGroupScreenJoints()
01293 {
01294   // Save the group
01295   if (!saveGroupScreen())
01296     return;
01297 
01298   // Find the group we are editing based on the goup name string
01299   loadJointsScreen(config_data_->findGroupByName(current_edit_group_));
01300 
01301   // Switch to screen
01302   changeScreen(1);  // 1 is index of joints
01303 }
01304 
01305 // ******************************************************************************************
01306 // Call when a new group is created and ready to progress to next screen
01307 // ******************************************************************************************
01308 void PlanningGroupsWidget::saveGroupScreenLinks()
01309 {
01310   // Save the group
01311   if (!saveGroupScreen())
01312     return;
01313 
01314   // Find the group we are editing based on the goup name string
01315   loadLinksScreen(config_data_->findGroupByName(current_edit_group_));
01316 
01317   // Switch to screen
01318   changeScreen(2);  // 2 is index of links
01319 }
01320 
01321 // ******************************************************************************************
01322 // Call when a new group is created and ready to progress to next screen
01323 // ******************************************************************************************
01324 void PlanningGroupsWidget::saveGroupScreenChain()
01325 {
01326   // Save the group
01327   if (!saveGroupScreen())
01328     return;
01329 
01330   // Find the group we are editing based on the goup name string
01331   loadChainScreen(config_data_->findGroupByName(current_edit_group_));
01332 
01333   // Switch to screen
01334   changeScreen(3);
01335 }
01336 
01337 // ******************************************************************************************
01338 // Call when a new group is created and ready to progress to next screen
01339 // ******************************************************************************************
01340 void PlanningGroupsWidget::saveGroupScreenSubgroups()
01341 {
01342   // Save the group
01343   if (!saveGroupScreen())
01344     return;
01345 
01346   // Find the group we are editing based on the goup name string
01347   loadSubgroupsScreen(config_data_->findGroupByName(current_edit_group_));
01348 
01349   // Switch to screen
01350   changeScreen(4);
01351 }
01352 
01353 // ******************************************************************************************
01354 // Call when edit screen is canceled
01355 // ******************************************************************************************
01356 void PlanningGroupsWidget::cancelEditing()
01357 {
01358   if (!current_edit_group_.empty() && adding_new_group_)
01359   {
01360     srdf::Model::Group* editing = config_data_->findGroupByName(current_edit_group_);
01361     if (editing && editing->joints_.empty() && editing->links_.empty() && editing->chains_.empty() &&
01362         editing->subgroups_.empty())
01363     {
01364       config_data_->group_meta_data_.erase(editing->name_);
01365       for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
01366            group_it != config_data_->srdf_->groups_.end(); ++group_it)
01367         if (&(*group_it) == editing)
01368         {
01369           config_data_->srdf_->groups_.erase(group_it);
01370           break;
01371         }
01372       current_edit_group_.clear();
01373       // Load the data to the tree
01374       loadGroupsTree();
01375     }
01376   }
01377 
01378   // Switch to main screen
01379   showMainScreen();
01380 }
01381 
01382 // ******************************************************************************************
01383 // Called when setup assistant navigation switches to this screen
01384 // ******************************************************************************************
01385 void PlanningGroupsWidget::focusGiven()
01386 {
01387   // Show the current groups screen
01388   showMainScreen();
01389 
01390   // Load the data to the tree
01391   loadGroupsTree();
01392 }
01393 
01394 // ******************************************************************************************
01395 // Expand/Collapse Tree
01396 // ******************************************************************************************
01397 void PlanningGroupsWidget::alterTree(const QString& link)
01398 {
01399   if (link.contains("expand"))
01400     groups_tree_->expandAll();
01401   else
01402     groups_tree_->collapseAll();
01403 }
01404 
01405 // ******************************************************************************************
01406 // Switch to current groups view
01407 // ******************************************************************************************
01408 void PlanningGroupsWidget::showMainScreen()
01409 {
01410   stacked_layout_->setCurrentIndex(0);
01411 
01412   // Announce that this widget is not in modal mode
01413   Q_EMIT isModal(false);
01414 }
01415 
01416 // ******************************************************************************************
01417 // Switch which screen is being shown
01418 // ******************************************************************************************
01419 void PlanningGroupsWidget::changeScreen(int index)
01420 {
01421   stacked_layout_->setCurrentIndex(index);
01422 
01423   // Announce this widget's mode
01424   Q_EMIT isModal(index != 0);
01425 }
01426 
01427 // ******************************************************************************************
01428 // Called from Double List widget to highlight a link
01429 // ******************************************************************************************
01430 void PlanningGroupsWidget::previewSelectedLink(std::vector<std::string> links)
01431 {
01432   // Unhighlight all links
01433   Q_EMIT unhighlightAll();
01434 
01435   for (int i = 0; i < links.size(); ++i)
01436   {
01437     if (links[i].empty())
01438     {
01439       continue;
01440     }
01441 
01442     // Highlight link
01443     Q_EMIT highlightLink(links[i], QColor(255, 0, 0));
01444   }
01445 }
01446 
01447 // ******************************************************************************************
01448 // Called from Double List widget to highlight joints
01449 // ******************************************************************************************
01450 void PlanningGroupsWidget::previewSelectedJoints(std::vector<std::string> joints)
01451 {
01452   // Unhighlight all links
01453   Q_EMIT unhighlightAll();
01454 
01455   for (int i = 0; i < joints.size(); ++i)
01456   {
01457     const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joints[i]);
01458 
01459     // Check that a joint model was found
01460     if (!joint_model)
01461     {
01462       continue;
01463     }
01464 
01465     // Get the name of the link
01466     const std::string link = joint_model->getChildLinkModel()->getName();
01467 
01468     if (link.empty())
01469     {
01470       continue;
01471     }
01472 
01473     // Highlight link
01474     Q_EMIT highlightLink(link, QColor(255, 0, 0));
01475   }
01476 }
01477 
01478 // ******************************************************************************************
01479 // Called from Double List widget to highlight a subgroup
01480 // ******************************************************************************************
01481 void PlanningGroupsWidget::previewSelectedSubgroup(std::vector<std::string> groups)
01482 {
01483   // Unhighlight all links
01484   Q_EMIT unhighlightAll();
01485 
01486   for (int i = 0; i < groups.size(); ++i)
01487   {
01488     // Highlight group
01489     Q_EMIT highlightGroup(groups[i]);
01490   }
01491 }
01492 
01493 }  // namespace
01494 
01495 // ******************************************************************************************
01496 // ******************************************************************************************
01497 // CLASS
01498 // ******************************************************************************************
01499 // ******************************************************************************************
01500 
01501 PlanGroupType::PlanGroupType(srdf::Model::Group* group, const moveit_setup_assistant::GroupType type)
01502   : group_(group), type_(type)
01503 {
01504 }


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