virtual_joints_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
38 #include "virtual_joints_widget.h"
39 // Qt
40 #include <QFormLayout>
41 #include <QMessageBox>
42 #include <QApplication>
43 
44 namespace moveit_setup_assistant
45 {
46 // ******************************************************************************************
47 // Constructor
48 // ******************************************************************************************
49 VirtualJointsWidget::VirtualJointsWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
50  : SetupScreenWidget(parent), config_data_(config_data)
51 {
52  // Basic widget container
53  QVBoxLayout* layout = new QVBoxLayout();
54 
55  // Top Header Area ------------------------------------------------
56 
57  HeaderWidget* header =
58  new HeaderWidget("Define Virtual Joints", "Create a virtual joint between a robot link and an external frame of "
59  "reference (considered fixed with respect to the robot).",
60  this);
61  layout->addWidget(header);
62 
63  // Create contents screens ---------------------------------------
64 
67 
68  // Create stacked layout -----------------------------------------
69  stacked_layout_ = new QStackedLayout(this);
70  stacked_layout_->addWidget(vjoint_list_widget_); // screen index 0
71  stacked_layout_->addWidget(vjoint_edit_widget_); // screen index 1
72 
73  // Create Widget wrapper for layout
74  QWidget* stacked_layout_widget = new QWidget(this);
75  stacked_layout_widget->setLayout(stacked_layout_);
76 
77  layout->addWidget(stacked_layout_widget);
78 
79  // Finish Layout --------------------------------------------------
80  this->setLayout(layout);
81 }
82 
83 // ******************************************************************************************
84 // Create the main content widget
85 // ******************************************************************************************
87 {
88  // Main widget
89  QWidget* content_widget = new QWidget(this);
90 
91  // Basic widget container
92  QVBoxLayout* layout = new QVBoxLayout(this);
93 
94  // Table ------------ ------------------------------------------------
95 
96  data_table_ = new QTableWidget(this);
97  data_table_->setColumnCount(4);
98  data_table_->setSortingEnabled(true);
99  data_table_->setSelectionBehavior(QAbstractItemView::SelectRows);
100  connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int)));
101  connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int)));
102  layout->addWidget(data_table_);
103 
104  // Set header labels
105  QStringList header_list;
106  header_list.append("Virtual Joint Name");
107  header_list.append("Child Link");
108  header_list.append("Parent Frame");
109  header_list.append("Type");
110  data_table_->setHorizontalHeaderLabels(header_list);
111 
112  // Bottom Buttons --------------------------------------------------
113 
114  QHBoxLayout* controls_layout = new QHBoxLayout();
115 
116  // Spacer
117  QWidget* spacer = new QWidget(this);
118  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
119  controls_layout->addWidget(spacer);
120 
121  // Edit Selected Button
122  btn_edit_ = new QPushButton("&Edit Selected", this);
123  btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
124  btn_edit_->setMaximumWidth(300);
125  btn_edit_->hide(); // show once we know if there are existing poses
126  connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
127  controls_layout->addWidget(btn_edit_);
128  controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
129 
130  // Delete Selected Button
131  btn_delete_ = new QPushButton("&Delete Selected", this);
132  connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteSelected()));
133  controls_layout->addWidget(btn_delete_);
134  controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
135 
136  // Add VJoint Button
137  QPushButton* btn_add = new QPushButton("&Add Virtual Joint", this);
138  btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
139  btn_add->setMaximumWidth(300);
140  connect(btn_add, SIGNAL(clicked()), this, SLOT(showNewScreen()));
141  controls_layout->addWidget(btn_add);
142  controls_layout->setAlignment(btn_add, Qt::AlignRight);
143 
144  // Add layout
145  layout->addLayout(controls_layout);
146 
147  // Set layout -----------------------------------------------------
148  content_widget->setLayout(layout);
149 
150  return content_widget;
151 }
152 
153 // ******************************************************************************************
154 // Create the edit widget
155 // ******************************************************************************************
157 {
158  // Main widget
159  QWidget* edit_widget = new QWidget(this);
160  // Layout
161  QVBoxLayout* layout = new QVBoxLayout();
162 
163  // Simple form -------------------------------------------
164  QFormLayout* form_layout = new QFormLayout();
165  // form_layout->setContentsMargins( 0, 15, 0, 15 );
166  form_layout->setRowWrapPolicy(QFormLayout::WrapAllRows);
167 
168  // Name input
169  vjoint_name_field_ = new QLineEdit(this);
170  form_layout->addRow("Virtual Joint Name:", vjoint_name_field_);
171 
172  // Child Link input
173  child_link_field_ = new QComboBox(this);
174  child_link_field_->setEditable(false);
175  form_layout->addRow("Child Link:", child_link_field_);
176 
177  // Parent frame name input
178  parent_name_field_ = new QLineEdit(this);
179  form_layout->addRow("Parent Frame Name:", parent_name_field_);
180 
181  // Type input
182  joint_type_field_ = new QComboBox(this);
183  joint_type_field_->setEditable(false);
184  loadJointTypesComboBox(); // only do this once
185  // connect( joint_type_field_, SIGNAL( currentIndexChanged( const QString & ) ),
186  // this, SLOT( loadJoinSliders( const QString & ) ) );
187  form_layout->addRow("Joint Type:", joint_type_field_);
188 
189  layout->addLayout(form_layout);
190 
191  // Bottom Buttons --------------------------------------------------
192 
193  QHBoxLayout* controls_layout = new QHBoxLayout();
194  controls_layout->setContentsMargins(0, 25, 0, 15);
195 
196  // Spacer
197  QWidget* spacer = new QWidget(this);
198  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
199  controls_layout->addWidget(spacer);
200 
201  // Save
202  QPushButton* btn_save_ = new QPushButton("&Save", this);
203  btn_save_->setMaximumWidth(200);
204  connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing()));
205  controls_layout->addWidget(btn_save_);
206  controls_layout->setAlignment(btn_save_, Qt::AlignRight);
207 
208  // Cancel
209  QPushButton* btn_cancel_ = new QPushButton("&Cancel", this);
210  btn_cancel_->setMaximumWidth(200);
211  connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing()));
212  controls_layout->addWidget(btn_cancel_);
213  controls_layout->setAlignment(btn_cancel_, Qt::AlignRight);
214 
215  // Add layout
216  layout->addLayout(controls_layout);
217 
218  // Set layout -----------------------------------------------------
219  edit_widget->setLayout(layout);
220 
221  return edit_widget;
222 }
223 
224 // ******************************************************************************************
225 // Show edit screen for creating a new vjoint
226 // ******************************************************************************************
228 {
229  // Remember that this is a new vjoint
230  current_edit_vjoint_.clear();
231 
232  // Clear previous data
233  vjoint_name_field_->setText("");
234  parent_name_field_->setText("");
235  child_link_field_->clearEditText();
236  joint_type_field_->clearEditText(); // actually this just chooses first option
237 
238  // Switch to screen
239  stacked_layout_->setCurrentIndex(1);
240 
241  // Announce that this widget is in modal mode
242  Q_EMIT isModal(true);
243 }
244 
245 // ******************************************************************************************
246 // Edit whatever element is selected
247 // ******************************************************************************************
248 void VirtualJointsWidget::editDoubleClicked(int row, int column)
249 {
250  editSelected();
251 }
252 
253 // ******************************************************************************************
254 // Preview whatever element is selected
255 // ******************************************************************************************
256 void VirtualJointsWidget::previewClicked(int row, int column)
257 {
258  // TODO: highlight the virtual joint?
259 
260  /* // Get list of all selected items
261  QList<QTableWidgetItem*> selected = data_table_->selectedItems();
262 
263  // Check that an element was selected
264  if( !selected.size() )
265  return;
266 
267  // Find the selected in datastructure
268  srdf::Model::GroupState *vjoint = findVjointByName( selected[0]->text().toStdString() );
269 
270  // Set vjoint joint values by adding them to the local joint state map
271  for( std::map<std::string, std::vector<double> >::const_iterator value_it = vjoint->joint_values_.begin();
272  value_it != vjoint->joint_values_.end(); ++value_it )
273  {
274  // Only copy the first joint value
275  joint_state_map_[ value_it->first ] = value_it->second[0];
276  }
277 
278  // Update the joints
279  publishJoints();
280  */
281 }
282 
283 // ******************************************************************************************
284 // Edit whatever element is selected
285 // ******************************************************************************************
287 {
288  // Get list of all selected items
289  QList<QTableWidgetItem*> selected = data_table_->selectedItems();
290 
291  // Check that an element was selected
292  if (!selected.size())
293  return;
294 
295  // Get selected name and edit it
296  edit(selected[0]->text().toStdString());
297 }
298 
299 // ******************************************************************************************
300 // Edit vjoint
301 // ******************************************************************************************
302 void VirtualJointsWidget::edit(const std::string& name)
303 {
304  // Remember what we are editing
305  current_edit_vjoint_ = name;
306 
307  // Find the selected in datastruture
309 
310  // Set vjoint name
311  vjoint_name_field_->setText(vjoint->name_.c_str());
312  parent_name_field_->setText(vjoint->parent_frame_.c_str());
313 
314  // Set vjoint child link
315  int index = child_link_field_->findText(vjoint->child_link_.c_str());
316  if (index == -1)
317  {
318  QMessageBox::critical(this, "Error Loading", "Unable to find child link in drop down box");
319  return;
320  }
321  child_link_field_->setCurrentIndex(index);
322 
323  // Set joint type
324  index = joint_type_field_->findText(vjoint->type_.c_str());
325  if (index == -1)
326  {
327  QMessageBox::critical(this, "Error Loading", "Unable to find joint type in drop down box");
328  return;
329  }
330  joint_type_field_->setCurrentIndex(index);
331 
332  // Switch to screen
333  stacked_layout_->setCurrentIndex(1);
334 
335  // Announce that this widget is in modal mode
336  Q_EMIT isModal(true);
337 }
338 
339 // ******************************************************************************************
340 // Populate the combo dropdown box with joint types
341 // ******************************************************************************************
343 {
344  // Remove all old items
345  joint_type_field_->clear();
346 
347  // joint types (hard coded)
348  joint_type_field_->addItem("fixed");
349  joint_type_field_->addItem("floating");
350  joint_type_field_->addItem("planar");
351 }
352 
353 // ******************************************************************************************
354 // Populate the combo dropdown box with avail child links
355 // ******************************************************************************************
357 {
358  // Remove all old links
359  child_link_field_->clear();
360 
361  // Get all links in robot model
362  std::vector<const robot_model::LinkModel*> link_models = config_data_->getRobotModel()->getLinkModels();
363 
364  // Add all links to combo box
365  for (std::vector<const robot_model::LinkModel*>::const_iterator link_it = link_models.begin();
366  link_it < link_models.end(); ++link_it)
367  {
368  child_link_field_->addItem((*link_it)->getName().c_str());
369  }
370 }
371 
372 // ******************************************************************************************
373 // Find the associated data by name
374 // ******************************************************************************************
376 {
377  // Find the group state we are editing based on the vjoint name
378  srdf::Model::VirtualJoint* searched_group = NULL; // used for holding our search results
379 
380  for (std::vector<srdf::Model::VirtualJoint>::iterator vjoint_it = config_data_->srdf_->virtual_joints_.begin();
381  vjoint_it != config_data_->srdf_->virtual_joints_.end(); ++vjoint_it)
382  {
383  if (vjoint_it->name_ == name) // string match
384  {
385  searched_group = &(*vjoint_it); // convert to pointer from iterator
386  break; // we are done searching
387  }
388  }
389 
390  // Check if vjoint was found
391  if (searched_group == NULL) // not found
392  {
393  QMessageBox::critical(this, "Error Saving", "An internal error has occured while saving. Quitting.");
394  QApplication::quit();
395  }
396 
397  return searched_group;
398 }
399 
400 // ******************************************************************************************
401 // Delete currently editing item
402 // ******************************************************************************************
404 {
405  // Get list of all selected items
406  QList<QTableWidgetItem*> selected = data_table_->selectedItems();
407 
408  // Check that an element was selected
409  if (!selected.size())
410  return;
411 
412  // Get selected name and edit it
413  current_edit_vjoint_ = selected[0]->text().toStdString();
414 
415  // Confirm user wants to delete group
416  if (QMessageBox::question(this, "Confirm Virtual Joint Deletion",
417  QString("Are you sure you want to delete the virtual joint '")
418  .append(current_edit_vjoint_.c_str())
419  .append("'?"),
420  QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
421  {
422  return;
423  }
424 
425  // Delete vjoint from vector
426  for (std::vector<srdf::Model::VirtualJoint>::iterator vjoint_it = config_data_->srdf_->virtual_joints_.begin();
427  vjoint_it != config_data_->srdf_->virtual_joints_.end(); ++vjoint_it)
428  {
429  // check if this is the group we want to delete
430  if (vjoint_it->name_ == current_edit_vjoint_) // string match
431  {
432  config_data_->srdf_->virtual_joints_.erase(vjoint_it);
433  break;
434  }
435  }
436 
437  // Reload main screen table
438  loadDataTable();
440 }
441 
442 // ******************************************************************************************
443 // Save editing changes
444 // ******************************************************************************************
446 {
447  // Get a reference to the supplied strings
448  const std::string vjoint_name = vjoint_name_field_->text().trimmed().toStdString();
449  const std::string parent_name = parent_name_field_->text().trimmed().toStdString();
450 
451  // Used for editing existing groups
452  srdf::Model::VirtualJoint* searched_data = NULL;
453 
454  // Check that name field is not empty
455  if (vjoint_name.empty())
456  {
457  QMessageBox::warning(this, "Error Saving", "A name must be given for the virtual joint!");
458  return;
459  }
460 
461  // Check that parent frame name field is not empty
462  if (parent_name.empty())
463  {
464  QMessageBox::warning(this, "Error Saving", "A name must be given for the parent frame");
465  return;
466  }
467 
468  // Check if this is an existing vjoint
469  if (!current_edit_vjoint_.empty())
470  {
471  // Find the group we are editing based on the goup name string
472  searched_data = findVJointByName(current_edit_vjoint_);
473  }
474 
475  // Check that the vjoint name is unique
476  for (std::vector<srdf::Model::VirtualJoint>::const_iterator data_it = config_data_->srdf_->virtual_joints_.begin();
477  data_it != config_data_->srdf_->virtual_joints_.end(); ++data_it)
478  {
479  if (data_it->name_.compare(vjoint_name) == 0) // the names are the same
480  {
481  // is this our existing vjoint? check if vjoint pointers are same
482  if (&(*data_it) != searched_data)
483  {
484  QMessageBox::warning(this, "Error Saving", "A virtual joint already exists with that name!");
485  return;
486  }
487  }
488  }
489 
490  // Check that a joint type was selected
491  if (joint_type_field_->currentText().isEmpty())
492  {
493  QMessageBox::warning(this, "Error Saving", "A joint type must be chosen!");
494  return;
495  }
496 
497  // Check that a child link was selected
498  if (child_link_field_->currentText().isEmpty())
499  {
500  QMessageBox::warning(this, "Error Saving", "A child link must be chosen!");
501  return;
502  }
503 
505 
506  // Save the new vjoint name or create the new vjoint ----------------------------
507  bool isNew = false;
508 
509  if (searched_data == NULL) // create new
510  {
511  isNew = true;
512 
513  searched_data = new srdf::Model::VirtualJoint();
514  }
515 
516  // Copy name data ----------------------------------------------------
517  searched_data->name_ = vjoint_name;
518  searched_data->parent_frame_ = parent_name;
519  searched_data->child_link_ = child_link_field_->currentText().toStdString();
520  searched_data->type_ = joint_type_field_->currentText().toStdString();
521 
522  bool emit_frame_notice = false;
523 
524  // Insert new vjoints into group state vector --------------------------
525  if (isNew)
526  {
527  if (searched_data->child_link_ == config_data_->getRobotModel()->getRootLinkName())
528  emit_frame_notice = true;
529  config_data_->srdf_->virtual_joints_.push_back(*searched_data);
530  config_data_->updateRobotModel();
531  }
532 
533  // Finish up ------------------------------------------------------
534 
535  // Reload main screen table
536  loadDataTable();
537 
538  // Switch to screen
539  stacked_layout_->setCurrentIndex(0);
540 
541  // Announce that this widget is not in modal mode
542  Q_EMIT isModal(false);
543 
544  // if the root frame changed for the robot, emit the signal
545  if (emit_frame_notice)
546  {
547  Q_EMIT referenceFrameChanged();
548  }
549 }
550 
551 // ******************************************************************************************
552 // Cancel changes
553 // ******************************************************************************************
555 {
556  // Switch to screen
557  stacked_layout_->setCurrentIndex(0);
558 
559  // Announce that this widget is not in modal mode
560  Q_EMIT isModal(false);
561 }
562 
563 // ******************************************************************************************
564 // Load the virtual joints into the table
565 // ******************************************************************************************
567 {
568  // Disable Table
569  data_table_->setUpdatesEnabled(false); // prevent table from updating until we are completely done
570  data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called
571  data_table_->clearContents();
572 
573  // Set size of datatable
574  data_table_->setRowCount(config_data_->srdf_->virtual_joints_.size());
575 
576  // Loop through every virtual joint
577  int row = 0;
578  for (std::vector<srdf::Model::VirtualJoint>::const_iterator data_it = config_data_->srdf_->virtual_joints_.begin();
579  data_it != config_data_->srdf_->virtual_joints_.end(); ++data_it)
580  {
581  // Create row elements
582  QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str());
583  data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
584  QTableWidgetItem* child_link_name = new QTableWidgetItem(data_it->child_link_.c_str());
585  child_link_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
586  QTableWidgetItem* parent_frame_name = new QTableWidgetItem(data_it->parent_frame_.c_str());
587  parent_frame_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
588  QTableWidgetItem* type_name = new QTableWidgetItem(data_it->type_.c_str());
589  type_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
590 
591  // Add to table
592  data_table_->setItem(row, 0, data_name);
593  data_table_->setItem(row, 1, child_link_name);
594  data_table_->setItem(row, 2, parent_frame_name);
595  data_table_->setItem(row, 3, type_name);
596 
597  // Increment counter
598  ++row;
599  }
600 
601  // Reenable
602  data_table_->setUpdatesEnabled(true); // prevent table from updating until we are completely done
603  data_table_->setDisabled(false); // make sure we disable it so that the cellChanged event is not called
604 
605  // Resize header
606  data_table_->resizeColumnToContents(0);
607  data_table_->resizeColumnToContents(1);
608  data_table_->resizeColumnToContents(2);
609  data_table_->resizeColumnToContents(3);
610 
611  // Show edit button if applicable
612  if (config_data_->srdf_->virtual_joints_.size())
613  btn_edit_->show();
614 }
615 
616 // ******************************************************************************************
617 // Called when setup assistant navigation switches to this screen
618 // ******************************************************************************************
620 {
621  // Show the current vjoints screen
622  stacked_layout_->setCurrentIndex(0);
623 
624  // Load the data to the tree
625  loadDataTable();
626 
627  // Load the avail groups to the combo box
629 }
630 
631 } // namespace
#define NULL
VirtualJointsWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
void isModal(bool isModal)
Event for when the current screen is in modal view. Essential disabled the left navigation.
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
void referenceFrameChanged()
Event sent when this widget updated the root joint, which changes the frame of reference for the mode...
text
void deleteSelected()
Delete currently editing ite.
srdf::Model::VirtualJoint * findVJointByName(const std::string &name)
unsigned int index
void previewClicked(int row, int column)
Preview whatever element is selected.
virtual void focusGiven()
Received when this widget is chosen from the navigation menu.
void editDoubleClicked(int row, int column)
Edit the double clicked element.
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void editSelected()
Edit whatever element is selected.
std::string current_edit_vjoint_
Orignal name of vjoint currently being edited. This is used to find the element in the vector...


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