kinematic_chain_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 <QVBoxLayout>
38 #include <QHBoxLayout>
39 #include <QGroupBox>
40 #include <QPushButton>
41 #include <QGridLayout>
42 #include <QLineEdit>
43 #include <QMessageBox>
44 #include <QString>
45 #include "kinematic_chain_widget.h"
46 
47 namespace moveit_setup_assistant
48 {
49 // ******************************************************************************************
50 // Constructor
51 // ******************************************************************************************
52 KinematicChainWidget::KinematicChainWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
53  : QWidget(parent), config_data_(config_data)
54 {
55  // Basic widget container
56  QVBoxLayout* layout = new QVBoxLayout();
57 
58  // Label ------------------------------------------------
59  title_ = new QLabel("", this); // specify the title from the parent widget
60  QFont group_title_font(QFont().defaultFamily(), 12, QFont::Bold);
61  title_->setFont(group_title_font);
62  layout->addWidget(title_);
63 
64  // Create link tree ------------------------------------------------------
65  link_tree_ = new QTreeWidget(this);
66  link_tree_->setHeaderLabel("Robot Links");
67  connect(link_tree_, SIGNAL(itemSelectionChanged()), this, SLOT(itemSelected()));
68  layout->addWidget(link_tree_);
69 
70  // Create Grid Layout for form --------------------------------------------
71  QGridLayout* form_grid = new QGridLayout();
72  form_grid->setContentsMargins(20, 20, 20, 20); // left top right bottom
73 
74  // Row 1: Base Link
75  QLabel* base_link_label = new QLabel("Base Link", this);
76  form_grid->addWidget(base_link_label, 0, 0, Qt::AlignRight);
77 
78  base_link_field_ = new QLineEdit(this);
79  base_link_field_->setMinimumWidth(300);
80  form_grid->addWidget(base_link_field_, 0, 1, Qt::AlignLeft);
81 
82  QPushButton* btn_base_link = new QPushButton("Choose Selected", this);
83  connect(btn_base_link, SIGNAL(clicked()), this, SLOT(baseLinkTreeClick()));
84  form_grid->addWidget(btn_base_link, 0, 2, Qt::AlignLeft);
85 
86  // Row 2: Tip Link
87  QLabel* tip_link_label = new QLabel("Tip Link", this);
88  form_grid->addWidget(tip_link_label, 1, 0, Qt::AlignRight);
89 
90  tip_link_field_ = new QLineEdit(this);
91  tip_link_field_->setMinimumWidth(300);
92  form_grid->addWidget(tip_link_field_, 1, 1, Qt::AlignLeft);
93 
94  QPushButton* btn_tip_link = new QPushButton("Choose Selected", this);
95  connect(btn_tip_link, SIGNAL(clicked()), this, SLOT(tipLinkTreeClick()));
96  form_grid->addWidget(btn_tip_link, 1, 2, Qt::AlignLeft);
97 
98  // Add form grid layout
99  layout->addLayout(form_grid);
100 
101  // Bottom Controls ---------------------------------------------------------
102  QHBoxLayout* controls_layout = new QHBoxLayout();
103 
104  // Expand/Contract controls
105  QLabel* expand_controls = new QLabel(this);
106  expand_controls->setText("<a href='expand'>Expand All</a> <a href='contract'>Collapse All</a>");
107  connect(expand_controls, SIGNAL(linkActivated(const QString)), this, SLOT(alterTree(const QString)));
108  controls_layout->addWidget(expand_controls);
109 
110  // Spacer
111  QWidget* spacer = new QWidget(this);
112  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
113  controls_layout->addWidget(spacer);
114 
115  // Save
116  QPushButton* btn_save = new QPushButton("&Save", this);
117  btn_save->setMaximumWidth(200);
118  connect(btn_save, SIGNAL(clicked()), this, SIGNAL(doneEditing()));
119  controls_layout->addWidget(btn_save);
120  controls_layout->setAlignment(btn_save, Qt::AlignRight);
121 
122  // Cancel
123  QPushButton* btn_cancel = new QPushButton("&Cancel", this);
124  btn_cancel->setMaximumWidth(200);
125  connect(btn_cancel, SIGNAL(clicked()), this, SIGNAL(cancelEditing()));
126  controls_layout->addWidget(btn_cancel);
127  controls_layout->setAlignment(btn_cancel, Qt::AlignRight);
128 
129  // Add layout
130  layout->addLayout(controls_layout);
131 
132  // Finish Layout --------------------------------------------------
133  this->setLayout(layout);
134 
135  // Remember that we have no loaded the chains yet
136  kinematic_chain_loaded_ = false;
137 }
138 
139 // ******************************************************************************************
140 // Load robot links into tree
141 // ******************************************************************************************
143 {
144  // Only load the kinematic chain once
146  return;
147 
148  // Retrieve pointer to the shared kinematic model
149  const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
150 
151  // Get the root joint
152  const robot_model::JointModel* root_joint = model->getRootJoint();
153 
154  addLinktoTreeRecursive(root_joint->getChildLinkModel(), NULL);
155 
156  // Remember that we have loaded the chain
158 }
159 
160 // ******************************************************************************************
161 //
162 // ******************************************************************************************
163 void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* link,
164  const robot_model::LinkModel* parent)
165 {
166  // Create new tree item
167  QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_);
168 
169  // Add item to tree
170  if (parent == NULL)
171  {
172  new_item->setText(0, link->getName().c_str());
173  link_tree_->addTopLevelItem(new_item);
174  }
175  else
176  {
177  for (int i = 0; i < link_tree_->topLevelItemCount(); i++)
178  {
179  if (addLinkChildRecursive(link_tree_->topLevelItem(i), link, parent->getName()))
180  {
181  break;
182  }
183  }
184  }
185  for (std::size_t i = 0; i < link->getChildJointModels().size(); i++)
186  {
187  addLinktoTreeRecursive(link->getChildJointModels()[i]->getChildLinkModel(), link);
188  }
189 }
190 
191 // ******************************************************************************************
192 //
193 // ******************************************************************************************
194 bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link,
195  const std::string& parent_name)
196 {
197  if (parent->text(0).toStdString() == parent_name)
198  {
199  QTreeWidgetItem* new_item = new QTreeWidgetItem(parent);
200  new_item->setText(0, link->getName().c_str());
201 
202  parent->addChild(new_item);
203  return true;
204  }
205  else
206  {
207  for (int i = 0; i < parent->childCount(); i++)
208  {
209  if (addLinkChildRecursive(parent->child(i), link, parent_name))
210  {
211  return true;
212  }
213  }
214  }
215 
216  return false;
217 }
218 
219 // ******************************************************************************************
220 // Set the link field with previous value
221 // ******************************************************************************************
222 void KinematicChainWidget::setSelected(const std::string& base_link, const std::string& tip_link)
223 {
224  base_link_field_->setText(QString(base_link.c_str()));
225  tip_link_field_->setText(QString(tip_link.c_str()));
226 }
227 
228 // ******************************************************************************************
229 // Choose the base link
230 // ******************************************************************************************
232 {
233  QTreeWidgetItem* item = link_tree_->currentItem();
234  if (item != NULL)
235  {
236  base_link_field_->setText(item->text(0));
237  }
238 }
239 
240 // ******************************************************************************************
241 // Choose the tip link
242 // ******************************************************************************************
244 {
245  QTreeWidgetItem* item = link_tree_->currentItem();
246  if (item != NULL)
247  {
248  tip_link_field_->setText(item->text(0));
249  }
250 }
251 
252 // ******************************************************************************************
253 // Expand/Collapse Tree
254 // ******************************************************************************************
255 void KinematicChainWidget::alterTree(const QString& link)
256 {
257  if (link.contains("expand"))
258  link_tree_->expandAll();
259  else
260  link_tree_->collapseAll();
261 }
262 
263 // ******************************************************************************************
264 // Highlight the currently selected link
265 // ******************************************************************************************
267 {
268  QTreeWidgetItem* item = link_tree_->currentItem();
269  if (item != NULL)
270  {
271  Q_EMIT unhighlightAll();
272 
273  std::string name = item->text(0).toStdString();
274 
275  // Don't try to highlight empty links!
276  if (name.empty())
277  return;
278 
279  // Check that item is not empty
280  Q_EMIT highlightLink(item->text(0).toStdString(), QColor(255, 0, 0));
281  }
282 }
283 }
void setAvailable()
Loads the availble data list.
#define NULL
void setSelected(const std::string &base_link, const std::string &tip_link)
Set the link field with previous value.
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
void highlightLink(const std::string &name, const QColor &)
Event for telling rviz to highlight a link of the robot.
name
void addLinktoTreeRecursive(const robot_model::LinkModel *link, const robot_model::LinkModel *parent)
void alterTree(const QString &link)
Expand/Collapse Tree.
KinematicChainWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
Constructor.
bool kinematic_chain_loaded_
Remember if the chain tree has been loaded.
bool addLinkChildRecursive(QTreeWidgetItem *parent, const robot_model::LinkModel *link, const std::string &parent_name)
void itemSelected()
Highlight the selected link in the kinematic chain.
void cancelEditing()
Event sent when user presses cancel button.
void unhighlightAll()
Event for telling rviz to unhighlight all links of the robot.
void doneEditing()
Event sent when this widget is done making data changes and parent widget can save.


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