robot_poses_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 "robot_poses_widget.h"
39 #include <moveit_msgs/JointLimits.h>
40 // Qt
41 #include <QFormLayout>
42 #include <QMessageBox>
43 #include <QDoubleValidator>
44 #include <QApplication>
45 
47 #include <moveit_msgs/DisplayRobotState.h>
48 
49 namespace moveit_setup_assistant
50 {
51 // ******************************************************************************************
52 // Outer User Interface for MoveIt! Configuration Assistant
53 // ******************************************************************************************
54 RobotPosesWidget::RobotPosesWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
55  : SetupScreenWidget(parent), config_data_(config_data)
56 {
57  // Set pointer to null so later we can tell if we need to delete it
59 
60  // Basic widget container
61  QVBoxLayout* layout = new QVBoxLayout();
62 
63  // Top Header Area ------------------------------------------------
64 
65  HeaderWidget* header = new HeaderWidget(
66  "Define Robot Poses", "Create poses for the robot. Poses are defined as sets of joint values for "
67  "particular planning groups. This is useful for things like <i>home position</i>.",
68  this);
69  layout->addWidget(header);
70 
71  // Create contents screens ---------------------------------------
74 
75  // Create stacked layout -----------------------------------------
76  stacked_layout_ = new QStackedLayout(this);
77  stacked_layout_->addWidget(pose_list_widget_); // screen index 0
78  stacked_layout_->addWidget(pose_edit_widget_); // screen index 1
79 
80  // Create Widget wrapper for layout
81  QWidget* stacked_layout_widget = new QWidget(this);
82  stacked_layout_widget->setLayout(stacked_layout_);
83 
84  layout->addWidget(stacked_layout_widget);
85 
86  // Finish Layout --------------------------------------------------
87  this->setLayout(layout);
88 
89  // Create joint publisher -----------------------------------------
90  ros::NodeHandle nh;
91 
92  // Create scene publisher for later use
93  pub_robot_state_ = nh.advertise<moveit_msgs::DisplayRobotState>(MOVEIT_ROBOT_STATE, 1);
94 
95  // Set the planning scene
96  config_data_->getPlanningScene()->setName("MoveIt! Planning Scene");
97 
98  // Collision Detection initializtion -------------------------------
99 
100  // Setup the request
101  request.contacts = true;
102  request.max_contacts = 1;
104  request.verbose = false;
105 }
106 
107 // ******************************************************************************************
108 // Create the main content widget
109 // ******************************************************************************************
111 {
112  // Main widget
113  QWidget* content_widget = new QWidget(this);
114 
115  // Basic widget container
116  QVBoxLayout* layout = new QVBoxLayout(this);
117 
118  // Table ------------ ------------------------------------------------
119 
120  data_table_ = new QTableWidget(this);
121  data_table_->setColumnCount(2);
122  data_table_->setSortingEnabled(true);
123  data_table_->setSelectionBehavior(QAbstractItemView::SelectRows);
124  connect(data_table_, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(editDoubleClicked(int, int)));
125  connect(data_table_, SIGNAL(cellClicked(int, int)), this, SLOT(previewClicked(int, int)));
126  layout->addWidget(data_table_);
127 
128  // Set header labels
129  QStringList header_list;
130  header_list.append("Pose Name");
131  header_list.append("Group Name");
132  data_table_->setHorizontalHeaderLabels(header_list);
133 
134  // Bottom Buttons --------------------------------------------------
135 
136  QHBoxLayout* controls_layout = new QHBoxLayout();
137 
138  // Set Default Button
139  QPushButton* btn_default = new QPushButton("&Show Default Pose", this);
140  btn_default->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
141  btn_default->setMaximumWidth(300);
142  connect(btn_default, SIGNAL(clicked()), this, SLOT(showDefaultPose()));
143  controls_layout->addWidget(btn_default);
144  controls_layout->setAlignment(btn_default, Qt::AlignLeft);
145 
146  // Set play button
147  QPushButton* btn_play = new QPushButton("&MoveIt!", this);
148  btn_play->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
149  btn_play->setMaximumWidth(300);
150  connect(btn_play, SIGNAL(clicked()), this, SLOT(playPoses()));
151  controls_layout->addWidget(btn_play);
152  controls_layout->setAlignment(btn_play, Qt::AlignLeft);
153 
154  // Spacer
155  QWidget* spacer = new QWidget(this);
156  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
157  controls_layout->addWidget(spacer);
158 
159  // Edit Selected Button
160  btn_edit_ = new QPushButton("&Edit Selected", this);
161  btn_edit_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
162  btn_edit_->setMaximumWidth(300);
163  btn_edit_->hide(); // show once we know if there are existing poses
164  connect(btn_edit_, SIGNAL(clicked()), this, SLOT(editSelected()));
165  controls_layout->addWidget(btn_edit_);
166  controls_layout->setAlignment(btn_edit_, Qt::AlignRight);
167 
168  // Delete
169  btn_delete_ = new QPushButton("&Delete Selected", this);
170  connect(btn_delete_, SIGNAL(clicked()), this, SLOT(deleteSelected()));
171  controls_layout->addWidget(btn_delete_);
172  controls_layout->setAlignment(btn_delete_, Qt::AlignRight);
173 
174  // Add Group Button
175  QPushButton* btn_add = new QPushButton("&Add Pose", this);
176  btn_add->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
177  btn_add->setMaximumWidth(300);
178  connect(btn_add, SIGNAL(clicked()), this, SLOT(showNewScreen()));
179  controls_layout->addWidget(btn_add);
180  controls_layout->setAlignment(btn_add, Qt::AlignRight);
181 
182  // Add layout
183  layout->addLayout(controls_layout);
184 
185  // Set layout -----------------------------------------------------
186  content_widget->setLayout(layout);
187 
188  return content_widget;
189 }
190 
191 // ******************************************************************************************
192 // Create the edit widget
193 // ******************************************************************************************
195 {
196  // Main widget
197  QWidget* edit_widget = new QWidget(this);
198  // Layout
199  QVBoxLayout* layout = new QVBoxLayout();
200 
201  // 2 columns -------------------------------------------------------
202 
203  QHBoxLayout* columns_layout = new QHBoxLayout();
204  QVBoxLayout* column1 = new QVBoxLayout();
205  column2_ = new QVBoxLayout();
206 
207  // Column 1 --------------------------------------------------------
208 
209  // Simple form -------------------------------------------
210  QFormLayout* form_layout = new QFormLayout();
211  // form_layout->setContentsMargins( 0, 15, 0, 15 );
212  form_layout->setRowWrapPolicy(QFormLayout::WrapAllRows);
213 
214  // Name input
215  pose_name_field_ = new QLineEdit(this);
216  // pose_name_field_->setMaximumWidth( 300 );
217  form_layout->addRow("Pose Name:", pose_name_field_);
218 
219  // Group name input
220  group_name_field_ = new QComboBox(this);
221  group_name_field_->setEditable(false);
222  // Connect the signal for changes to the drop down box
223  connect(group_name_field_, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(loadJointSliders(const QString&)));
224  // group_name_field_->setMaximumWidth( 300 );
225  form_layout->addRow("Planning Group:", group_name_field_);
226 
227  // Indicator that robot is in collision or not
228  collision_warning_ = new QLabel("<font color='red'><b>Robot in Collision State</b></font>", this);
229  collision_warning_->setTextFormat(Qt::RichText);
230  collision_warning_->hide(); // show later
231  form_layout->addRow(" ", collision_warning_);
232 
233  column1->addLayout(form_layout);
234  columns_layout->addLayout(column1);
235 
236  // Column 2 --------------------------------------------------------
237 
238  // Box to hold joint sliders
239  joint_list_widget_ = new QWidget(this);
240 
241  // Create scroll area
242  scroll_area_ = new QScrollArea(this);
243  // scroll_area_->setBackgroundRole(QPalette::Dark);
244  scroll_area_->setWidget(joint_list_widget_);
245 
246  column2_->addWidget(scroll_area_);
247 
248  columns_layout->addLayout(column2_);
249 
250  // Set columns in main layout
251  layout->addLayout(columns_layout);
252 
253  // Bottom Buttons --------------------------------------------------
254 
255  QHBoxLayout* controls_layout = new QHBoxLayout();
256  controls_layout->setContentsMargins(0, 25, 0, 15);
257 
258  // Spacer
259  QWidget* spacer = new QWidget(this);
260  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
261  controls_layout->addWidget(spacer);
262 
263  // Save
264  QPushButton* btn_save_ = new QPushButton("&Save", this);
265  btn_save_->setMaximumWidth(200);
266  connect(btn_save_, SIGNAL(clicked()), this, SLOT(doneEditing()));
267  controls_layout->addWidget(btn_save_);
268  controls_layout->setAlignment(btn_save_, Qt::AlignRight);
269 
270  // Cancel
271  QPushButton* btn_cancel_ = new QPushButton("&Cancel", this);
272  btn_cancel_->setMaximumWidth(200);
273  connect(btn_cancel_, SIGNAL(clicked()), this, SLOT(cancelEditing()));
274  controls_layout->addWidget(btn_cancel_);
275  controls_layout->setAlignment(btn_cancel_, Qt::AlignRight);
276 
277  // Add layout
278  layout->addLayout(controls_layout);
279 
280  // Set layout -----------------------------------------------------
281  edit_widget->setLayout(layout);
282 
283  return edit_widget;
284 }
285 
286 // ******************************************************************************************
287 // Show edit screen for creating a new pose
288 // ******************************************************************************************
290 {
291  // Switch to screen - do this before clearEditText()
292  stacked_layout_->setCurrentIndex(1);
293 
294  // Remember that this is a new pose
295  current_edit_pose_ = nullptr;
296 
297  // Manually send the load joint sliders signal
298  if (!group_name_field_->currentText().isEmpty())
299  loadJointSliders(group_name_field_->currentText());
300 
301  // Clear previous data
302  pose_name_field_->setText("");
303 
304  // Announce that this widget is in modal mode
305  Q_EMIT isModal(true);
306 }
307 
308 // ******************************************************************************************
309 // Edit whatever element is selected
310 // ******************************************************************************************
311 void RobotPosesWidget::editDoubleClicked(int row, int column)
312 {
313  // We'll just base the edit on the selection highlight
314  editSelected();
315 }
316 
317 // ******************************************************************************************
318 // Preview whatever element is selected
319 // ******************************************************************************************
320 void RobotPosesWidget::previewClicked(int row, int column)
321 {
322  const std::string& name = data_table_->item(row, 0)->text().toStdString();
323  const std::string& group = data_table_->item(row, 1)->text().toStdString();
324 
325  // Find the selected in datastructure
326  srdf::Model::GroupState* pose = findPoseByName(name, group);
327 
328  showPose(pose);
329 }
330 
331 // ******************************************************************************************
332 // Show the robot in the current pose
333 // ******************************************************************************************
335 {
336  // Set pose joint values by adding them to the local joint state map
337  for (std::map<std::string, std::vector<double> >::const_iterator value_it = pose->joint_values_.begin();
338  value_it != pose->joint_values_.end(); ++value_it)
339  {
340  // Only copy the first joint value // TODO: add capability for multi-DOF joints?
341  joint_state_map_[value_it->first] = value_it->second[0];
342  }
343 
344  // Update the joints
345  publishJoints();
346 
347  // Unhighlight all links
348  Q_EMIT unhighlightAll();
349 
350  // Highlight group
351  Q_EMIT highlightGroup(pose->group_);
352 }
353 
354 // ******************************************************************************************
355 // Show the robot in its default joint positions
356 // ******************************************************************************************
358 {
359  // Get list of all joints for the robot
360  std::vector<const robot_model::JointModel*> joint_models = config_data_->getRobotModel()->getJointModels();
361 
362  // Iterate through the joints
363  for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models.begin();
364  joint_it < joint_models.end(); ++joint_it)
365  {
366  // Check that this joint only represents 1 variable.
367  if ((*joint_it)->getVariableCount() == 1)
368  {
369  double init_value;
370 
371  // get the first joint value in its vector
372  (*joint_it)->getVariableDefaultPositions(&init_value);
373 
374  // Change joint's value in joint_state_map to the default
375  joint_state_map_[(*joint_it)->getName()] = init_value;
376  }
377  }
378 
379  // Update the joints
380  publishJoints();
381 
382  // Unhighlight all links
383  Q_EMIT unhighlightAll();
384 }
385 
386 // ******************************************************************************************
387 // Play through the poses
388 // ******************************************************************************************
390 {
391  // Loop through each pose and play them
392  for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
393  pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
394  {
395  ROS_INFO_STREAM("Showing pose " << pose_it->name_);
396  showPose(&(*pose_it));
397  ros::Duration(0.05).sleep();
398  QApplication::processEvents();
399  ros::Duration(0.45).sleep();
400  }
401 }
402 
403 // ******************************************************************************************
404 // Edit whatever element is selected
405 // ******************************************************************************************
407 {
408  const auto& ranges = data_table_->selectedRanges();
409  if (!ranges.size())
410  return;
411  edit(ranges[0].bottomRow());
412 }
413 
414 // ******************************************************************************************
415 // Edit pose
416 // ******************************************************************************************
418 {
419  const std::string& name = data_table_->item(row, 0)->text().toStdString();
420  const std::string& group = data_table_->item(row, 1)->text().toStdString();
421 
422  // Find the selected in datastruture
423  srdf::Model::GroupState* pose = findPoseByName(name, group);
424  current_edit_pose_ = pose;
425 
426  // Set pose name
427  pose_name_field_->setText(pose->name_.c_str());
428 
429  // Set group:
430  int index = group_name_field_->findText(pose->group_.c_str());
431  if (index == -1)
432  {
433  QMessageBox::critical(this, "Error Loading", "Unable to find group name in drop down box");
434  return;
435  }
436  group_name_field_->setCurrentIndex(index);
437 
438  // Set pose joint values by adding them to the local joint state map
439  for (std::map<std::string, std::vector<double> >::const_iterator value_it = pose->joint_values_.begin();
440  value_it != pose->joint_values_.end(); ++value_it)
441  {
442  // Only copy the first joint value // TODO: add capability for multi-DOF joints?
443  joint_state_map_[value_it->first] = value_it->second[0];
444  }
445 
446  // Update robot model in rviz
447  publishJoints();
448 
449  // Switch to screen - do this before setCurrentIndex
450  stacked_layout_->setCurrentIndex(1);
451 
452  // Announce that this widget is in modal mode
453  Q_EMIT isModal(true);
454 
455  // Manually send the load joint sliders signal
456  loadJointSliders(QString(pose->group_.c_str()));
457 }
458 
459 // ******************************************************************************************
460 // Populate the combo dropdown box with avail group names
461 // ******************************************************************************************
463 {
464  // Remove all old groups
465  group_name_field_->clear();
466 
467  // Add all group names to combo box
468  for (std::vector<srdf::Model::Group>::iterator group_it = config_data_->srdf_->groups_.begin();
469  group_it != config_data_->srdf_->groups_.end(); ++group_it)
470  {
471  group_name_field_->addItem(group_it->name_.c_str());
472  }
473 }
474 
475 // ******************************************************************************************
476 // Load the joint sliders based on group selected from combo box
477 // ******************************************************************************************
478 void RobotPosesWidget::loadJointSliders(const QString& selected)
479 {
480  // Ignore this event if the combo box is empty. This occurs when clearing the combo box and reloading with the
481  // newest groups. Also ignore if we are not on the edit screen
482  if (!group_name_field_->count() || selected.isEmpty() || stacked_layout_->currentIndex() == 0)
483  return;
484 
485  // Get group name from input
486  const std::string group_name = selected.toStdString();
487 
488  // Check that joint model exist
489  if (!config_data_->getRobotModel()->hasJointModelGroup(group_name))
490  {
491  QMessageBox::critical(this, "Error Loading", QString("Unable to find joint model group for group: ")
492  .append(group_name.c_str())
493  .append(" Are you sure this group has associated joints/links?"));
494  return;
495  }
496 
497  // Delete old sliders from joint_list_layout_ if this has been loaded before
498  if (joint_list_layout_)
499  {
500  delete joint_list_layout_;
501  qDeleteAll(joint_list_widget_->children());
502  }
503 
504  // Create layout again
505  joint_list_layout_ = new QVBoxLayout();
506  joint_list_widget_->setLayout(joint_list_layout_);
507  // joint_list_widget_->setSizePolicy( QSizePolicy::Expanding, QSizePolicy::Expanding );
508  joint_list_widget_->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Ignored);
509  joint_list_widget_->setMinimumSize(50, 50); // w, h
510 
511  // Get list of associated joints
512  const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name);
513  joint_models_ = joint_model_group->getJointModels();
514 
515  // Iterate through the joints
516  int num_joints = 0;
517  for (const robot_model::JointModel* joint_model : joint_models_)
518  {
519  if (joint_model->getVariableCount() != 1 || // only consider 1-variable joints
520  joint_model->isPassive() || // ignore passive
521  joint_model->getMimic()) // and mimic joints
522  continue;
523 
524  double init_value;
525 
526  // Decide what this joint's initial value is
527  if (joint_state_map_.find(joint_model->getName()) == joint_state_map_.end())
528  {
529  // The joint state map does not yet have an entry for this joint
530  // Get the first joint value in its vector
531  joint_model->getVariableDefaultPositions(&init_value);
532  }
533  else // There is already a value in the map
534  {
535  init_value = joint_state_map_[joint_model->getName()];
536  }
537 
538  // For each joint in group add slider
539  SliderWidget* sw = new SliderWidget(this, joint_model, init_value);
540  joint_list_layout_->addWidget(sw);
541 
542  // Connect value change event
543  connect(sw, SIGNAL(jointValueChanged(const std::string&, double)), this,
544  SLOT(updateRobotModel(const std::string&, double)));
545 
546  ++num_joints;
547  }
548 
549  // Copy the width of column 2 and manually calculate height from number of joints
550  joint_list_widget_->resize(300, num_joints * 70); // w, h
551 
552  // Update the robot model in Rviz with newly selected joint values
553  publishJoints();
554 
555  // Highlight the planning group
556  Q_EMIT unhighlightAll();
557  Q_EMIT highlightGroup(group_name);
558 }
559 
560 // ******************************************************************************************
561 // Find the associated data by name
562 // ******************************************************************************************
563 srdf::Model::GroupState* RobotPosesWidget::findPoseByName(const std::string& name, const std::string& group)
564 {
565  // Find the group state we are editing based on the pose name
566  srdf::Model::GroupState* searched_state = NULL; // used for holding our search results
567 
568  for (srdf::Model::GroupState& state : config_data_->srdf_->group_states_)
569  {
570  if (state.name_ == name && state.group_ == group) // match
571  {
572  searched_state = &state;
573  break;
574  }
575  }
576 
577  return searched_state;
578 }
579 
580 // ******************************************************************************************
581 // Delete currently editing item
582 // ******************************************************************************************
584 {
585  const auto& ranges = data_table_->selectedRanges();
586  if (!ranges.size())
587  return;
588  int row = ranges[0].bottomRow();
589 
590  const std::string& name = data_table_->item(row, 0)->text().toStdString();
591  const std::string& group = data_table_->item(row, 1)->text().toStdString();
592 
593  // Confirm user wants to delete group
594  if (QMessageBox::question(this, "Confirm Pose Deletion",
595  QString("Are you sure you want to delete the pose '").append(name.c_str()).append("'?"),
596  QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
597  {
598  return;
599  }
600 
601  // Delete pose from vector
602  for (std::vector<srdf::Model::GroupState>::iterator pose_it = config_data_->srdf_->group_states_.begin();
603  pose_it != config_data_->srdf_->group_states_.end(); ++pose_it)
604  {
605  // check if this is the group we want to delete
606  if (pose_it->name_ == name && pose_it->group_ == group) // match
607  {
608  config_data_->srdf_->group_states_.erase(pose_it);
609  break;
610  }
611  }
612 
613  // Reload main screen table
614  loadDataTable();
616 }
617 
618 // ******************************************************************************************
619 // Save editing changes
620 // ******************************************************************************************
622 {
623  // Get a reference to the supplied strings
624  const std::string& name = pose_name_field_->text().toStdString();
625  const std::string& group = group_name_field_->currentText().toStdString();
626 
627  // Used for editing existing groups
628  srdf::Model::GroupState* searched_data = NULL;
629 
630  // Check that name field is not empty
631  if (name.empty())
632  {
633  QMessageBox::warning(this, "Error Saving", "A name must be given for the pose!");
634  pose_name_field_->setFocus();
635  return;
636  }
637  // Check that a group was selected
638  if (group.empty())
639  {
640  QMessageBox::warning(this, "Error Saving", "A planning group must be chosen!");
641  group_name_field_->setFocus();
642  return;
643  }
644 
645  // If creating a new pose, check if the (name, group) pair already exists
646  if (!current_edit_pose_)
647  {
648  searched_data = findPoseByName(name, group);
649  if (searched_data != current_edit_pose_)
650  {
651  if (QMessageBox::warning(this, "Warning Saving", "A pose already exists with that name! Overwrite?",
652  QMessageBox::Yes | QMessageBox::No, QMessageBox::No) == QMessageBox::No)
653  return;
654  }
655  }
656  else
657  searched_data = current_edit_pose_; // overwrite edited pose
658 
660 
661  // Save the new pose name or create the new pose ----------------------------
662  bool isNew = false;
663 
664  if (searched_data == NULL) // create new
665  {
666  isNew = true;
667  searched_data = new srdf::Model::GroupState();
668  }
669 
670  // Copy name data ----------------------------------------------------
671  searched_data->name_ = name;
672  searched_data->group_ = group;
673 
674  // Copy joint positions ----------------------------------------
675 
676  // Clear the old values
677  searched_data->joint_values_.clear();
678 
679  // Iterate through the current group's joints and add to SRDF
680  for (std::vector<const robot_model::JointModel*>::const_iterator joint_it = joint_models_.begin();
681  joint_it < joint_models_.end(); ++joint_it)
682  {
683  // Check that this joint only represents 1 variable.
684  if ((*joint_it)->getVariableCount() == 1)
685  {
686  // Create vector for new joint values
687  std::vector<double> joint_value;
688  joint_value.push_back(joint_state_map_[(*joint_it)->getName()]);
689 
690  // Add joint vector to SRDF
691  searched_data->joint_values_[(*joint_it)->getName()] = joint_value;
692  }
693  }
694 
695  // Insert new poses into group state vector --------------------------
696  if (isNew)
697  {
698  config_data_->srdf_->group_states_.push_back(*searched_data);
699  }
700 
701  // Finish up ------------------------------------------------------
702 
703  // Reload main screen table
704  loadDataTable();
705 
706  // Switch to screen
707  stacked_layout_->setCurrentIndex(0);
708 
709  // Announce that this widget is done with modal mode
710  Q_EMIT isModal(false);
711 }
712 
713 // ******************************************************************************************
714 // Cancel changes
715 // ******************************************************************************************
717 {
718  // Switch to screen
719  stacked_layout_->setCurrentIndex(0);
720 
721  // Announce that this widget is done with modal mode
722  Q_EMIT isModal(false);
723 }
724 
725 // ******************************************************************************************
726 // Load the robot poses into the table
727 // ******************************************************************************************
729 {
730  // Disable Table
731  data_table_->setUpdatesEnabled(false); // prevent table from updating until we are completely done
732  data_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called
733  data_table_->clearContents();
734 
735  // Set size of datatable
736  data_table_->setRowCount(config_data_->srdf_->group_states_.size());
737 
738  // Loop through every pose
739  int row = 0;
740  for (std::vector<srdf::Model::GroupState>::const_iterator data_it = config_data_->srdf_->group_states_.begin();
741  data_it != config_data_->srdf_->group_states_.end(); ++data_it)
742  {
743  // Create row elements
744  QTableWidgetItem* data_name = new QTableWidgetItem(data_it->name_.c_str());
745  data_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
746  QTableWidgetItem* group_name = new QTableWidgetItem(data_it->group_.c_str());
747  group_name->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
748 
749  // Add to table
750  data_table_->setItem(row, 0, data_name);
751  data_table_->setItem(row, 1, group_name);
752 
753  // Increment counter
754  ++row;
755  }
756 
757  // Reenable
758  data_table_->setUpdatesEnabled(true); // prevent table from updating until we are completely done
759  data_table_->setDisabled(false); // make sure we disable it so that the cellChanged event is not called
760 
761  // Resize header
762  data_table_->resizeColumnToContents(0);
763  data_table_->resizeColumnToContents(1);
764 
765  // Show edit button if applicable
766  if (config_data_->srdf_->group_states_.size())
767  btn_edit_->show();
768 }
769 
770 // ******************************************************************************************
771 // Called when setup assistant navigation switches to this screen
772 // ******************************************************************************************
774 {
775  // Show the current poses screen
776  stacked_layout_->setCurrentIndex(0);
777 
778  // Load the data to the tree
779  loadDataTable();
780 
781  // Load the avail groups to the combo box
783 }
784 
785 // ******************************************************************************************
786 // Call when one of the sliders has its value changed
787 // ******************************************************************************************
788 void RobotPosesWidget::updateRobotModel(const std::string& name, double value)
789 {
790  // Save the new value
791  joint_state_map_[name] = value;
792 
793  // Update the robot model/rviz
794  publishJoints();
795 }
796 
797 // ******************************************************************************************
798 // Publish the joint values in the joint_state_map_ to Rviz
799 // ******************************************************************************************
801 {
802  // Change the scene
803  // scene.getCurrentState().setToDefaultValues();//set to default values of 0 OR half between low and high joint values
804  // config_data_->getPlanningScene()->getCurrentState().setToRandomValues();
805 
806  // Set the joints based on the map
807  config_data_->getPlanningScene()->getCurrentStateNonConst().setVariablePositions(joint_state_map_);
808 
809  // Create a planning scene message
810  moveit_msgs::DisplayRobotState msg;
811  robot_state::robotStateToRobotStateMsg(config_data_->getPlanningScene()->getCurrentState(), msg.state);
812 
813  // Publish!
815 
816  // Prevent dirty collision body transforms
817  config_data_->getPlanningScene()->getCurrentStateNonConst().update();
818 
819  // Decide if current state is in collision
821  config_data_->getPlanningScene()->checkSelfCollision(
822  request, result, config_data_->getPlanningScene()->getCurrentState(), config_data_->allowed_collision_matrix_);
823  // Show result notification
824  if (result.contacts.size())
825  {
826  collision_warning_->show();
827  }
828  else
829  {
830  collision_warning_->hide();
831  }
832 }
833 
834 // ******************************************************************************************
835 // ******************************************************************************************
836 // Slider Widget
837 // ******************************************************************************************
838 // ******************************************************************************************
839 
840 // ******************************************************************************************
841 // Simple widget for adjusting joints of a robot
842 // ******************************************************************************************
843 SliderWidget::SliderWidget(QWidget* parent, const robot_model::JointModel* joint_model, double init_value)
844  : QWidget(parent), joint_model_(joint_model)
845 {
846  // Create layouts
847  QVBoxLayout* layout = new QVBoxLayout();
848  QHBoxLayout* row2 = new QHBoxLayout();
849 
850  // Row 1
851  joint_label_ = new QLabel(joint_model_->getName().c_str(), this);
852  joint_label_->setContentsMargins(0, 0, 0, 0);
853  layout->addWidget(joint_label_);
854 
855  // Row 2 -------------------------------------------------------------
856  joint_slider_ = new QSlider(Qt::Horizontal, this);
857  joint_slider_->setTickPosition(QSlider::TicksBelow);
858  joint_slider_->setSingleStep(10);
859  joint_slider_->setPageStep(500);
860  joint_slider_->setTickInterval(1000);
861  joint_slider_->setContentsMargins(0, 0, 0, 0);
862  row2->addWidget(joint_slider_);
863 
864  // Joint Value Box ------------------------------------------------
865  joint_value_ = new QLineEdit(this);
866  joint_value_->setMaximumWidth(62);
867  joint_value_->setContentsMargins(0, 0, 0, 0);
868  connect(joint_value_, SIGNAL(editingFinished()), this, SLOT(changeJointSlider()));
869  row2->addWidget(joint_value_);
870 
871  // Joint Limits ----------------------------------------------------
872  const std::vector<moveit_msgs::JointLimits>& limits = joint_model_->getVariableBoundsMsg();
873  if (limits.empty())
874  {
875  QMessageBox::critical(this, "Error Loading", "An internal error has occured while loading the joints");
876  return;
877  }
878 
879  // Only use the first limit, because there is only 1 variable (as checked earlier)
880  moveit_msgs::JointLimits joint_limit = limits[0];
881  max_position_ = joint_limit.max_position;
882  min_position_ = joint_limit.min_position;
883 
884  // Set the slider limits
885  joint_slider_->setMaximum(max_position_ * 10000);
886  joint_slider_->setMinimum(min_position_ * 10000);
887 
888  // Connect slider to joint value box
889  connect(joint_slider_, SIGNAL(valueChanged(int)), this, SLOT(changeJointValue(int)));
890 
891  // Initial joint values -------------------------------------------
892  int value = init_value * 10000; // scale double to integer for slider use
893  joint_slider_->setSliderPosition(value); // set slider value
894  changeJointValue(value); // show in textbox
895 
896  // Finish GUI ----------------------------------------
897  layout->addLayout(row2);
898 
899  this->setContentsMargins(0, 0, 0, 0);
900  // this->setSizePolicy( QSizePolicy::Expanding, QSizePolicy::Preferred );
901  this->setGeometry(QRect(110, 80, 120, 80));
902  this->setLayout(layout);
903 
904  // Declare std::string as metatype so we can use it in a signal
905  qRegisterMetaType<std::string>("std::string");
906 }
907 
908 // ******************************************************************************************
909 // Deconstructor
910 // ******************************************************************************************
912 {
913 }
914 
915 // ******************************************************************************************
916 // Called when the joint value slider is changed
917 // ******************************************************************************************
919 {
920  // Get joint value
921  const double double_value = double(value) / 10000;
922 
923  // Set textbox
924  joint_value_->setText(QString("%1").arg(double_value, 0, 'f', 4));
925 
926  // Send event to parent widget
927  Q_EMIT jointValueChanged(joint_model_->getName(), double_value);
928 }
929 
930 // ******************************************************************************************
931 // Called when the joint value box is changed
932 // ******************************************************************************************
934 {
935  // Get joint value
936  double value = joint_value_->text().toDouble();
937 
938  if (min_position_ > value || value > max_position_)
939  {
940  value = (min_position_ > value) ? min_position_ : max_position_;
941  joint_value_->setText(QString("%1").arg(value, 0, 'f', 4));
942  }
943 
944  // We assume it converts to double because of the validator
945  joint_slider_->setSliderPosition(value * 10000);
946 
947  // Send event to parent widget
948  Q_EMIT jointValueChanged(joint_model_->getName(), value);
949 }
950 
951 } // namespace
srdf::Model::GroupState * current_edit_pose_
Pointer to currently edited group state.
#define NULL
void publishJoints()
Publishes a joint state message based on all the slider locations in a planning group, to rviz.
void deleteSelected()
Delete currently editing ite.
std::map< std::string, std::vector< double > > joint_values_
void publish(const boost::shared_ptr< M > &message) const
void isModal(bool isModal)
Event for when the current screen is in modal view. Essential disabled the left navigation.
bool sleep() const
std::map< std::string, double > joint_state_map_
All the joint slider values that have thus far been seen. May contain more than just the current join...
RobotPosesWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
name
void showPose(srdf::Model::GroupState *pose)
const robot_model::JointModel * joint_model_
void changeJointValue(int value)
Called when the joint value slider is changed.
group
void unhighlightAll()
Event for telling rviz to unhighlight all links of the robot.
unsigned int index
void playPoses()
Play through the poses.
srdf::Model::GroupState * findPoseByName(const std::string &name, const std::string &group)
SliderWidget(QWidget *parent, const robot_model::JointModel *joint_model, double init_value)
void editDoubleClicked(int row, int column)
Edit the double clicked element.
void jointValueChanged(const std::string &name, double value)
Indicate joint name and value when slider widget changed.
void editSelected()
Edit whatever element is selected.
static const std::string MOVEIT_ROBOT_STATE
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void focusGiven()
Received when this widget is chosen from the navigation menu.
void updateRobotModel(const std::string &name, double value)
std::vector< const robot_model::JointModel * > joint_models_
The joints currently in the selected planning group.
ros::Publisher pub_robot_state_
Remember the publisher for quick publishing later.
void previewClicked(int row, int column)
Preview whatever element is selected.
#define ROS_INFO_STREAM(args)
void changeJointSlider()
Called when the joint value box is changed.
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void showDefaultPose()
Show the robot in its default joint positions.
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
collision_detection::CollisionRequest request
void highlightGroup(const std::string &name)
Event for telling rviz to highlight a group of the robot.
int value
void loadJointSliders(const QString &selected)
Run this whenever the group is changed.


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