moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
end_effectors_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// SA
40
41// Qt
42#include <QApplication>
43#include <QComboBox>
44#include <QFormLayout>
45#include <QGroupBox>
46#include <QHBoxLayout>
47#include <QLabel>
48#include <QLineEdit>
49#include <QMessageBox>
50#include <QPushButton>
51#include <QScrollArea>
52#include <QStackedWidget>
53#include <QString>
54#include <QTableWidget>
55#include <QVBoxLayout>
56#include <QWidget>
57
58namespace moveit_setup
59{
60namespace srdf_setup
61{
62// ******************************************************************************************
63// Constructor
64// ******************************************************************************************
66{
67 // Basic widget container
68 QVBoxLayout* layout = new QVBoxLayout();
69
70 // Top Header Area ------------------------------------------------
71
72 auto header = new HeaderWidget("Define End Effectors",
73 "Setup your robot's end effectors. These are planning groups "
74 "corresponding to grippers or tools, attached to a parent "
75 "planning group (an arm). The specified parent link is used as the "
76 "reference frame for IK attempts.",
77 this);
78 layout->addWidget(header);
79
80 // Create contents screens ---------------------------------------
81
82 effector_list_widget_ = createContentsWidget();
83 effector_edit_widget_ = createEditWidget();
84
85 // Create stacked layout -----------------------------------------
86 stacked_widget_ = new QStackedWidget(this);
87 stacked_widget_->addWidget(effector_list_widget_); // screen index 0
88 stacked_widget_->addWidget(effector_edit_widget_); // screen index 1
89 layout->addWidget(stacked_widget_);
90
91 // Finish Layout --------------------------------------------------
92 setLayout(layout);
93}
94
95// ******************************************************************************************
96// Create the main content widget
97// ******************************************************************************************
98QWidget* EndEffectorsWidget::createContentsWidget()
99{
100 // Main widget
101 QWidget* content_widget = new QWidget(this);
102
103 // Basic widget container
104 QVBoxLayout* layout = new QVBoxLayout(this);
105
106 // Table ------------ ------------------------------------------------
107
108 data_table_ = new QTableWidget(this);
109 data_table_->setColumnCount(4);
110 data_table_->setSortingEnabled(true);
111 data_table_->setSelectionBehavior(QAbstractItemView::SelectRows);
112 connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int)));
113 connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int)));
114 layout->addWidget(data_table_);
115
116 // Set header labels
117 QStringList header_list;
118 header_list.append("End Effector Name");
119 header_list.append("Group Name");
120 header_list.append("Parent Link");
121 header_list.append("Parent Group");
122 data_table_->setHorizontalHeaderLabels(header_list);
123
124 // Bottom Buttons --------------------------------------------------
125
126 QHBoxLayout* controls_layout = new QHBoxLayout();
127
128 // Spacer
129 controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum));
130
131 // Edit Selected Button
132 btn_edit_ = new QPushButton("&Edit Selected", this);
133 btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
134 btn_edit_->setMaximumWidth(300);
135 btn_edit_->hide(); // show once we know if there are existing poses
136 connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
137 controls_layout->addWidget(btn_edit_);
138 controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
139
140 // Delete Selected Button
141 btn_delete_ = new QPushButton("&Delete Selected", this);
142 connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteSelected()));
143 controls_layout->addWidget(btn_delete_);
144 controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
145
146 // Add end effector Button
147 QPushButton* btn_add = new QPushButton("&Add End Effector", this);
148 btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
149 btn_add->setMaximumWidth(300);
150 connect(btn_add, SIGNAL(clicked()), this, SLOT(showNewScreen()));
151 controls_layout->addWidget(btn_add);
152 controls_layout->setAlignment(btn_add, Qt::AlignRight);
153
154 // Add layout
155 layout->addLayout(controls_layout);
156
157 // Set layout -----------------------------------------------------
158 content_widget->setLayout(layout);
159
160 return content_widget;
161}
162
163// ******************************************************************************************
164// Create the edit widget
165// ******************************************************************************************
166QWidget* EndEffectorsWidget::createEditWidget()
167{
168 // Main widget
169 QWidget* edit_widget = new QWidget(this);
170 // Layout
171 QVBoxLayout* layout = new QVBoxLayout();
172
173 // Simple form -------------------------------------------
174 QFormLayout* form_layout = new QFormLayout();
175 // form_layout->setContentsMargins( 0, 15, 0, 15 );
176 form_layout->setRowWrapPolicy(QFormLayout::WrapAllRows);
177
178 // Name input
179 effector_name_field_ = new QLineEdit(this);
180 form_layout->addRow("End Effector Name:", effector_name_field_);
181
182 // Group input
183 group_name_field_ = new QComboBox(this);
184 group_name_field_->setEditable(false);
185 form_layout->addRow("End Effector Group:", group_name_field_);
186 connect(group_name_field_, SIGNAL(currentIndexChanged(const QString&)), this,
187 SLOT(previewClickedString(const QString&)));
188
189 // Parent Link input
190 parent_name_field_ = new QComboBox(this);
191 parent_name_field_->setEditable(false);
192 form_layout->addRow("Parent Link (usually part of the arm):", parent_name_field_);
193
194 // Parent Group input
195 parent_group_name_field_ = new QComboBox(this);
196 parent_group_name_field_->setEditable(false);
197 form_layout->addRow("Parent Group (optional):", parent_group_name_field_);
198
199 layout->addLayout(form_layout);
200
201 // Bottom Buttons --------------------------------------------------
202
203 QHBoxLayout* controls_layout = new QHBoxLayout();
204 controls_layout->setContentsMargins(0, 25, 0, 15);
205
206 // Spacer
207 controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum));
208
209 // Save
210 btn_save_ = new QPushButton("&Save", this);
211 btn_save_->setMaximumWidth(200);
212 connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing()));
213 controls_layout->addWidget(btn_save_);
214 controls_layout->setAlignment(btn_save_, Qt::AlignRight);
215
216 // Cancel
217 btn_cancel_ = new QPushButton("&Cancel", this);
218 btn_cancel_->setMaximumWidth(200);
219 connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing()));
220 controls_layout->addWidget(btn_cancel_);
221 controls_layout->setAlignment(btn_cancel_, Qt::AlignRight);
222
223 // Add layout
224 layout->addLayout(controls_layout);
225
226 // Set layout -----------------------------------------------------
227 edit_widget->setLayout(layout);
228
229 return edit_widget;
230}
231
232// ******************************************************************************************
233// Show edit screen for creating a new effector
234// ******************************************************************************************
235void EndEffectorsWidget::showNewScreen()
236{
237 // Remember that this is a new effector
238 current_edit_effector_.clear();
239
240 // Clear previous data
241 effector_name_field_->setText("");
242 parent_name_field_->clearEditText();
243 group_name_field_->clearEditText(); // actually this just chooses first option
244 parent_group_name_field_->clearEditText(); // actually this just chooses first option
245
246 // Switch to screen
247 stacked_widget_->setCurrentIndex(1);
248
249 // Announce that this widget is in modal mode
250 Q_EMIT setModalMode(true);
251}
252
253// ******************************************************************************************
254// Edit whatever element is selected
255// ******************************************************************************************
256void EndEffectorsWidget::editDoubleClicked(int /*row*/, int /*column*/)
257{
258 editSelected();
259}
260
261// ******************************************************************************************
262// Preview whatever element is selected
263// ******************************************************************************************
264void EndEffectorsWidget::previewClicked(int /*row*/, int /*column*/)
265{
266 // Get list of all selected items
267 QList<QTableWidgetItem*> selected = data_table_->selectedItems();
268
269 // Check that an element was selected
270 if (selected.empty())
271 return;
272
273 // Find the selected in datastructure
274 srdf::Model::EndEffector* effector = getEndEffector(selected[0]->text().toStdString());
275
276 // Unhighlight all links
278
279 // Highlight group
280 rviz_panel_->highlightGroup(effector->component_group_);
281}
282
283// ******************************************************************************************
284// Preview the planning group that is selected
285// ******************************************************************************************
286void EndEffectorsWidget::previewClickedString(const QString& name)
287{
288 // Don't highlight if we are on the overview end effectors screen. we are just populating drop down box
289 if (stacked_widget_->currentIndex() == 0)
290 return;
291
292 // Unhighlight all links
294
295 // Highlight group
296 rviz_panel_->highlightGroup(name.toStdString());
297}
298
299// ******************************************************************************************
300// Edit whatever element is selected
301// ******************************************************************************************
302void EndEffectorsWidget::editSelected()
303{
304 // Get list of all selected items
305 QList<QTableWidgetItem*> selected = data_table_->selectedItems();
306
307 // Check that an element was selected
308 if (selected.empty())
309 return;
310
311 // Get selected name and edit it
312 edit(selected[0]->text().toStdString());
313}
314
315// ******************************************************************************************
316// Edit effector
317// ******************************************************************************************
318void EndEffectorsWidget::edit(const std::string& name)
319{
320 // Remember what we are editing
321 current_edit_effector_ = name;
322
323 // Find the selected in datastruture
324 srdf::Model::EndEffector* effector = getEndEffector(name);
325
326 // Set effector name
327 effector_name_field_->setText(effector->name_.c_str());
328
329 // Set effector parent link
330 int index = parent_name_field_->findText(effector->parent_link_.c_str());
331 if (index == -1)
332 {
333 QMessageBox::critical(this, "Error Loading", "Unable to find parent link in drop down box");
334 return;
335 }
336 parent_name_field_->setCurrentIndex(index);
337
338 // Set group:
339 index = group_name_field_->findText(effector->component_group_.c_str());
340 if (index == -1)
341 {
342 QMessageBox::critical(this, "Error Loading", "Unable to find group name in drop down box");
343 return;
344 }
345 group_name_field_->setCurrentIndex(index);
346
347 // Set parent group:
348 index = parent_group_name_field_->findText(effector->parent_group_.c_str());
349 if (index == -1)
350 {
351 QMessageBox::critical(this, "Error Loading", "Unable to find parent group name in drop down box");
352 return;
353 }
354 parent_group_name_field_->setCurrentIndex(index);
355
356 // Switch to screen
357 stacked_widget_->setCurrentIndex(1);
358
359 // Announce that this widget is in modal mode
360 Q_EMIT setModalMode(true);
361}
362
363// ******************************************************************************************
364// Populate the combo dropdown box with avail group names
365// ******************************************************************************************
366void EndEffectorsWidget::loadGroupsComboBox()
367{
368 // Remove all old groups
369 group_name_field_->clear();
371 parent_group_name_field_->addItem(""); // optional setting
372
373 // Add all group names to combo box
374 for (const std::string& group_name : setup_step_.getGroupNames())
375 {
376 group_name_field_->addItem(group_name.c_str());
377 parent_group_name_field_->addItem(group_name.c_str());
378 }
379}
380
381// ******************************************************************************************
382// Populate the combo dropdown box with avail parent links
383// ******************************************************************************************
384void EndEffectorsWidget::loadParentComboBox()
385{
386 // Remove all old groups
387 parent_name_field_->clear();
388
389 // Get all links in robot model
390 // Add all links to combo box
391 for (const std::string& link_name : setup_step_.getLinkNames())
392 {
393 parent_name_field_->addItem(link_name.c_str());
394 }
395}
396
397// ******************************************************************************************
398// Find the associated data by name
399// ******************************************************************************************
400srdf::Model::EndEffector* EndEffectorsWidget::getEndEffector(const std::string& name)
401{
402 srdf::Model::EndEffector* searched_group = setup_step_.find(name);
403
404 // Check if effector was found
405 if (searched_group == nullptr) // not found
406 {
407 QMessageBox::critical(this, "Error Saving", "An internal error has occurred while saving. Quitting.");
408 QApplication::quit();
409 }
410
411 return searched_group;
412}
413
414// ******************************************************************************************
415// Delete currently editing item
416// ******************************************************************************************
417void EndEffectorsWidget::deleteSelected()
418{
419 // Get list of all selected items
420 QList<QTableWidgetItem*> selected = data_table_->selectedItems();
421
422 // Check that an element was selected
423 if (selected.empty())
424 return;
425
426 // Get selected name and edit it
427 current_edit_effector_ = selected[0]->text().toStdString();
428
429 // Confirm user wants to delete group
430 if (QMessageBox::question(this, "Confirm End Effector Deletion",
431 QString("Are you sure you want to delete the end effector '")
432 .append(current_edit_effector_.c_str())
433 .append("'?"),
434 QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
435 {
436 return;
437 }
438
439 setup_step_.remove(current_edit_effector_);
440
441 // Reload main screen table
442 loadDataTable();
443}
444
445// ******************************************************************************************
446// Save editing changes
447// ******************************************************************************************
448void EndEffectorsWidget::doneEditing()
449{
450 // Get a reference to the supplied strings
451 const std::string effector_name = effector_name_field_->text().toStdString();
452
453 // Check that name field is not empty
454 if (effector_name.empty())
455 {
456 QMessageBox::warning(this, "Error Saving", "A name must be specified for the end effector!");
457 return;
458 }
459
460 // Check that a group was selected
461 if (group_name_field_->currentText().isEmpty())
462 {
463 QMessageBox::warning(this, "Error Saving", "A group that contains the links of the end-effector must be chosen!");
464 return;
465 }
466
467 // Check that a parent link was selected
468 if (parent_name_field_->currentText().isEmpty())
469 {
470 QMessageBox::warning(this, "Error Saving", "A parent link must be chosen!");
471 return;
472 }
473
474 if (!parent_group_name_field_->currentText().isEmpty())
475 {
476 if (!setup_step_.isLinkInGroup(parent_name_field_->currentText().toStdString(),
477 parent_group_name_field_->currentText().toStdString()))
478 {
479 QMessageBox::warning(this, "Error Saving",
480 QString::fromStdString("The specified parent group '" +
481 parent_group_name_field_->currentText().toStdString() +
482 "' must contain the specified parent link '" +
483 parent_name_field_->currentText().toStdString() + "'."));
484 return;
485 }
486 }
487
488 // Save the new effector name or create the new effector ----------------------------
489
490 try
491 {
492 srdf::Model::EndEffector* searched_data = setup_step_.get(effector_name, current_edit_effector_);
493 // Copy name data ----------------------------------------------------
494 setup_step_.setProperties(searched_data, parent_name_field_->currentText().toStdString(),
495 group_name_field_->currentText().toStdString(),
496 parent_group_name_field_->currentText().toStdString());
497 }
498 catch (const std::runtime_error& e)
499 {
500 QMessageBox::warning(this, "Error Saving", e.what());
501 return;
502 }
503
504 // Finish up ------------------------------------------------------
505
506 // Reload main screen table
507 loadDataTable();
508
509 // Switch to screen
510 stacked_widget_->setCurrentIndex(0);
511
512 // Announce that this widget is not in modal mode
513 Q_EMIT setModalMode(false);
514}
515
516// ******************************************************************************************
517// Cancel changes
518// ******************************************************************************************
519void EndEffectorsWidget::cancelEditing()
520{
521 // Switch to screen
522 stacked_widget_->setCurrentIndex(0);
523
524 // Re-highlight the currently selected end effector group
525 previewClicked(0, 0); // the number parameters are actually meaningless
526
527 // Announce that this widget is not in modal mode
528 Q_EMIT setModalMode(false);
529}
530
531// ******************************************************************************************
532// Load the end effectors into the table
533// ******************************************************************************************
534void EndEffectorsWidget::loadDataTable()
535{
536 // Disable Table
537 data_table_->setUpdatesEnabled(false); // prevent table from updating until we are completely done
538 data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called
539 data_table_->clearContents();
540
541 const auto& end_effectors = setup_step_.getEndEffectors();
542
543 // Set size of datatable
544 data_table_->setRowCount(end_effectors.size());
545
546 // Loop through every end effector
547 int row = 0;
548 for (const auto& eef : end_effectors)
549 {
550 // Create row elements
551 QTableWidgetItem* data_name = new QTableWidgetItem(eef.name_.c_str());
552 data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
553 QTableWidgetItem* group_name = new QTableWidgetItem(eef.component_group_.c_str());
554 group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
555 QTableWidgetItem* parent_name = new QTableWidgetItem(eef.parent_link_.c_str());
556 group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
557 QTableWidgetItem* parent_group_name = new QTableWidgetItem(eef.parent_group_.c_str());
558 parent_group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
559
560 // Add to table
561 data_table_->setItem(row, 0, data_name);
562 data_table_->setItem(row, 1, group_name);
563 data_table_->setItem(row, 2, parent_name);
564 data_table_->setItem(row, 3, parent_group_name);
565
566 // Increment counter
567 ++row;
568 }
569
570 // Re-enable
571 data_table_->setUpdatesEnabled(true); // prevent table from updating until we are completely done
572 data_table_->setDisabled(false); // make sure we disable it so that the cellChanged event is not called
573
574 // Resize header
575 data_table_->resizeColumnToContents(0);
576 data_table_->resizeColumnToContents(1);
577 data_table_->resizeColumnToContents(2);
578 data_table_->resizeColumnToContents(3);
579
580 // Show edit button if applicable
581 if (!end_effectors.empty())
582 btn_edit_->show();
583}
584
585// ******************************************************************************************
586// Called when setup assistant navigation switches to this screen
587// ******************************************************************************************
589{
590 // Show the current effectors screen
591 stacked_widget_->setCurrentIndex(0);
592
593 // Load the data to the tree
594 loadDataTable();
595
596 // Load the avail groups to the combo box
597 loadGroupsComboBox();
598 loadParentComboBox();
599}
600
601} // namespace srdf_setup
602} // namespace moveit_setup
603
604#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
void highlightGroup(const std::string &group_name)
The GUI code for one SetupStep.
void setModalMode(bool isModal)
Event for when the current screen is in modal view. Disables the left navigation.
void focusGiven() override
Received when this widget is chosen from the navigation menu.
std::vector< srdf::Model::EndEffector > & getEndEffectors()
bool isLinkInGroup(const std::string &link, const std::string &group) const
void setProperties(srdf::Model::EndEffector *eef, const std::string &parent_link, const std::string &component_group, const std::string &parent_group)
T * get(const std::string &name, const std::string &old_name="")
Get a pointer to an item with the given name, creating if necessary. If old_name is provided (and is ...
T * find(const std::string &name)
Return a pointer to an item with the given name if it exists, otherwise null.
Definition srdf_step.hpp:98
bool remove(const std::string &name)
Delete an item with the given name from the list.
std::string append(const std::string &left, const std::string &right)
name
Definition setup.py:7