default_collisions_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 #include <QHBoxLayout>
38 #include <QMessageBox>
39 #include <QProgressDialog>
40 #include <QString>
41 #include <QFont>
42 #include <QApplication>
43 #include <QButtonGroup>
44 #include <QRadioButton>
45 #include <QKeyEvent>
46 #include <QMenu>
47 
49 #include "../tools/collision_matrix_model.h"
50 #include "../tools/collision_linear_model.h"
51 #include "../tools/rotated_header_view.h"
52 #include <ros/console.h>
53 
54 namespace moveit_setup_assistant
55 {
56 // ******************************************************************************************
57 // User interface for editing the default collision matrix list in an SRDF
58 // ******************************************************************************************
59 DefaultCollisionsWidget::DefaultCollisionsWidget(QWidget* parent, MoveItConfigDataPtr config_data)
60  : SetupScreenWidget(parent), model_(NULL), selection_model_(NULL), worker_(NULL), config_data_(config_data)
61 {
62  // Basic widget container
63  layout_ = new QVBoxLayout(this);
64 
65  // Top Label Area ------------------------------------------------
66  HeaderWidget* header = new HeaderWidget(
67  "Optimize Self-Collision Checking",
68  "This searches for pairs of robot links that can safely be disabled from collision checking, decreasing motion "
69  "planning time. These pairs are disabled when they are always in collision, never in collision, in collision in "
70  "the robot's default position, or when the links are adjacent to each other on the kinematic chain. Sampling "
71  "density specifies how many random robot positions to check for self collision.",
72  this);
73  layout_->addWidget(header);
74 
75  // Top Button Area -----------------------------------------------
76  controls_box_ = new QGroupBox(this);
77  layout_->addWidget(controls_box_);
78  QVBoxLayout* controls_box_layout = new QVBoxLayout(controls_box_);
79 
80  QHBoxLayout* slider_layout = new QHBoxLayout();
81  slider_layout->setAlignment(Qt::AlignTop | Qt::AlignLeft);
82  controls_box_layout->addLayout(slider_layout);
83 
84  // Slider Label
85  QLabel* density_left_label = new QLabel(this);
86  density_left_label->setText("Sampling Density: Low");
87  slider_layout->addWidget(density_left_label);
88 
89  // Slider
90  density_slider_ = new QSlider(this);
91  density_slider_->setTickPosition(QSlider::TicksBelow);
92  density_slider_->setMinimum(0);
93  density_slider_->setMaximum(99);
94  density_slider_->setSingleStep(10);
95  density_slider_->setPageStep(50);
96  density_slider_->setSliderPosition(9); // 10,000 is default
97  density_slider_->setTickInterval(10);
98  density_slider_->setOrientation(Qt::Horizontal);
99  slider_layout->addWidget(density_slider_);
100  connect(density_slider_, SIGNAL(valueChanged(int)), this, SLOT(changeDensityLabel(int)));
101 
102  // Slider Right Label
103  QLabel* density_right_label = new QLabel(this);
104  density_right_label->setText("High ");
105  slider_layout->addWidget(density_right_label);
106 
107  // Slider Value Label
108  density_value_label_ = new QLabel(this);
109  density_value_label_->setMinimumWidth(50);
110  slider_layout->addWidget(density_value_label_);
111  changeDensityLabel(density_slider_->value()); // initialize label with value
112 
113  QHBoxLayout* buttons_layout = new QHBoxLayout();
114  buttons_layout->setAlignment(Qt::AlignRight);
115  controls_box_layout->addLayout(buttons_layout);
116 
117  // Fraction spin box
118  fraction_label_ = new QLabel(this);
119  fraction_label_->setText("Min. collisions for \"always\"-colliding pairs:");
120  buttons_layout->addWidget(fraction_label_);
121 
122  fraction_spinbox_ = new QSpinBox(this);
123  fraction_spinbox_->setRange(1, 100);
124  fraction_spinbox_->setValue(95);
125  fraction_spinbox_->setSuffix("%");
126  buttons_layout->addWidget(fraction_spinbox_);
127 
128  // Generate Button
129  btn_generate_ = new QPushButton(this);
130  btn_generate_->setText("&Generate Collision Matrix");
131  btn_generate_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
132  connect(btn_generate_, SIGNAL(clicked()), this, SLOT(startGeneratingCollisionTable()));
133  buttons_layout->addWidget(btn_generate_);
134 
135  // Progress Bar Area ---------------------------------------------
136 
137  // Progress Label
138  progress_label_ = new QLabel(this);
139  progress_label_->setText("Generating Default Collision Matrix");
140  progress_label_->hide();
141  layout_->addWidget(progress_label_);
142 
143  // Progress Bar
144  progress_bar_ = new QProgressBar(this);
145  progress_bar_->setMaximum(100);
146  progress_bar_->setMinimum(0);
147  progress_bar_->hide(); // only show when computation begins
148  layout_->addWidget(progress_bar_);
149 
150  // Table Area --------------------------------------------
151 
152  // Table
153  collision_table_ = new QTableView(this);
154  layout_->addWidget(collision_table_);
155 
156  QAction* action;
157  action = new QAction(tr("Show"), this);
158  header_actions_ << action;
159  connect(action, SIGNAL(triggered()), this, SLOT(showSections()));
160  action = new QAction(tr("Hide"), this);
161  header_actions_ << action;
162  connect(action, SIGNAL(triggered()), this, SLOT(hideSections()));
163  action = new QAction(tr("Hide others"), this);
164  header_actions_ << action;
165  connect(action, SIGNAL(triggered()), this, SLOT(hideOtherSections()));
166 
167  // Bottom Area ----------------------------------------
168 
169  QHBoxLayout* bottom_layout = new QHBoxLayout();
170  bottom_layout->setAlignment(Qt::AlignRight);
171  layout_->addLayout(bottom_layout);
172 
173  // Link Filter QLineEdit
174  link_name_filter_ = new QLineEdit(this);
175  link_name_filter_->setPlaceholderText("link name filter");
176  bottom_layout->addWidget(link_name_filter_);
177 
178  // Collision Filter Checkbox
179  collision_checkbox_ = new QCheckBox(this);
180  collision_checkbox_->setText("show enabled pairs");
181  connect(collision_checkbox_, SIGNAL(toggled(bool)), this, SLOT(checkedFilterChanged()));
182  bottom_layout->addWidget(collision_checkbox_);
183 
184  // View Mode Buttons
185  view_mode_buttons_ = new QButtonGroup(this);
186  QRadioButton* radio_btn;
187  radio_btn = new QRadioButton("linear view");
188  bottom_layout->addWidget(radio_btn);
189  view_mode_buttons_->addButton(radio_btn, LinearMode);
190  radio_btn->setChecked(true);
191 
192  radio_btn = new QRadioButton("matrix view");
193  bottom_layout->addWidget(radio_btn);
194  view_mode_buttons_->addButton(radio_btn, MatrixMode);
195  connect(view_mode_buttons_, SIGNAL(buttonClicked(int)), this, SLOT(loadCollisionTable()));
196 
197  // Revert Button
198  btn_revert_ = new QPushButton(this);
199  btn_revert_->setText("&Revert");
200  btn_revert_->setToolTip("Revert current changes to collision matrix");
201  btn_revert_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
202  btn_revert_->setDisabled(true);
203  connect(btn_revert_, SIGNAL(clicked()), this, SLOT(revertChanges()));
204  bottom_layout->addWidget(btn_revert_);
205 
206  setLayout(layout_);
207  setWindowTitle("Default Collision Matrix");
208 
209  collision_table_->installEventFilter(this);
210 }
211 
213 {
214  delete model_;
215 }
216 
217 // ******************************************************************************************
218 // start thread generating the collision table
219 // ******************************************************************************************
221 {
222  // Disable controls on form
223  disableControls(true);
224  btn_revert_->setEnabled(true); // allow to interrupt and revert
225 
226  // create a MonitorThread running generateCollisionTable() in a worker thread and monitoring the progress
228  connect(worker_, SIGNAL(finished()), this, SLOT(finishGeneratingCollisionTable()));
229  worker_->start(); // start after having finished() signal connected
230 }
231 
232 // ******************************************************************************************
233 // cleanup after worker_ thread has finished
234 // ******************************************************************************************
236 {
237  if (worker_->canceled())
238  return;
239 
240  // Load the results into the GUI
242 
243  // Hide the progress bar
244  disableControls(false); // enable everything else
245 
247  worker_->deleteLater();
248  worker_ = NULL;
249 }
250 
251 // ******************************************************************************************
252 // The worker function to compute the collision matrix
253 // ******************************************************************************************
254 void DefaultCollisionsWidget::generateCollisionTable(unsigned int* collision_progress)
255 {
256  unsigned int num_trials = density_slider_->value() * 1000 + 1000; // scale to trials amount
257  double min_frac = (double)fraction_spinbox_->value() / 100.0;
258 
259  const bool verbose = true; // Output benchmarking and statistics
260  const bool include_never_colliding = true;
261 
262  // clear previously loaded collision matrix entries
263  config_data_->getPlanningScene()->getAllowedCollisionMatrixNonConst().clear();
264 
265  // Find the default collision matrix - all links that are allowed to collide
267  config_data_->getPlanningScene(), collision_progress, include_never_colliding, num_trials, min_frac, verbose);
268 
269  // End the progress bar loop
270  *collision_progress = 100;
271 
272  ROS_INFO_STREAM("Thread complete " << link_pairs_.size());
273 }
274 
275 // ******************************************************************************************
276 // Displays data in the link_pairs_ data structure into a QtTableWidget
277 // ******************************************************************************************
279 {
280  CollisionMatrixModel* matrix_model = new CollisionMatrixModel(
281  link_pairs_, config_data_->getPlanningScene()->getRobotModel()->getLinkModelNamesWithCollisionGeometry());
282  QAbstractItemModel* model;
283 
284  if (view_mode_buttons_->checkedId() == MatrixMode)
285  {
286  model = matrix_model;
287  }
288  else
289  {
290  CollisionLinearModel* linear_model = new CollisionLinearModel(matrix_model);
291  SortFilterProxyModel* sorted_model = new SortFilterProxyModel();
292  model = sorted_model;
293  sorted_model->setSourceModel(linear_model);
294  }
295  connect(link_name_filter_, SIGNAL(textChanged(QString)), model, SLOT(setFilterRegExp(QString)));
296  QMetaObject::invokeMethod(model, "setFilterRegExp", Q_ARG(QString, link_name_filter_->text()));
297 
298  collision_table_->setModel(model);
299  // delete old and remember new model
300  delete model_;
301  model_ = model;
302 
303  // delete old and fetch new selection model
304  delete selection_model_;
305  selection_model_ = collision_table_->selectionModel();
306 
307  QHeaderView *horizontal_header, *vertical_header;
308 
309  // activate some model-specific settings
310  if (view_mode_buttons_->checkedId() == MatrixMode)
311  {
312  connect(selection_model_, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this,
313  SLOT(previewSelectedMatrix(QModelIndex)));
314 
315  collision_table_->setSelectionBehavior(QAbstractItemView::SelectItems);
316  collision_table_->setSelectionMode(QAbstractItemView::ExtendedSelection);
317 
318  collision_table_->setHorizontalHeader(horizontal_header = new RotatedHeaderView(Qt::Horizontal, this));
319  collision_table_->setVerticalHeader(vertical_header = new RotatedHeaderView(Qt::Vertical, this));
320  collision_table_->setSortingEnabled(false);
321 
322  collision_checkbox_->hide();
323  horizontal_header->setVisible(true);
324  vertical_header->setVisible(true);
325 
326  horizontal_header->setContextMenuPolicy(Qt::CustomContextMenu);
327  connect(horizontal_header, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(showHeaderContextMenu(QPoint)));
328  vertical_header->setContextMenuPolicy(Qt::CustomContextMenu);
329  connect(vertical_header, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(showHeaderContextMenu(QPoint)));
330  }
331  else
332  {
333  connect(selection_model_, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this,
334  SLOT(previewSelectedLinear(QModelIndex)));
335 
336  collision_table_->setSelectionBehavior(QAbstractItemView::SelectRows);
337  collision_table_->setSelectionMode(QAbstractItemView::ExtendedSelection);
338 
339  collision_table_->setHorizontalHeader(horizontal_header = new QHeaderView(Qt::Horizontal, this));
340  collision_table_->setVerticalHeader(vertical_header = new QHeaderView(Qt::Vertical, this));
341  collision_table_->sortByColumn(0, Qt::AscendingOrder);
342  collision_table_->setSortingEnabled(true);
343 
344  collision_checkbox_->show();
345  horizontal_header->setVisible(true);
346  vertical_header->setVisible(true);
347 
348  vertical_header->setContextMenuPolicy(Qt::CustomContextMenu);
349  connect(vertical_header, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(showHeaderContextMenu(QPoint)));
350 
351 #if (QT_VERSION >= QT_VERSION_CHECK(5, 0, 0))
352  horizontal_header->setSectionsClickable(true);
353  vertical_header->setSectionsClickable(true);
354 #else
355  horizontal_header->setClickable(true);
356  vertical_header->setClickable(true);
357 #endif
358  }
359 
360 // notice changes to the model
361 #if (QT_VERSION >= QT_VERSION_CHECK(5, 0, 0))
362  connect(model_, SIGNAL(dataChanged(QModelIndex, QModelIndex, QVector<int>)), this,
363  SLOT(collisionsChanged(QModelIndex)));
364 #else
365  connect(model_, SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(collisionsChanged(QModelIndex)));
366 #endif
367 }
368 
369 void DefaultCollisionsWidget::collisionsChanged(const QModelIndex& index)
370 {
371  btn_revert_->setEnabled(true); // enable revert button
372 
373  if (!index.isValid())
374  return;
375  // Hm. For some reason, QTableView doesn't change selection if we click a checkbox
376  bool linear_mode = (view_mode_buttons_->checkedId() == LinearMode);
377  const QItemSelection& selection = selection_model_->selection();
378  if ((linear_mode && !selection.contains(index)) || // in linear mode: index not in selection
379  (!linear_mode &&
380  !(selection.contains(index) || // in matrix mode: index or symmetric index not in selection
381  selection.contains(model_->index(index.column(), index.row())))))
382  {
383  QItemSelectionModel::SelectionFlags flags = QItemSelectionModel::Select | QItemSelectionModel::Current;
384  if (linear_mode)
385  flags |= QItemSelectionModel::Rows;
386  selection_model_->setCurrentIndex(index, flags);
387  }
388 }
389 
391 {
392  // This method might be triggered from either of the headers
393  QPoint global;
394  if (sender() == collision_table_->verticalHeader())
395  {
396  clicked_section_ = collision_table_->verticalHeader()->logicalIndexAt(p);
397  clicked_headers_ = Qt::Vertical;
398  global = collision_table_->verticalHeader()->mapToGlobal(p);
399  }
400  else if (sender() == collision_table_->horizontalHeader())
401  {
402  clicked_section_ = collision_table_->horizontalHeader()->logicalIndexAt(p);
403  clicked_headers_ = Qt::Horizontal;
404  global = collision_table_->horizontalHeader()->mapToGlobal(p);
405  }
406  else
407  {
408  clicked_section_ = -1;
409  clicked_headers_ = Qt::Horizontal | Qt::Vertical;
410  }
411 
412  QMenu menu;
413  if (clicked_section_ < 0)
414  menu.addAction(header_actions_.at(0)); // only 'show' action
415  else
416  menu.addActions(header_actions_);
417  menu.exec(global);
418 
419  clicked_headers_ = 0;
420  clicked_section_ = -1;
421 }
422 
424 {
425  QList<int> list;
426  QHeaderView* header = 0;
427  if (clicked_headers_ == Qt::Horizontal)
428  {
429  for (const QModelIndex& index : selection_model_->selectedColumns())
430  list << index.column();
431  header = collision_table_->horizontalHeader();
432  }
433  else if (clicked_headers_ == Qt::Vertical)
434  {
435  for (const QModelIndex& index : selection_model_->selectedRows())
436  list << index.row();
437  header = collision_table_->verticalHeader();
438  }
439 
440  // if somewhere else than the selection was clicked, hide only this row/column
441  if (!list.contains(clicked_section_))
442  {
443  list.clear();
444  list << clicked_section_;
445  }
446 
447  for (auto index : list)
448  header->setSectionHidden(index, true);
449 }
450 
452 {
453  QList<int> list;
454  QHeaderView* header = 0;
455  if (clicked_headers_ == Qt::Horizontal)
456  {
457  header = collision_table_->horizontalHeader();
458  for (const QModelIndex& index : selection_model_->selectedColumns())
459  if (!header->isSectionHidden(index.column()))
460  list << index.column();
461  }
462  else if (clicked_headers_ == Qt::Vertical)
463  {
464  header = collision_table_->verticalHeader();
465  for (const QModelIndex& index : selection_model_->selectedRows())
466  if (!header->isSectionHidden(index.row()))
467  list << index.row();
468  }
469 
470  // if somewhere else than the selection was clicked, hide only this row/column
471  if (!list.contains(clicked_section_))
472  {
473  list.clear();
474  list << clicked_section_;
475  }
476 
477  // first hide all sections
478  for (std::size_t index = 0, end = header->count(); index != end; ++index)
479  header->setSectionHidden(index, true);
480 
481  // and subsequently show selected ones
482  for (auto index : list)
483  header->setSectionHidden(index, false);
484 }
485 
487 {
488  QList<int> list;
489  if (clicked_section_ < 0) // show all
490  {
491  if (clicked_headers_.testFlag(Qt::Horizontal))
492  {
493  // show all columns
494  list.clear();
495  list << 0 << model_->columnCount() - 1;
496  showSections(collision_table_->horizontalHeader(), list);
497  }
498 
499  if (clicked_headers_.testFlag(Qt::Vertical)) // show all rows
500  {
501  list.clear();
502  list << 0 << model_->rowCount() - 1;
503  showSections(collision_table_->verticalHeader(), list);
504  }
505  return;
506  }
507 
508  QHeaderView* header = 0;
509  if (clicked_headers_ == Qt::Horizontal)
510  {
511  for (const QModelIndex& index : selection_model_->selectedColumns())
512  list << index.column();
513  header = collision_table_->horizontalHeader();
514  }
515  else if (clicked_headers_ == Qt::Vertical)
516  {
517  for (const QModelIndex& index : selection_model_->selectedRows())
518  list << index.row();
519  header = collision_table_->verticalHeader();
520  }
521 
522  // if somewhere else than the selection was clicked, hide only this row/column
523  if (!list.contains(clicked_section_))
524  {
525  list.clear();
526  list << clicked_section_;
527  }
528  showSections(header, list);
529 }
530 void DefaultCollisionsWidget::showSections(QHeaderView* header, const QList<int>& logicalIndexes)
531 {
532  if (logicalIndexes.size() < 2)
533  return;
534  int prev = 0;
535  for (int next = 1, end = logicalIndexes.size(); next != end; prev = next, ++next)
536  {
537  for (int index = logicalIndexes[prev], index_end = logicalIndexes[next]; index <= index_end; ++index)
538  header->setSectionHidden(index, false);
539  }
540 }
541 
543 {
546  btn_revert_->setEnabled(false); // no changes to revert
547 }
548 
549 bool DefaultCollisionsWidget::eventFilter(QObject* object, QEvent* event)
550 {
551  if (object != collision_table_)
552  return false; // leave event unhandled
553 
554  if (event->type() == QEvent::Enter)
555  {
556  // grab focus as soon as mouse enters to allow for <space> to work in all cases
557  collision_table_->setFocus();
558  return false;
559  }
560  else if (event->type() == QEvent::KeyPress)
561  {
562  QKeyEvent* keyEvent = static_cast<QKeyEvent*>(event);
563  if (keyEvent->key() != Qt::Key_Space)
564  return false;
565 
566  toggleSelection(selection_model_->selection());
567  return true; // no need for further processing
568  }
569 
570  return false;
571 }
572 
573 void DefaultCollisionsWidget::toggleSelection(QItemSelection selection)
574 {
575  // remove hidden rows / columns from selection
576  int rows = model_->rowCount();
577  int cols = model_->columnCount();
578  for (int r = 0; r != rows; ++r)
579  {
580  if (collision_table_->isRowHidden(r))
581  selection.merge(QItemSelection(model_->index(r, 0), model_->index(r, cols - 1)), QItemSelectionModel::Deselect);
582  }
583  for (int c = 0; c != cols; ++c)
584  {
585  if (collision_table_->isColumnHidden(c))
586  selection.merge(QItemSelection(model_->index(0, c), model_->index(rows - 1, c)), QItemSelectionModel::Deselect);
587  }
588 
589  // set all selected items to inverse value of current item
590  const QModelIndex& cur_idx = selection_model_->currentIndex();
591  if (view_mode_buttons_->checkedId() == MatrixMode)
592  {
593  QModelIndex input_index;
594  if (cur_idx.flags() & Qt::ItemIsUserCheckable)
595  input_index = cur_idx; // if current index is checkable, this serves as input
596  else
597  { // search for first checkable index in selection that can serve as input
598  for (const auto idx : selection.indexes())
599  {
600  if (idx.flags() & Qt::ItemIsUserCheckable)
601  {
602  input_index = idx;
603  break;
604  }
605  }
606  if (!input_index.isValid())
607  return; // no valid selection
608  }
609 
610  bool current = model_->data(input_index, Qt::CheckStateRole) == Qt::Checked;
612  m->setEnabled(selection, !current);
613  }
614  else
615  {
616  bool current = model_->data(model_->index(cur_idx.row(), 2), Qt::CheckStateRole) == Qt::Checked;
618  m->setEnabled(selection, !current);
619  }
620 }
621 
622 // ******************************************************************************************
623 // GUI func for showing sampling density amount
624 // ******************************************************************************************
626 {
627  density_value_label_->setText(QString::number(value * 1000 + 1000)); //.append(" samples") );
628 }
629 
630 // ******************************************************************************************
631 // Helper function to disable parts of GUI during computation
632 // ******************************************************************************************
634 {
635  controls_box_->setDisabled(disable);
636  collision_table_->setDisabled(disable);
637 
638  if (disable)
639  {
640  progress_bar_->show(); // only show when computation begins
641  progress_label_->show();
642  }
643  else
644  {
645  progress_label_->hide();
646  progress_bar_->hide();
647  }
648 
649  QApplication::processEvents(); // allow the progress bar to be shown
650 }
651 
652 // ******************************************************************************************
653 // Changes the table to show or hide collisions that are not disabled (that have collision checking enabled)
654 // ******************************************************************************************
656 {
657  SortFilterProxyModel* m = qobject_cast<SortFilterProxyModel*>(model_);
658  m->setShowAll(collision_checkbox_->checkState() == Qt::Checked);
659 }
660 
661 // Output Link Pairs to SRDF Format and update the collision matrix
662 // ******************************************************************************************
664 {
665  // reset the data in the SRDF Writer class
666  config_data_->srdf_->disabled_collisions_.clear();
667 
668  // Create temp disabled collision
670 
671  // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
672  for (moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end();
673  ++pair_it)
674  {
675  // Only copy those that are actually disabled
676  if (pair_it->second.disable_check)
677  {
678  dc.link1_ = pair_it->first.first;
679  dc.link2_ = pair_it->first.second;
680  dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason);
681  config_data_->srdf_->disabled_collisions_.push_back(dc);
682  }
683  }
684 
685  // Update collision_matrix for robot pose's use
686  config_data_->loadAllowedCollisionMatrix();
687 }
688 
689 // ******************************************************************************************
690 // Load Link Pairs from SRDF Format
691 // ******************************************************************************************
693 {
694  // Clear all the previous data in the compute_default_collisions tool
695  link_pairs_.clear();
696 
697  // Create new instance of planning scene using pointer
698  planning_scene::PlanningScenePtr scene = config_data_->getPlanningScene()->diff();
699 
700  // Populate link_pairs_ list with every possible n choose 2 combination of links
702 
703  // Create temp link pair data struct
705  std::pair<std::string, std::string> link_pair;
706 
707  // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created
708  for (std::vector<srdf::Model::DisabledCollision>::const_iterator collision_it =
709  config_data_->srdf_->disabled_collisions_.begin();
710  collision_it != config_data_->srdf_->disabled_collisions_.end(); ++collision_it)
711  {
712  // Set the link names
713  link_pair.first = collision_it->link1_;
714  link_pair.second = collision_it->link2_;
715  if (link_pair.first >= link_pair.second)
716  std::swap(link_pair.first, link_pair.second);
717 
718  // Set the link meta data
719  link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString(collision_it->reason_);
720  link_pair_data.disable_check = true; // disable checking the collision btw the 2 links
721 
722  // Insert into map
723  link_pairs_[link_pair] = link_pair_data;
724  }
725 }
726 
727 // ******************************************************************************************
728 // Preview whatever element is selected
729 // ******************************************************************************************
731 {
732  // Unhighlight all links
733  Q_EMIT unhighlightAll();
734 
735  if (!index.isValid())
736  return;
737 
738  // normalize index
739  int r = index.row();
740  int c = index.column();
741  if (r == c)
742  return;
743  if (r > c)
744  std::swap(r, c);
745 
746  // Highlight link pair
747  const QString& first_link = model_->headerData(r, Qt::Vertical, Qt::DisplayRole).toString();
748  const QString& second_link = model_->headerData(c, Qt::Horizontal, Qt::DisplayRole).toString();
749  uint check_state = model_->data(index, Qt::CheckStateRole).toUInt();
750 
751  QColor color = (check_state == Qt::Checked) ? QColor(0, 255, 0) : QColor(255, 0, 0);
752  Q_EMIT highlightLink(first_link.toStdString(), color);
753  Q_EMIT highlightLink(second_link.toStdString(), color);
754 }
755 
757 {
758  // Unhighlight all links
759  Q_EMIT unhighlightAll();
760 
761  if (!index.isValid())
762  return;
763 
764  // Highlight link pair
765  const QString& first_link = model_->data(model_->index(index.row(), 0), Qt::DisplayRole).toString();
766  const QString& second_link = model_->data(model_->index(index.row(), 1), Qt::DisplayRole).toString();
767  uint check_state = model_->data(model_->index(index.row(), 2), Qt::CheckStateRole).toUInt();
768 
769  QColor color = (check_state == Qt::Checked) ? QColor(0, 255, 0) : QColor(255, 0, 0);
770  Q_EMIT highlightLink(first_link.toStdString(), color);
771  Q_EMIT highlightLink(second_link.toStdString(), color);
772 }
773 
774 // ******************************************************************************************
775 // Called when setup assistant navigation switches to this screen
776 // ******************************************************************************************
778 {
779  // Convert the SRDF data to LinkPairData format
781 
782  // Load the data to the table
784 
785  // Enable the table
786  disableControls(false);
787  btn_revert_->setEnabled(false); // no changes to revert
788 }
789 
791 {
792  if (worker_)
793  {
794  if (QMessageBox::No == QMessageBox::question(this, "Collision Matrix Generation",
795  "Collision Matrix Generation is still active. Cancel computation?",
796  QMessageBox::Yes | QMessageBox::No, QMessageBox::No))
797  return false;
798  worker_->cancel();
799  worker_->wait();
800  }
801 
802  // Copy changes to srdf_writer object and config_data_->allowed_collision_matrix_
803  linkPairsToSRDF();
804  return true;
805 }
806 
807 moveit_setup_assistant::MonitorThread::MonitorThread(const boost::function<void(unsigned int*)>& f,
808  QProgressBar* progress_bar)
809  : progress_(0), canceled_(false)
810 {
811  // start worker thread
812  worker_ = boost::thread(boost::bind(f, &progress_));
813  // connect progress bar for updates
814  if (progress_bar)
815  connect(this, SIGNAL(progress(int)), progress_bar, SLOT(setValue(int)));
816 }
817 
819 {
820  // loop until worker thread is finished or cancel is requested
821  while (!canceled_ && progress_ < 100)
822  {
823  Q_EMIT progress(progress_);
824  QThread::msleep(100); // sleep 100ms
825  }
826 
827  // cancel worker thread
828  if (canceled_)
829  worker_.interrupt();
830 
831  worker_.join();
832 
833  progress_ = 100;
834  Q_EMIT progress(progress_);
835 }
836 
837 } // namespace
bool verbose
#define NULL
void disableControls(bool disable)
Helper function to disable parts of GUI during computation.
DefaultCollisionsWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
User interface for editing the default collision matrix list in an SRDF.
MonitorThread(const boost::function< void(unsigned int *)> &f, QProgressBar *progress_bar=NULL)
Store details on a pair of links.
int rows
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
void linkPairsFromSRDF()
Load Link Pairs from SRDF Format.
f
moveit_setup_assistant::LinkPairMap link_pairs_
main storage of link pair data
void highlightLink(const std::string &name, const QColor &)
Event for telling rviz to highlight a link of the robot.
void collisionsChanged(const QModelIndex &index)
Collision model changed.
void linkPairsToSRDF()
Output Link Pairs to SRDF Format.
void generateCollisionTable(unsigned int *collision_progress)
The thread that is called to allow the GUI to update. Calls an external function to do calcs...
void computeLinkPairs(const planning_scene::PlanningScene &scene, LinkPairMap &link_pairs)
Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically. n choose 2 pairs.
void unhighlightAll()
Event for telling rviz to unhighlight all links of the robot.
unsigned int index
void loadCollisionTable()
Update view and data model for the link_pairs data structure.
void previewSelectedMatrix(const QModelIndex &index)
Called when current row has changed.
void changeDensityLabel(int value)
GUI func for showing sampling density amount.
const std::string disabledReasonToString(DisabledReason reason)
Converts a reason for disabling a link pair into a string.
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
bool focusLost()
Called when setup assistant navigation switches away from this screen.
void focusGiven()
Called when setup assistant navigation switches to this screen.
r
action
#define ROS_INFO_STREAM(args)
void setShowAll(bool show_all)
QThread to monitor progress of a boost::thread.
void toggleSelection(QItemSelection selection)
Toggle enabled status of selection.
int cols
void checkedFilterChanged()
Change filter settings to show/hide enabled collisions.
bool eventFilter(QObject *object, QEvent *event)
Allow toggling of all checkboxes in selection by filtering <space> keypresses.
void setEnabled(const QItemSelection &selection, bool value)
void startGeneratingCollisionTable()
start generating collision matrix in a worker thread
void revertChanges()
Revert current changes to collision matrix.
LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr &parent_scene, unsigned int *progress, const bool include_never_colliding, const unsigned int trials, const double min_collision_faction, const bool verbose)
Generate an adjacency list of links that are always and never in collision, to speed up collision det...
void finishGeneratingCollisionTable()
finish generating collision matrix after worker thread has finished
void setEnabled(const QItemSelection &selection, bool value)
DisabledReason disabledReasonFromString(const std::string &reason)
Converts a string reason for disabling a link pair into a struct data type.


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