planning_groups_widget.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman */
36 
37 // ******************************************************************************************
38 /* DEVELOPER NOTES
39 
40  This widget has 6 subscreens, located in somewhat different places
41  - Main screen, the tree view of all groups & subgroups - embedded in this file as a function
42  - Add/Edit Group screen - located in group_edit_widget.cpp
43  - Joint Collection Screen - implements the double_list_widget.cpp widget
44  - Link Collection Screen - implements the double_list_widget.cpp widget
45  - Kinematic Chain Screen - uses it own custom widget - kinematic_chain_widget.cpp
46  - Subgroup Screen - implements the double_list_widget.cpp widget
47 */
48 // ******************************************************************************************
49 
50 #include "ros/ros.h"
51 #include "header_widget.h"
52 #include "planning_groups_widget.h"
53 #include <boost/thread.hpp>
54 #include <boost/lexical_cast.hpp> // for checking convertion of string to double
55 // Qt
56 #include <QApplication>
57 #include <QDebug>
58 #include <QVBoxLayout>
59 #include <QHBoxLayout>
60 #include <QLabel>
61 #include <QPushButton>
62 #include <QMessageBox>
63 #include <QString>
64 #include <QLineEdit>
65 #include <QTreeWidgetItem>
66 #include <QHeaderView>
67 // Cycle checking
68 #include <boost/utility.hpp>
69 #include <boost/graph/adjacency_list.hpp>
70 #include <boost/graph/depth_first_search.hpp>
71 #include <boost/graph/visitors.hpp>
72 
73 namespace moveit_setup_assistant
74 {
75 // Name of rviz topic in ROS
76 static const std::string VIS_TOPIC_NAME = "planning_components_visualization";
77 
78 // Used for checking for cycles in a subgroup hierarchy
79 struct cycle_detector : public boost::dfs_visitor<>
80 {
81  cycle_detector(bool& has_cycle) : m_has_cycle(has_cycle)
82  {
83  }
84 
85  template <class Edge, class Graph>
86  void back_edge(Edge, Graph&)
87  {
88  m_has_cycle = true;
89  }
90 
91 protected:
92  bool& m_has_cycle;
93 };
94 
95 // ******************************************************************************************
96 // Constructor
97 // ******************************************************************************************
98 PlanningGroupsWidget::PlanningGroupsWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
99  : SetupScreenWidget(parent), config_data_(config_data)
100 {
101  // Basic widget container
102  QVBoxLayout* layout = new QVBoxLayout();
103 
104  // Top Label Area ------------------------------------------------
105  HeaderWidget* header = new HeaderWidget(
106  "Define Planning Groups",
107  "Create and edit 'joint model' groups for your robot based on joint collections, "
108  "link collections, kinematic chains or subgroups. "
109  "A planning group defines the set of (joint, link) pairs considered for planning "
110  "and collision checking. Define individual groups for each subset of the robot you want to plan for."
111  "Note: when adding a link to the group, its parent joint is added too and vice versa.",
112  this);
113  layout->addWidget(header);
114 
115  // Left Side ---------------------------------------------
116 
117  // Create left side widgets
118  groups_tree_widget_ = createContentsWidget(); // included in this file
119 
120  // Joints edit widget
121  joints_widget_ = new DoubleListWidget(this, config_data_, "Joint Collection", "Joint");
122  connect(joints_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
123  connect(joints_widget_, SIGNAL(doneEditing()), this, SLOT(saveJointsScreen()));
124  connect(joints_widget_, SIGNAL(previewSelected(std::vector<std::string>)), this,
125  SLOT(previewSelectedJoints(std::vector<std::string>)));
126 
127  // Links edit widget
128  links_widget_ = new DoubleListWidget(this, config_data_, "Link Collection", "Link");
129  connect(links_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
130  connect(links_widget_, SIGNAL(doneEditing()), this, SLOT(saveLinksScreen()));
131  connect(links_widget_, SIGNAL(previewSelected(std::vector<std::string>)), this,
132  SLOT(previewSelectedLink(std::vector<std::string>)));
133 
134  // Chain Widget
135  chain_widget_ = new KinematicChainWidget(this, config_data);
136  connect(chain_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
137  connect(chain_widget_, SIGNAL(doneEditing()), this, SLOT(saveChainScreen()));
138  connect(chain_widget_, SIGNAL(unhighlightAll()), this, SIGNAL(unhighlightAll()));
139  connect(chain_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
140  SIGNAL(highlightLink(const std::string&, const QColor&)));
141 
142  // Subgroups Widget
143  subgroups_widget_ = new DoubleListWidget(this, config_data_, "Subgroup", "Subgroup");
144  connect(subgroups_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
145  connect(subgroups_widget_, SIGNAL(doneEditing()), this, SLOT(saveSubgroupsScreen()));
146  connect(subgroups_widget_, SIGNAL(previewSelected(std::vector<std::string>)), this,
147  SLOT(previewSelectedSubgroup(std::vector<std::string>)));
148 
149  // Group Edit Widget
151  connect(group_edit_widget_, SIGNAL(cancelEditing()), this, SLOT(cancelEditing()));
152  connect(group_edit_widget_, SIGNAL(deleteGroup()), this, SLOT(deleteGroup()));
153  connect(group_edit_widget_, SIGNAL(save()), this, SLOT(saveGroupScreenEdit()));
154  connect(group_edit_widget_, SIGNAL(saveJoints()), this, SLOT(saveGroupScreenJoints()));
155  connect(group_edit_widget_, SIGNAL(saveLinks()), this, SLOT(saveGroupScreenLinks()));
156  connect(group_edit_widget_, SIGNAL(saveChain()), this, SLOT(saveGroupScreenChain()));
157  connect(group_edit_widget_, SIGNAL(saveSubgroups()), this, SLOT(saveGroupScreenSubgroups()));
158 
159  // Combine into stack
160  stacked_layout_ = new QStackedLayout(this);
161  stacked_layout_->addWidget(groups_tree_widget_); // screen index 0
162  stacked_layout_->addWidget(joints_widget_); // screen index 1
163  stacked_layout_->addWidget(links_widget_); // screen index 2
164  stacked_layout_->addWidget(chain_widget_); // screen index 3
165  stacked_layout_->addWidget(subgroups_widget_); // screen index 4
166  stacked_layout_->addWidget(group_edit_widget_); // screen index 5
167 
168  showMainScreen();
169 
170  // Finish GUI -----------------------------------------------------------
171 
172  // Create Widget wrapper for layout
173  QWidget* stacked_layout_widget = new QWidget(this);
174  stacked_layout_widget->setLayout(stacked_layout_);
175 
176  layout->addWidget(stacked_layout_widget);
177 
178  setLayout(layout);
179 
180  // process the gui
181  QApplication::processEvents();
182 }
183 
184 // ******************************************************************************************
185 // Create the main tree view widget
186 // ******************************************************************************************
188 {
189  // Main widget
190  QWidget* content_widget = new QWidget(this);
191 
192  // Basic widget container
193  QVBoxLayout* layout = new QVBoxLayout(this);
194 
195  // Tree Box ----------------------------------------------------------------------
196 
197  groups_tree_ = new QTreeWidget(this);
198  groups_tree_->setHeaderLabel("Current Groups");
199  connect(groups_tree_, SIGNAL(itemDoubleClicked(QTreeWidgetItem*, int)), this, SLOT(editSelected()));
200  connect(groups_tree_, SIGNAL(itemClicked(QTreeWidgetItem*, int)), this, SLOT(previewSelected()));
201  layout->addWidget(groups_tree_);
202 
203  // Bottom Controls -------------------------------------------------------------
204 
205  QHBoxLayout* controls_layout = new QHBoxLayout();
206 
207  // Expand/Contract controls
208  QLabel* expand_controls = new QLabel(this);
209  expand_controls->setText("<a href='expand'>Expand All</a> <a href='contract'>Collapse All</a>");
210  connect(expand_controls, SIGNAL(linkActivated(const QString)), this, SLOT(alterTree(const QString)));
211  controls_layout->addWidget(expand_controls);
212 
213  // Spacer
214  QWidget* spacer = new QWidget(this);
215  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
216  controls_layout->addWidget(spacer);
217 
218  // Delete Selected Button
219  btn_delete_ = new QPushButton("&Delete Selected", this);
220  btn_delete_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
221  btn_delete_->setMaximumWidth(300);
222  connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteGroup()));
223  controls_layout->addWidget(btn_delete_);
224  controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
225 
226  // Edit Selected Button
227  btn_edit_ = new QPushButton("&Edit Selected", this);
228  btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
229  btn_edit_->setMaximumWidth(300);
230  btn_edit_->hide(); // show once we know if there are existing groups
231  connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
232  controls_layout->addWidget(btn_edit_);
233  controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
234 
235  // Add Group Button
236  QPushButton* btn_add = new QPushButton("&Add Group", this);
237  btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
238  btn_add->setMaximumWidth(300);
239  connect(btn_add, SIGNAL(clicked()), this, SLOT(addGroup()));
240  controls_layout->addWidget(btn_add);
241  controls_layout->setAlignment(btn_add, Qt::AlignRight);
242 
243  // Add Controls to layout
244  layout->addLayout(controls_layout);
245 
246  // Set layout
247  content_widget->setLayout(layout);
248 
249  return content_widget;
250 }
251 
252 // ******************************************************************************************
253 // Displays data in the link_pairs_ data structure into a QtTableWidget
254 // ******************************************************************************************
256 {
257  // Disable Tree
258  groups_tree_->setUpdatesEnabled(false); // prevent table from updating until we are completely done
259  groups_tree_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called
260  groups_tree_->clear(); // reset the tree
261 
262  // Display all groups by looping through them
263  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
264  group_it != config_data_->srdf_->groups_.end(); ++group_it)
265  {
266  loadGroupsTreeRecursive(*group_it, NULL);
267  }
268 
269  // Reenable Tree
270  groups_tree_->setUpdatesEnabled(true); // prevent table from updating until we are completely done
271  groups_tree_->setDisabled(false); // make sure we disable it so that the cellChanged event is not called
272 
273  // Show Edit button if there are things to edit
274  if (!config_data_->srdf_->groups_.empty())
275  {
276  btn_edit_->show();
277  btn_delete_->show();
278  }
279  else
280  {
281  btn_edit_->hide();
282  btn_delete_->hide();
283  }
284 
285  alterTree("expand");
286 }
287 
288 // ******************************************************************************************
289 // Recursively Adds Groups, and subgroups to groups...
290 // ******************************************************************************************
292 {
293  // Fonts for tree
294  const QFont top_level_font(QFont().defaultFamily(), 11, QFont::Bold);
295  const QFont type_font(QFont().defaultFamily(), 11, QFont::Normal, QFont::StyleItalic);
296 
297  QTreeWidgetItem* group;
298 
299  // Allow a subgroup to open into a whole new group
300  if (parent == NULL)
301  {
302  group = new QTreeWidgetItem(groups_tree_);
303  group->setText(0, group_it.name_.c_str());
304  group->setFont(0, top_level_font);
305  group->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, GROUP)));
306  groups_tree_->addTopLevelItem(group);
307  }
308  else
309  {
310  group = new QTreeWidgetItem(parent);
311  group->setText(0, group_it.name_.c_str());
312  group->setFont(0, top_level_font);
313  group->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, GROUP)));
314  parent->addChild(group);
315  }
316 
317  // Joints --------------------------------------------------------------
318  QTreeWidgetItem* joints = new QTreeWidgetItem(group);
319  joints->setText(0, "Joints");
320  joints->setFont(0, type_font);
321  joints->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, JOINT)));
322  group->addChild(joints);
323 
324  // Retrieve pointer to the shared kinematic model
325  const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
326 
327  // Loop through all aval. joints
328  for (std::vector<std::string>::const_iterator joint_it = group_it.joints_.begin(); joint_it != group_it.joints_.end();
329  ++joint_it)
330  {
331  QTreeWidgetItem* j = new QTreeWidgetItem(joints);
332  j->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, JOINT)));
333  std::string joint_name;
334 
335  // Get the type of joint this is
336  const robot_model::JointModel* jm = model->getJointModel(*joint_it);
337  if (jm) // check if joint model was found
338  {
339  joint_name = *joint_it + " - " + jm->getTypeName();
340  }
341  else
342  {
343  joint_name = *joint_it;
344  }
345 
346  // Add to tree
347  j->setText(0, joint_name.c_str());
348  joints->addChild(j);
349  }
350 
351  // Links -------------------------------------------------------------
352  QTreeWidgetItem* links = new QTreeWidgetItem(group);
353  links->setText(0, "Links");
354  links->setFont(0, type_font);
355  links->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, LINK)));
356  group->addChild(links);
357 
358  // Loop through all aval. links
359  for (std::vector<std::string>::const_iterator joint_it = group_it.links_.begin(); joint_it != group_it.links_.end();
360  ++joint_it)
361  {
362  QTreeWidgetItem* j = new QTreeWidgetItem(links);
363  j->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, LINK)));
364  j->setText(0, joint_it->c_str());
365  links->addChild(j);
366  }
367 
368  // Chains -------------------------------------------------------------
369  QTreeWidgetItem* chains = new QTreeWidgetItem(group);
370  chains->setText(0, "Chain");
371  chains->setFont(0, type_font);
372  chains->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, CHAIN)));
373  group->addChild(chains);
374 
375  // Warn if there is more than 1 chain per group
376  static bool warn_once = true;
377  if (group_it.chains_.size() > 1 && warn_once)
378  {
379  warn_once = false;
380  QMessageBox::warning(this, "Group with Multiple Kinematic Chains",
381  "Warning: this MoveIt! Setup Assistant is only designed to handle one kinematic chain per "
382  "group. The loaded SRDF has more than one kinematic chain for a group. A possible loss of "
383  "data may occur.");
384  }
385 
386  // Loop through all aval. chains
387  for (std::vector<std::pair<std::string, std::string> >::const_iterator chain_it = group_it.chains_.begin();
388  chain_it != group_it.chains_.end(); ++chain_it)
389  {
390  QTreeWidgetItem* j = new QTreeWidgetItem(chains);
391  j->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, CHAIN)));
392  j->setText(0, QString(chain_it->first.c_str()).append(" -> ").append(chain_it->second.c_str()));
393  chains->addChild(j);
394  }
395 
396  // Subgroups -------------------------------------------------------------
397  QTreeWidgetItem* subgroups = new QTreeWidgetItem(group);
398  subgroups->setText(0, "Subgroups");
399  subgroups->setFont(0, type_font);
400  subgroups->setData(0, Qt::UserRole, QVariant::fromValue(PlanGroupType(&group_it, SUBGROUP)));
401  group->addChild(subgroups);
402 
403  // Loop through all aval. subgroups
404  for (std::vector<std::string>::iterator subgroup_it = group_it.subgroups_.begin();
405  subgroup_it != group_it.subgroups_.end(); ++subgroup_it)
406  {
407  // Find group with this subgroups' name
408 
409  srdf::Model::Group* searched_group = NULL; // used for holding our search results
410 
411  for (std::vector<srdf::Model::Group>::iterator group2_it = config_data_->srdf_->groups_.begin();
412  group2_it != config_data_->srdf_->groups_.end(); ++group2_it)
413  {
414  if (group2_it->name_ == *subgroup_it) // this is the group we are looking for
415  {
416  searched_group = &(*group2_it); // convert to pointer from iterator
417  break; // we are done searching
418  }
419  }
420 
421  // Check if subgroup was found
422  if (searched_group == NULL) // not found
423  {
424  QMessageBox::critical(this, "Error Loading SRDF", QString("Subgroup '")
425  .append(subgroup_it->c_str())
426  .append("' of group '")
427  .append(group_it.name_.c_str())
428  .append("' not found. Your SRDF is invalid"));
429  return; // TODO: something better for error handling?
430  }
431 
432  // subgroup found!
433 
434  // Recurse this function for each new group
435  loadGroupsTreeRecursive(*searched_group, subgroups);
436  }
437 }
438 
439 // ******************************************************************************************
440 // Highlight the group of whatever element is selected in the tree view
441 // ******************************************************************************************
443 {
444  QTreeWidgetItem* item = groups_tree_->currentItem();
445 
446  // Check that something was actually selected
447  if (item == NULL)
448  return;
449 
450  // Get the user custom properties of the currently selected row
451  PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
452 
453  // Unhighlight all links
454  Q_EMIT unhighlightAll();
455 
456  // Highlight the group
457  Q_EMIT(highlightGroup(plan_group.group_->name_));
458 }
459 
460 // ******************************************************************************************
461 // Edit whatever element is selected in the tree view
462 // ******************************************************************************************
464 {
465  QTreeWidgetItem* item = groups_tree_->currentItem();
466 
467  // Check that something was actually selected
468  if (item == NULL)
469  return;
470 
471  adding_new_group_ = false;
472 
473  // Get the user custom properties of the currently selected row
474  PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
475 
476  if (plan_group.type_ == JOINT)
477  {
478  // Load the data
479  loadJointsScreen(plan_group.group_);
480 
481  // Switch to screen
482  changeScreen(1); // 1 is index of joints
483  }
484  else if (plan_group.type_ == LINK)
485  {
486  // Load the data
487  loadLinksScreen(plan_group.group_);
488 
489  // Switch to screen
490  changeScreen(2);
491  }
492  else if (plan_group.type_ == CHAIN)
493  {
494  // Load the data
495  loadChainScreen(plan_group.group_);
496 
497  // Switch to screen
498  changeScreen(3);
499  }
500  else if (plan_group.type_ == SUBGROUP)
501  {
502  // Load the data
503  loadSubgroupsScreen(plan_group.group_);
504 
505  // Switch to screen
506  changeScreen(4);
507  }
508  else if (plan_group.type_ == GROUP)
509  {
510  // Load the data
511  loadGroupScreen(plan_group.group_);
512 
513  // Switch to screen
514  changeScreen(5);
515  }
516  else
517  {
518  QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading.");
519  }
520 }
521 
522 // ******************************************************************************************
523 // Load the popup screen with correct data for joints
524 // ******************************************************************************************
526 {
527  // Retrieve pointer to the shared kinematic model
528  const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
529 
530  // Get the names of the all joints
531  const std::vector<std::string>& joints = model->getJointModelNames();
532 
533  if (joints.size() == 0)
534  {
535  QMessageBox::critical(this, "Error Loading", "No joints found for robot model");
536  return;
537  }
538 
539  // Set the available joints (left box)
540  joints_widget_->setAvailable(joints);
541 
542  // Set the selected joints (right box)
543  joints_widget_->setSelected(this_group->joints_);
544 
545  // Set the title
546  joints_widget_->title_->setText(
547  QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Joint Collection"));
548 
549  // Remember what is currently being edited so we can later save changes
550  current_edit_group_ = this_group->name_;
552 }
553 
554 // ******************************************************************************************
555 // Load the popup screen with correct data for links
556 // ******************************************************************************************
558 {
559  // Retrieve pointer to the shared kinematic model
560  const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
561 
562  // Get the names of the all links
563  const std::vector<std::string>& links = model->getLinkModelNames();
564 
565  if (links.size() == 0)
566  {
567  QMessageBox::critical(this, "Error Loading", "No links found for robot model");
568  return;
569  }
570 
571  // Set the available links (left box)
572  links_widget_->setAvailable(links);
573 
574  // Set the selected links (right box)
575  links_widget_->setSelected(this_group->links_);
576 
577  // Set the title
578  links_widget_->title_->setText(
579  QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Link Collection"));
580 
581  // Remember what is currently being edited so we can later save changes
582  current_edit_group_ = this_group->name_;
584 }
585 
586 // ******************************************************************************************
587 // Load the popup screen with correct data for chains
588 // ******************************************************************************************
590 {
591  // Tell the kin chain widget to load up the chain from a kinematic model
593 
594  // Make sure there isn't more than 1 chain pair
595  if (this_group->chains_.size() > 1)
596  {
597  QMessageBox::warning(this, "Multiple Kinematic Chains", "Warning: This setup assistant is only designed to handle "
598  "one kinematic chain per group. The loaded SRDF has more "
599  "than one kinematic chain for a group. A possible loss of "
600  "data may occur.");
601  }
602 
603  // Set the selected tip and base of chain if one exists
604  if (this_group->chains_.size() > 0)
605  {
606  chain_widget_->setSelected(this_group->chains_[0].first, this_group->chains_[0].second);
607  }
608 
609  // Set the title
610  chain_widget_->title_->setText(
611  QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Kinematic Chain"));
612 
613  // Remember what is currently being edited so we can later save changes
614  current_edit_group_ = this_group->name_;
616 }
617 
618 // ******************************************************************************************
619 // Load the popup screen with correct data for subgroups
620 // ******************************************************************************************
622 {
623  // Load all groups into the subgroup screen except the current group
624  std::vector<std::string> subgroups;
625 
626  // Display all groups by looping through them
627  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
628  group_it != config_data_->srdf_->groups_.end(); ++group_it)
629  {
630  if (group_it->name_ != this_group->name_) // do not include current group
631  {
632  // add to available subgroups list
633  subgroups.push_back(group_it->name_);
634  }
635  }
636 
637  // Set the available subgroups (left box)
638  subgroups_widget_->setAvailable(subgroups);
639 
640  // Set the selected subgroups (right box)
642 
643  // Set the title
644  subgroups_widget_->title_->setText(
645  QString("Edit '").append(QString::fromUtf8(this_group->name_.c_str())).append("' Subgroups"));
646 
647  // Remember what is currently being edited so we can later save changes
648  current_edit_group_ = this_group->name_;
650 }
651 
652 // ******************************************************************************************
653 // Load the popup screen with correct data for groups
654 // ******************************************************************************************
656 {
657  // Load the avail kin solvers. This function only runs once
659 
660  if (this_group == NULL) // this is a new screen
661  {
662  current_edit_group_.clear(); // provide a blank group name
663  group_edit_widget_->title_->setText("Create New Planning Group");
665  group_edit_widget_->new_buttons_widget_->show(); // helps user choose next step
666  group_edit_widget_->btn_save_->hide(); // this is only for edit mode
667  }
668  else // load the group name into the widget
669  {
670  current_edit_group_ = this_group->name_;
671  group_edit_widget_->title_->setText(
672  QString("Edit Planning Group '").append(current_edit_group_.c_str()).append("'"));
674  group_edit_widget_->new_buttons_widget_->hide(); // not necessary for existing groups
675  group_edit_widget_->btn_save_->show(); // this is only for edit mode
676  }
677 
678  // Set the data in the edit box
680 
681  // Remember what is currently being edited so we can later save changes
683 }
684 
685 // ******************************************************************************************
686 // Delete a group
687 // ******************************************************************************************
689 {
690  std::string group = current_edit_group_;
691  if (group.empty())
692  {
693  QTreeWidgetItem* item = groups_tree_->currentItem();
694  // Check that something was actually selected
695  if (item == NULL)
696  return;
697  // Get the user custom properties of the currently selected row
698  PlanGroupType plan_group = item->data(0, Qt::UserRole).value<PlanGroupType>();
699  if (plan_group.group_)
700  group = plan_group.group_->name_;
701  }
702  else
703  current_edit_group_.clear();
704  if (group.empty())
705  return;
706 
707  // Find the group we are editing based on the goup name string
708  srdf::Model::Group* searched_group = config_data_->findGroupByName(group);
709 
710  // Confirm user wants to delete group
711  if (QMessageBox::question(
712  this, "Confirm Group Deletion",
713  QString("Are you sure you want to delete the planning group '")
714  .append(searched_group->name_.c_str())
715  .append("'? This will also delete all references in subgroups, robot poses and end effectors."),
716  QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
717  {
718  return;
719  }
720 
721  // delete all robot poses that use that group's name
722  bool haveConfirmedGroupStateDeletion = false;
723  bool haveDeletedGroupState = true;
724  while (haveDeletedGroupState)
725  {
726  haveDeletedGroupState = false;
727  for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
728  pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
729  {
730  // check if this group state depends on the currently being deleted group
731  if (pose_it->group_ == searched_group->name_)
732  {
733  if (!haveConfirmedGroupStateDeletion)
734  {
735  haveConfirmedGroupStateDeletion = true;
736 
737  // confirm the user wants to delete group states
738  if (QMessageBox::question(
739  this, "Confirm Group State Deletion",
740  QString("The group that is about to be deleted has robot poses (robot states) that depend on this "
741  "group. Are you sure you want to delete this group as well as all dependend robot poses?"),
742  QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
743  {
744  return;
745  }
746  }
748 
749  // the user has confirmed, now delete this group state
750  config_data_->srdf_->group_states_.erase(pose_it);
751  haveDeletedGroupState = true;
752  break; // you can only delete 1 item in vector before invalidating iterator
753  }
754  }
755  }
756 
757  // delete all end effectors that use that group's name
758  bool haveConfirmedEndEffectorDeletion = false;
759  bool haveDeletedEndEffector = true;
760  while (haveDeletedEndEffector)
761  {
762  haveDeletedEndEffector = false;
763  for (std::vector<srdf::Model::EndEffector>::iterator effector_it = config_data_->srdf_->end_effectors_.begin();
764  effector_it != config_data_->srdf_->end_effectors_.end(); ++effector_it)
765  {
766  // check if this group state depends on the currently being deleted group
767  if (effector_it->component_group_ == searched_group->name_)
768  {
769  if (!haveConfirmedEndEffectorDeletion)
770  {
771  haveConfirmedEndEffectorDeletion = true;
772 
773  // confirm the user wants to delete group states
774  if (QMessageBox::question(
775  this, "Confirm End Effector Deletion",
776  QString("The group that is about to be deleted has end effectors (grippers) that depend on this "
777  "group. Are you sure you want to delete this group as well as all dependend end effectors?"),
778  QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
779  {
780  return;
781  }
782  }
784 
785  // the user has confirmed, now delete this group state
786  config_data_->srdf_->end_effectors_.erase(effector_it);
787  haveDeletedEndEffector = true;
788  break; // you can only delete 1 item in vector before invalidating iterator
789  }
790  }
791  }
792 
794 
795  // delete actual group
796  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
797  group_it != config_data_->srdf_->groups_.end(); ++group_it)
798  {
799  // check if this is the group we want to delete
800  if (group_it->name_ == group) // string match
801  {
802  config_data_->srdf_->groups_.erase(group_it);
803  break;
804  }
805  }
806 
807  // loop again to delete all subgroup references
808  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
809  group_it != config_data_->srdf_->groups_.end(); ++group_it)
810  {
811  // delete all subgroup references
812  bool deleted_subgroup = true;
813  while (deleted_subgroup)
814  {
815  deleted_subgroup = false;
816 
817  // check if the subgroups reference our deleted group
818  for (std::vector<std::string>::iterator subgroup_it = group_it->subgroups_.begin();
819  subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
820  {
821  // Check if that subgroup references the deletion group. if so, delete it
822  if (subgroup_it->compare(group) == 0) // same name
823  {
824  group_it->subgroups_.erase(subgroup_it); // delete
825  deleted_subgroup = true;
826  break;
827  }
828  }
829  }
830  }
831 
832  // Switch to main screen
833  showMainScreen();
834 
835  // Reload main screen table
836  loadGroupsTree();
837 
838  // Update the kinematic model with changes
839  config_data_->updateRobotModel();
840 }
841 
842 // ******************************************************************************************
843 // Create a new, empty group
844 // ******************************************************************************************
846 {
847  adding_new_group_ = true;
848 
849  // Load the data
850  loadGroupScreen(NULL); // NULL indicates this is a new group, not an existing one
851 
852  // Switch to screen
853  changeScreen(5);
854 }
855 
856 // ******************************************************************************************
857 // Call when joints edit sceen is done and needs to be saved
858 // ******************************************************************************************
860 {
861  // Find the group we are editing based on the goup name string
862  srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
863 
864  // clear the old data
865  searched_group->joints_.clear();
866 
867  // copy the data
868  for (int i = 0; i < joints_widget_->selected_data_table_->rowCount(); ++i)
869  {
870  searched_group->joints_.push_back(joints_widget_->selected_data_table_->item(i, 0)->text().toStdString());
871  }
872 
873  // Switch to main screen
874  showMainScreen();
875 
876  // Reload main screen table
877  loadGroupsTree();
878 
879  // Update the kinematic model with changes
880  config_data_->updateRobotModel();
882 }
883 
884 // ******************************************************************************************
885 // Call when links edit sceen is done and needs to be saved
886 // ******************************************************************************************
888 {
889  // Find the group we are editing based on the goup name string
890  srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
891 
892  // Find the group we are editing based on the goup name string
893  // clear the old data
894  searched_group->links_.clear();
895 
896  // copy the data
897  for (int i = 0; i < links_widget_->selected_data_table_->rowCount(); ++i)
898  {
899  searched_group->links_.push_back(links_widget_->selected_data_table_->item(i, 0)->text().toStdString());
900  }
901 
902  // Switch to main screen
903  showMainScreen();
904 
905  // Reload main screen table
906  loadGroupsTree();
907 
908  // Update the kinematic model with changes
909  config_data_->updateRobotModel();
911 }
912 
913 // ******************************************************************************************
914 // Call when chains edit sceen is done and needs to be saved
915 // ******************************************************************************************
917 {
918  // Find the group we are editing based on the goup name string
919  srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
920 
921  // Get a reference to the supplied strings
922  const std::string& tip = chain_widget_->tip_link_field_->text().trimmed().toStdString();
923  const std::string& base = chain_widget_->base_link_field_->text().trimmed().toStdString();
924 
925  // Check that box the tip and base, or neither, have text
926  if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty()))
927  {
928  QMessageBox::warning(this, "Error Saving", "You must specify a link for both the base and tip, or leave both "
929  "blank.");
930  return;
931  }
932 
933  // Check that both given links are valid links, unless they are both blank
934  if (!tip.empty() && !base.empty())
935  {
936  // Check that they are not the same link
937  if (tip.compare(base) == 0) // they are same
938  {
939  QMessageBox::warning(this, "Error Saving", "Tip and base link cannot be the same link.");
940  return;
941  }
942 
943  bool found_tip = false;
944  bool found_base = false;
945  const std::vector<std::string>& links = config_data_->getRobotModel()->getLinkModelNames();
946 
947  for (std::vector<std::string>::const_iterator link_it = links.begin(); link_it != links.end(); ++link_it)
948  {
949  // Check if string matches either of user specefied links
950  if (link_it->compare(tip) == 0) // they are same
951  found_tip = true;
952  else if (link_it->compare(base) == 0) // they are same
953  found_base = true;
954 
955  // Check if we are done searching
956  if (found_tip && found_base)
957  break;
958  }
959 
960  // Check if we found both links
961  if (!found_tip || !found_base)
962  {
963  QMessageBox::warning(this, "Error Saving", "Tip or base link(s) were not found in kinematic chain.");
964  return;
965  }
966  }
967 
968  // clear the old data
969  searched_group->chains_.clear();
970 
971  // Save the data if there is data to save
972  if (!tip.empty() && !base.empty())
973  {
974  searched_group->chains_.push_back(std::pair<std::string, std::string>(base, tip));
975  }
976 
977  // Switch to main screen
978  showMainScreen();
979 
980  // Reload main screen table
981  loadGroupsTree();
982 
983  // Update the kinematic model with changes
984  config_data_->updateRobotModel();
986 }
987 
988 // ******************************************************************************************
989 // Call when subgroups edit sceen is done and needs to be saved
990 // ******************************************************************************************
992 {
993  // Find the group we are editing based on the goup name string
994  srdf::Model::Group* searched_group = config_data_->findGroupByName(current_edit_group_);
995 
996  // Check for cycles -------------------------------
997 
998  // Create vector index of all nodes
999  std::map<std::string, int> group_nodes;
1000 
1001  // Create vector of all nodes for use as id's
1002  int node_id = 0;
1003  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
1004  group_it != config_data_->srdf_->groups_.end(); ++group_it)
1005  {
1006  // Add string to vector
1007  group_nodes.insert(std::pair<std::string, int>(group_it->name_, node_id));
1008  ++node_id;
1009  }
1010 
1011  // Create the empty graph
1012  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS> Graph;
1013  Graph g(group_nodes.size());
1014 
1015  // Traverse the group list again, this time inserting subgroups into graph
1016  int from_id = 0; // track the outer node we are on to reduce searches performed
1017  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
1018  group_it != config_data_->srdf_->groups_.end(); ++group_it)
1019  {
1020  // Check if group_it is same as current group
1021  if (group_it->name_ == searched_group->name_) // yes, same group
1022  {
1023  // add new subgroup list from widget, not old one. this way we can check for new cycles
1024  for (int i = 0; i < subgroups_widget_->selected_data_table_->rowCount(); ++i)
1025  {
1026  // Get std::string of subgroup
1027  const std::string to_string = subgroups_widget_->selected_data_table_->item(i, 0)->text().toStdString();
1028 
1029  // convert subgroup string to associated id
1030  int to_id = group_nodes[to_string];
1031 
1032  // Add edge from from_id to to_id
1033  add_edge(from_id, to_id, g);
1034  }
1035  }
1036  else // this group is not the group we are editing, so just add subgroups from memory
1037  {
1038  // add new subgroup list from widget, not old one. this way we can check for new cycles
1039  for (unsigned int i = 0; i < group_it->subgroups_.size(); ++i)
1040  {
1041  // Get std::string of subgroup
1042  const std::string to_string = group_it->subgroups_.at(i);
1043 
1044  // convert subgroup string to associated id
1045  int to_id = group_nodes[to_string];
1046 
1047  // Add edge from from_id to to_id
1048  add_edge(from_id, to_id, g);
1049  }
1050  }
1051 
1052  ++from_id;
1053  }
1054 
1055  // Check for cycles
1056  bool has_cycle = false;
1057  cycle_detector vis(has_cycle);
1058  boost::depth_first_search(g, visitor(vis));
1059 
1060  if (has_cycle)
1061  {
1062  // report to user the error
1063  QMessageBox::warning(this, "Error Saving", "Depth first search reveals a cycle in the subgroups");
1064  return;
1065  }
1066 
1067  // clear the old data
1068  searched_group->subgroups_.clear();
1069 
1070  // copy the data
1071  for (int i = 0; i < subgroups_widget_->selected_data_table_->rowCount(); ++i)
1072  {
1073  searched_group->subgroups_.push_back(subgroups_widget_->selected_data_table_->item(i, 0)->text().toStdString());
1074  }
1075 
1076  // Switch to main screen
1077  showMainScreen();
1078 
1079  // Reload main screen table
1080  loadGroupsTree();
1081 
1082  // Update the kinematic model with changes
1083  config_data_->updateRobotModel();
1085 }
1086 
1087 // ******************************************************************************************
1088 // Call when groups edit sceen is done and needs to be saved
1089 // ******************************************************************************************
1091 {
1092  // Get a reference to the supplied strings
1093  const std::string& group_name = group_edit_widget_->group_name_field_->text().trimmed().toStdString();
1094  const std::string& kinematics_solver = group_edit_widget_->kinematics_solver_field_->currentText().toStdString();
1095  const std::string& default_planner = group_edit_widget_->default_planner_field_->currentText().toStdString();
1096  const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString();
1097  const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString();
1098  const std::string& kinematics_attempts = group_edit_widget_->kinematics_attempts_field_->text().toStdString();
1099 
1100  // Used for editing existing groups
1101  srdf::Model::Group* searched_group = NULL;
1102 
1103  // Check that a valid group name has been given
1104  if (group_name.empty())
1105  {
1106  QMessageBox::warning(this, "Error Saving", "A name must be given for the group!");
1107  return false;
1108  }
1109 
1110  // Check if this is an existing group
1111  if (!current_edit_group_.empty())
1112  {
1113  // Find the group we are editing based on the goup name string
1114  searched_group = config_data_->findGroupByName(current_edit_group_);
1115  }
1116 
1117  // Check that the group name is unique
1118  for (std::vector<srdf::Model::Group>::const_iterator group_it = config_data_->srdf_->groups_.begin();
1119  group_it != config_data_->srdf_->groups_.end(); ++group_it)
1120  {
1121  if (group_it->name_.compare(group_name) == 0) // the names are the same
1122  {
1123  // is this our existing group? check if group pointers are same
1124  if (&(*group_it) != searched_group)
1125  {
1126  QMessageBox::warning(this, "Error Saving", "A group already exists with that name!");
1127  return false;
1128  }
1129  }
1130  }
1131 
1132  // Check that the resolution is an double number
1133  double kinematics_resolution_double;
1134  try
1135  {
1136  kinematics_resolution_double = boost::lexical_cast<double>(kinematics_resolution);
1137  }
1138  catch (boost::bad_lexical_cast&)
1139  {
1140  QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics resolution to a double number.");
1141  return false;
1142  }
1143 
1144  // Check that the timeout is a double number
1145  double kinematics_timeout_double;
1146  try
1147  {
1148  kinematics_timeout_double = boost::lexical_cast<double>(kinematics_timeout);
1149  }
1150  catch (boost::bad_lexical_cast&)
1151  {
1152  QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics solver timeout to a double number.");
1153  return false;
1154  }
1155 
1156  // Check that the attempts is an int number
1157  int kinematics_attempts_int;
1158  try
1159  {
1160  kinematics_attempts_int = boost::lexical_cast<int>(kinematics_attempts);
1161  }
1162  catch (boost::bad_lexical_cast&)
1163  {
1164  QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics solver attempts to an int number.");
1165  return false;
1166  }
1167 
1168  // Check that all numbers are >0
1169  if (kinematics_resolution_double <= 0)
1170  {
1171  QMessageBox::warning(this, "Error Saving", "Kinematics solver search resolution must be greater than 0.");
1172  return false;
1173  }
1174  if (kinematics_timeout_double <= 0)
1175  {
1176  QMessageBox::warning(this, "Error Saving", "Kinematics solver search timeout must be greater than 0.");
1177  return false;
1178  }
1179  if (kinematics_attempts_int <= 0)
1180  {
1181  QMessageBox::warning(this, "Error Saving", "Kinematics solver attempts must be greater than 0.");
1182  return false;
1183  }
1184 
1185  adding_new_group_ = false;
1186 
1187  // Save the new group name or create the new group
1188  if (searched_group == NULL) // create new
1189  {
1190  srdf::Model::Group new_group;
1191  new_group.name_ = group_name;
1192  config_data_->srdf_->groups_.push_back(new_group);
1193  adding_new_group_ = true; // remember this group is new
1195  }
1196  else
1197  {
1198  // Remember old group name
1199  const std::string old_group_name = searched_group->name_;
1200 
1201  // Change group name
1202  searched_group->name_ = group_name;
1203 
1204  // Change all references to this group name in other subgroups
1205  // Loop through every group
1206  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
1207  group_it != config_data_->srdf_->groups_.end(); ++group_it)
1208  {
1209  // Loop through every subgroup
1210  for (std::vector<std::string>::iterator subgroup_it = group_it->subgroups_.begin();
1211  subgroup_it != group_it->subgroups_.end(); ++subgroup_it)
1212  {
1213  // Check if that subgroup references old group name. if so, update it
1214  if (subgroup_it->compare(old_group_name) == 0) // same name
1215  {
1216  subgroup_it->assign(group_name); // updated
1218  }
1219  }
1220  }
1221 
1222  // Change all references to this group name in the end effectors screen
1223  for (std::vector<srdf::Model::EndEffector>::iterator eef_it = config_data_->srdf_->end_effectors_.begin();
1224  eef_it != config_data_->srdf_->end_effectors_.end(); ++eef_it)
1225  {
1226  // Check if this eef's parent group references old group name. if so, update it
1227  if (eef_it->parent_group_.compare(old_group_name) == 0) // same name
1228  {
1229  ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new parent group name "
1230  << group_name);
1231  eef_it->parent_group_ = group_name; // updated
1233  }
1234 
1235  // Check if this eef's group references old group name. if so, update it
1236  if (eef_it->component_group_.compare(old_group_name) == 0) // same name
1237  {
1238  ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed eef '" << eef_it->name_ << "' to new group name "
1239  << group_name);
1240  eef_it->component_group_ = group_name; // updated
1242  }
1243  }
1244 
1245  // Change all references to this group name in the robot poses screen
1246  for (std::vector<srdf::Model::GroupState>::iterator state_it = config_data_->srdf_->group_states_.begin();
1247  state_it != config_data_->srdf_->group_states_.end(); ++state_it)
1248  {
1249  // Check if this eef's parent group references old group name. if so, update it
1250  if (state_it->group_.compare(old_group_name) == 0) // same name
1251  {
1252  ROS_DEBUG_STREAM_NAMED("setup_assistant", "Changed group state group '" << state_it->group_
1253  << "' to new parent group name "
1254  << group_name);
1255  state_it->group_ = group_name; // updated
1257  }
1258  }
1259 
1260  // Now update the robot model based on our changed to the SRDF
1261  config_data_->updateRobotModel();
1262  }
1263 
1264  // Save the group meta data
1265  config_data_->group_meta_data_[group_name].kinematics_solver_ = kinematics_solver;
1266  config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double;
1267  config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double;
1268  config_data_->group_meta_data_[group_name].kinematics_solver_attempts_ = kinematics_attempts_int;
1269  config_data_->group_meta_data_[group_name].default_planner_ = default_planner;
1271 
1272  // Reload main screen table
1273  loadGroupsTree();
1274 
1275  // Update the current edit group so that we can proceed to the next screen, if user desires
1276  current_edit_group_ = group_name;
1277 
1278  return true;
1279 }
1280 
1281 // ******************************************************************************************
1282 // Call when a new group is created and ready to progress to next screen
1283 // ******************************************************************************************
1285 {
1286  // Save the group
1287  if (!saveGroupScreen())
1288  return;
1289 
1290  // Switch to main screen
1291  showMainScreen();
1292 }
1293 
1294 // ******************************************************************************************
1295 // Call when a new group is created and ready to progress to next screen
1296 // ******************************************************************************************
1298 {
1299  // Save the group
1300  if (!saveGroupScreen())
1301  return;
1302 
1303  // Find the group we are editing based on the goup name string
1305 
1306  // Switch to screen
1307  changeScreen(1); // 1 is index of joints
1308 }
1309 
1310 // ******************************************************************************************
1311 // Call when a new group is created and ready to progress to next screen
1312 // ******************************************************************************************
1314 {
1315  // Save the group
1316  if (!saveGroupScreen())
1317  return;
1318 
1319  // Find the group we are editing based on the goup name string
1321 
1322  // Switch to screen
1323  changeScreen(2); // 2 is index of links
1324 }
1325 
1326 // ******************************************************************************************
1327 // Call when a new group is created and ready to progress to next screen
1328 // ******************************************************************************************
1330 {
1331  // Save the group
1332  if (!saveGroupScreen())
1333  return;
1334 
1335  // Find the group we are editing based on the goup name string
1337 
1338  // Switch to screen
1339  changeScreen(3);
1340 }
1341 
1342 // ******************************************************************************************
1343 // Call when a new group is created and ready to progress to next screen
1344 // ******************************************************************************************
1346 {
1347  // Save the group
1348  if (!saveGroupScreen())
1349  return;
1350 
1351  // Find the group we are editing based on the goup name string
1353 
1354  // Switch to screen
1355  changeScreen(4);
1356 }
1357 
1358 // ******************************************************************************************
1359 // Call when edit screen is canceled
1360 // ******************************************************************************************
1362 {
1363  if (!current_edit_group_.empty() && adding_new_group_)
1364  {
1365  srdf::Model::Group* editing = config_data_->findGroupByName(current_edit_group_);
1366  if (editing && editing->joints_.empty() && editing->links_.empty() && editing->chains_.empty() &&
1367  editing->subgroups_.empty())
1368  {
1369  config_data_->group_meta_data_.erase(editing->name_);
1370  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
1371  group_it != config_data_->srdf_->groups_.end(); ++group_it)
1372  if (&(*group_it) == editing)
1373  {
1374  config_data_->srdf_->groups_.erase(group_it);
1375  break;
1376  }
1377  current_edit_group_.clear();
1378  // Load the data to the tree
1379  loadGroupsTree();
1380  }
1381  }
1382 
1383  // Switch to main screen
1384  showMainScreen();
1385 }
1386 
1387 // ******************************************************************************************
1388 // Called when setup assistant navigation switches to this screen
1389 // ******************************************************************************************
1391 {
1392  // Show the current groups screen
1393  showMainScreen();
1394 
1395  // Load the data to the tree
1396  loadGroupsTree();
1397 }
1398 
1399 // ******************************************************************************************
1400 // Expand/Collapse Tree
1401 // ******************************************************************************************
1402 void PlanningGroupsWidget::alterTree(const QString& link)
1403 {
1404  if (link.contains("expand"))
1405  groups_tree_->expandAll();
1406  else
1407  groups_tree_->collapseAll();
1408 }
1409 
1410 // ******************************************************************************************
1411 // Switch to current groups view
1412 // ******************************************************************************************
1414 {
1415  stacked_layout_->setCurrentIndex(0);
1416 
1417  // Announce that this widget is not in modal mode
1418  Q_EMIT isModal(false);
1419 }
1420 
1421 // ******************************************************************************************
1422 // Switch which screen is being shown
1423 // ******************************************************************************************
1425 {
1426  stacked_layout_->setCurrentIndex(index);
1427 
1428  // Announce this widget's mode
1429  Q_EMIT isModal(index != 0);
1430 }
1431 
1432 // ******************************************************************************************
1433 // Called from Double List widget to highlight a link
1434 // ******************************************************************************************
1435 void PlanningGroupsWidget::previewSelectedLink(std::vector<std::string> links)
1436 {
1437  // Unhighlight all links
1438  Q_EMIT unhighlightAll();
1439 
1440  for (std::size_t i = 0; i < links.size(); ++i)
1441  {
1442  if (links[i].empty())
1443  {
1444  continue;
1445  }
1446 
1447  // Highlight link
1448  Q_EMIT highlightLink(links[i], QColor(255, 0, 0));
1449  }
1450 }
1451 
1452 // ******************************************************************************************
1453 // Called from Double List widget to highlight joints
1454 // ******************************************************************************************
1455 void PlanningGroupsWidget::previewSelectedJoints(std::vector<std::string> joints)
1456 {
1457  // Unhighlight all links
1458  Q_EMIT unhighlightAll();
1459 
1460  for (std::size_t i = 0; i < joints.size(); ++i)
1461  {
1462  const robot_model::JointModel* joint_model = config_data_->getRobotModel()->getJointModel(joints[i]);
1463 
1464  // Check that a joint model was found
1465  if (!joint_model)
1466  {
1467  continue;
1468  }
1469 
1470  // Get the name of the link
1471  const std::string link = joint_model->getChildLinkModel()->getName();
1472 
1473  if (link.empty())
1474  {
1475  continue;
1476  }
1477 
1478  // Highlight link
1479  Q_EMIT highlightLink(link, QColor(255, 0, 0));
1480  }
1481 }
1482 
1483 // ******************************************************************************************
1484 // Called from Double List widget to highlight a subgroup
1485 // ******************************************************************************************
1486 void PlanningGroupsWidget::previewSelectedSubgroup(std::vector<std::string> groups)
1487 {
1488  // Unhighlight all links
1489  Q_EMIT unhighlightAll();
1490 
1491  for (std::size_t i = 0; i < groups.size(); ++i)
1492  {
1493  // Highlight group
1494  Q_EMIT highlightGroup(groups[i]);
1495  }
1496 }
1497 
1498 } // namespace
1499 
1500 // ******************************************************************************************
1501 // ******************************************************************************************
1502 // CLASS
1503 // ******************************************************************************************
1504 // ******************************************************************************************
1505 
1507  : group_(group), type_(type)
1508 {
1509 }
void setAvailable()
Loads the availble data list.
#define NULL
void setSelected(const std::string &base_link, const std::string &tip_link)
Set the link field with previous value.
std::vector< std::string > subgroups_
void showMainScreen()
Switch to current groups view.
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
MatrixDerived base(const Eigen::MatrixBase< MatrixDerived > &m)
virtual void focusGiven()
Received when this widget is chosen from the navigation menu.
#define ROS_DEBUG_STREAM_NAMED(name, args)
QStackedLayout * stacked_layout_
For changing between table and different add/edit views.
void cancelEditing()
Call when edit screen is canceled.
void setSelected(const std::vector< std::string > &items)
Set the right box.
std::vector< std::pair< std::string, std::string > > chains_
PlanningGroupsWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
std::string current_edit_group_
Remember what group we are editing when an edit screen is being shown.
void loadLinksScreen(srdf::Model::Group *this_group)
void isModal(bool isModal)
Event for when the current screen is in modal view. Essential disabled the left navigation.
void loadGroupsTree()
Displays data in the link_pairs_ data structure into a QtTableWidget.
void highlightLink(const std::string &name, const QColor &)
Event for telling rviz to highlight a link of the robot.
void previewSelectedSubgroup(std::vector< std::string > groups)
Called from Double List widget to highlight a subgroup.
void alterTree(const QString &link)
Called when user clicks link part of bottom left label.
void loadGroupScreen(srdf::Model::Group *this_group)
QTreeWidget * groups_tree_
Main table for holding groups.
QWidget * createContentsWidget()
Builds the main screen list widget.
void loadGroupsTreeRecursive(srdf::Model::Group &group_it, QTreeWidgetItem *parent)
Recursively build the SRDF tree.
srdf::Model::Group * group_
group
void unhighlightAll()
Event for telling rviz to unhighlight all links of the robot.
void setAvailable(const std::vector< std::string > &items)
Loads the availble data list.
void editSelected()
Edit whatever element is selected in the tree view.
GroupType current_edit_element_
Remember what group element we are editing when an edit screen is being shown.
static const std::string VIS_TOPIC_NAME
void previewSelectedJoints(std::vector< std::string > joints)
Called from Double List widget to highlight a joint.
bool adding_new_group_
Remember whethere we&#39;re editing a group or adding a new one.
void loadSubgroupsScreen(srdf::Model::Group *this_group)
std::string name_
void loadJointsScreen(srdf::Model::Group *this_group)
void previewSelected()
Highlight the group of whatever element is selected in the tree view.
std::vector< std::string > links_
void loadKinematicPlannersComboBox()
Populate the combo dropdown box with kinematic planners.
void loadChainScreen(srdf::Model::Group *this_group)
void saveJointsScreen()
Call when screen is done being edited.
std::vector< std::string > joints_
void previewSelectedLink(std::vector< std::string > links)
Called from Double List widget to highlight a link.
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
QPushButton * btn_edit_
Show and hide edit button.
void highlightGroup(const std::string &name)
Event for telling rviz to highlight a group of the robot.
moveit_setup_assistant::GroupType type_
void setSelected(const std::string &group_name)
Set the previous data.


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sun Oct 18 2020 13:19:28