kinematic_chain_widget.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman */
00036 
00037 #include <QVBoxLayout>
00038 #include <QHBoxLayout>
00039 #include <QGroupBox>
00040 #include <QPushButton>
00041 #include <QGridLayout>
00042 #include <QLineEdit>
00043 #include <QMessageBox>
00044 #include <QString>
00045 #include "kinematic_chain_widget.h"
00046 
00047 namespace moveit_setup_assistant
00048 {
00049 // ******************************************************************************************
00050 // Constructor
00051 // ******************************************************************************************
00052 KinematicChainWidget::KinematicChainWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
00053   : QWidget(parent), config_data_(config_data)
00054 {
00055   // Basic widget container
00056   QVBoxLayout* layout = new QVBoxLayout();
00057 
00058   // Label ------------------------------------------------
00059   title_ = new QLabel("", this);  // specify the title from the parent widget
00060   QFont group_title_font("Arial", 12, QFont::Bold);
00061   title_->setFont(group_title_font);
00062   layout->addWidget(title_);
00063 
00064   // Create link tree ------------------------------------------------------
00065   link_tree_ = new QTreeWidget(this);
00066   link_tree_->setHeaderLabel("Robot Links");
00067   connect(link_tree_, SIGNAL(itemSelectionChanged()), this, SLOT(itemSelected()));
00068   layout->addWidget(link_tree_);
00069 
00070   // Create Grid Layout for form --------------------------------------------
00071   QGridLayout* form_grid = new QGridLayout();
00072   form_grid->setContentsMargins(20, 20, 20, 20);  // left top right bottom
00073 
00074   // Row 1: Base Link
00075   QLabel* base_link_label = new QLabel("Base Link", this);
00076   form_grid->addWidget(base_link_label, 0, 0, Qt::AlignRight);
00077 
00078   base_link_field_ = new QLineEdit(this);
00079   base_link_field_->setMinimumWidth(300);
00080   form_grid->addWidget(base_link_field_, 0, 1, Qt::AlignLeft);
00081 
00082   QPushButton* btn_base_link = new QPushButton("Choose Selected", this);
00083   connect(btn_base_link, SIGNAL(clicked()), this, SLOT(baseLinkTreeClick()));
00084   form_grid->addWidget(btn_base_link, 0, 2, Qt::AlignLeft);
00085 
00086   // Row 2: Tip Link
00087   QLabel* tip_link_label = new QLabel("Tip Link", this);
00088   form_grid->addWidget(tip_link_label, 1, 0, Qt::AlignRight);
00089 
00090   tip_link_field_ = new QLineEdit(this);
00091   tip_link_field_->setMinimumWidth(300);
00092   form_grid->addWidget(tip_link_field_, 1, 1, Qt::AlignLeft);
00093 
00094   QPushButton* btn_tip_link = new QPushButton("Choose Selected", this);
00095   connect(btn_tip_link, SIGNAL(clicked()), this, SLOT(tipLinkTreeClick()));
00096   form_grid->addWidget(btn_tip_link, 1, 2, Qt::AlignLeft);
00097 
00098   // Add form grid layout
00099   layout->addLayout(form_grid);
00100 
00101   // Bottom Controls ---------------------------------------------------------
00102   QHBoxLayout* controls_layout = new QHBoxLayout();
00103 
00104   // Expand/Contract controls
00105   QLabel* expand_controls = new QLabel(this);
00106   expand_controls->setText("<a href='expand'>Expand All</a> <a href='contract'>Collapse All</a>");
00107   connect(expand_controls, SIGNAL(linkActivated(const QString)), this, SLOT(alterTree(const QString)));
00108   controls_layout->addWidget(expand_controls);
00109 
00110   // Spacer
00111   QWidget* spacer = new QWidget(this);
00112   spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00113   controls_layout->addWidget(spacer);
00114 
00115   // Save
00116   QPushButton* btn_save = new QPushButton("&Save", this);
00117   btn_save->setMaximumWidth(200);
00118   connect(btn_save, SIGNAL(clicked()), this, SIGNAL(doneEditing()));
00119   controls_layout->addWidget(btn_save);
00120   controls_layout->setAlignment(btn_save, Qt::AlignRight);
00121 
00122   // Cancel
00123   QPushButton* btn_cancel = new QPushButton("&Cancel", this);
00124   btn_cancel->setMaximumWidth(200);
00125   connect(btn_cancel, SIGNAL(clicked()), this, SIGNAL(cancelEditing()));
00126   controls_layout->addWidget(btn_cancel);
00127   controls_layout->setAlignment(btn_cancel, Qt::AlignRight);
00128 
00129   // Add layout
00130   layout->addLayout(controls_layout);
00131 
00132   // Finish Layout --------------------------------------------------
00133   this->setLayout(layout);
00134 
00135   // Remember that we have no loaded the chains yet
00136   kinematic_chain_loaded_ = false;
00137 }
00138 
00139 // ******************************************************************************************
00140 // Load robot links into tree
00141 // ******************************************************************************************
00142 void KinematicChainWidget::setAvailable()
00143 {
00144   // Only load the kinematic chain once
00145   if (kinematic_chain_loaded_)
00146     return;
00147 
00148   // Retrieve pointer to the shared kinematic model
00149   const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();
00150 
00151   // Get the root joint
00152   const robot_model::JointModel* root_joint = model->getRootJoint();
00153 
00154   addLinktoTreeRecursive(root_joint->getChildLinkModel(), NULL);
00155 
00156   // Remember that we have loaded the chain
00157   kinematic_chain_loaded_ = true;
00158 }
00159 
00160 // ******************************************************************************************
00161 //
00162 // ******************************************************************************************
00163 void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* link,
00164                                                   const robot_model::LinkModel* parent)
00165 {
00166   // Create new tree item
00167   QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_);
00168 
00169   // Add item to tree
00170   if (parent == NULL)
00171   {
00172     new_item->setText(0, link->getName().c_str());
00173     link_tree_->addTopLevelItem(new_item);
00174   }
00175   else
00176   {
00177     for (int i = 0; i < link_tree_->topLevelItemCount(); i++)
00178     {
00179       if (addLinkChildRecursive(link_tree_->topLevelItem(i), link, parent->getName()))
00180       {
00181         break;
00182       }
00183     }
00184   }
00185   for (size_t i = 0; i < link->getChildJointModels().size(); i++)
00186   {
00187     addLinktoTreeRecursive(link->getChildJointModels()[i]->getChildLinkModel(), link);
00188   }
00189 }
00190 
00191 // ******************************************************************************************
00192 //
00193 // ******************************************************************************************
00194 bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link,
00195                                                  const std::string& parent_name)
00196 {
00197   if (parent->text(0).toStdString() == parent_name)
00198   {
00199     QTreeWidgetItem* new_item = new QTreeWidgetItem(parent);
00200     new_item->setText(0, link->getName().c_str());
00201 
00202     parent->addChild(new_item);
00203     return true;
00204   }
00205   else
00206   {
00207     for (int i = 0; i < parent->childCount(); i++)
00208     {
00209       if (addLinkChildRecursive(parent->child(i), link, parent_name))
00210       {
00211         return true;
00212       }
00213     }
00214   }
00215 
00216   return false;
00217 }
00218 
00219 // ******************************************************************************************
00220 // Set the link field with previous value
00221 // ******************************************************************************************
00222 void KinematicChainWidget::setSelected(const std::string& base_link, const std::string& tip_link)
00223 {
00224   base_link_field_->setText(QString(base_link.c_str()));
00225   tip_link_field_->setText(QString(tip_link.c_str()));
00226 }
00227 
00228 // ******************************************************************************************
00229 // Choose the base link
00230 // ******************************************************************************************
00231 void KinematicChainWidget::baseLinkTreeClick()
00232 {
00233   QTreeWidgetItem* item = link_tree_->currentItem();
00234   if (item != NULL)
00235   {
00236     base_link_field_->setText(item->text(0));
00237   }
00238 }
00239 
00240 // ******************************************************************************************
00241 // Choose the tip link
00242 // ******************************************************************************************
00243 void KinematicChainWidget::tipLinkTreeClick()
00244 {
00245   QTreeWidgetItem* item = link_tree_->currentItem();
00246   if (item != NULL)
00247   {
00248     tip_link_field_->setText(item->text(0));
00249   }
00250 }
00251 
00252 // ******************************************************************************************
00253 // Expand/Collapse Tree
00254 // ******************************************************************************************
00255 void KinematicChainWidget::alterTree(const QString& link)
00256 {
00257   if (link.contains("expand"))
00258     link_tree_->expandAll();
00259   else
00260     link_tree_->collapseAll();
00261 }
00262 
00263 // ******************************************************************************************
00264 // Highlight the currently selected link
00265 // ******************************************************************************************
00266 void KinematicChainWidget::itemSelected()
00267 {
00268   QTreeWidgetItem* item = link_tree_->currentItem();
00269   if (item != NULL)
00270   {
00271     Q_EMIT unhighlightAll();
00272 
00273     std::string name = item->text(0).toStdString();
00274 
00275     // Don't try to highlight empty links!
00276     if (name.empty())
00277       return;
00278 
00279     // Check that item is not empty
00280     Q_EMIT highlightLink(item->text(0).toStdString(), QColor(255, 0, 0));
00281   }
00282 }
00283 }


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Mon Jul 24 2017 02:22:29