kinematic_chain_widget.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman */

#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGroupBox>
#include <QPushButton>
#include <QGridLayout>
#include <QLineEdit>
#include <QMessageBox>
#include <QString>
#include "kinematic_chain_widget.h"

namespace moveit_setup_assistant
{
// ******************************************************************************************
// Constructor
// ******************************************************************************************
KinematicChainWidget::KinematicChainWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
  : QWidget(parent), config_data_(config_data)
{
  // Basic widget container
  QVBoxLayout* layout = new QVBoxLayout();

  // Label ------------------------------------------------
  title_ = new QLabel("", this);  // specify the title from the parent widget
  QFont group_title_font("Arial", 12, QFont::Bold);
  title_->setFont(group_title_font);
  layout->addWidget(title_);

  // Create link tree ------------------------------------------------------
  link_tree_ = new QTreeWidget(this);
  link_tree_->setHeaderLabel("Robot Links");
  connect(link_tree_, SIGNAL(itemSelectionChanged()), this, SLOT(itemSelected()));
  layout->addWidget(link_tree_);

  // Create Grid Layout for form --------------------------------------------
  QGridLayout* form_grid = new QGridLayout();
  form_grid->setContentsMargins(20, 20, 20, 20);  // left top right bottom

  // Row 1: Base Link
  QLabel* base_link_label = new QLabel("Base Link", this);
  form_grid->addWidget(base_link_label, 0, 0, Qt::AlignRight);

  base_link_field_ = new QLineEdit(this);
  base_link_field_->setMinimumWidth(300);
  form_grid->addWidget(base_link_field_, 0, 1, Qt::AlignLeft);

  QPushButton* btn_base_link = new QPushButton("Choose Selected", this);
  connect(btn_base_link, SIGNAL(clicked()), this, SLOT(baseLinkTreeClick()));
  form_grid->addWidget(btn_base_link, 0, 2, Qt::AlignLeft);

  // Row 2: Tip Link
  QLabel* tip_link_label = new QLabel("Tip Link", this);
  form_grid->addWidget(tip_link_label, 1, 0, Qt::AlignRight);

  tip_link_field_ = new QLineEdit(this);
  tip_link_field_->setMinimumWidth(300);
  form_grid->addWidget(tip_link_field_, 1, 1, Qt::AlignLeft);

  QPushButton* btn_tip_link = new QPushButton("Choose Selected", this);
  connect(btn_tip_link, SIGNAL(clicked()), this, SLOT(tipLinkTreeClick()));
  form_grid->addWidget(btn_tip_link, 1, 2, Qt::AlignLeft);

  // Add form grid layout
  layout->addLayout(form_grid);

  // Bottom Controls ---------------------------------------------------------
  QHBoxLayout* controls_layout = new QHBoxLayout();

  // Expand/Contract controls
  QLabel* expand_controls = new QLabel(this);
  expand_controls->setText("<a href='expand'>Expand All</a> <a href='contract'>Collapse All</a>");
  connect(expand_controls, SIGNAL(linkActivated(const QString)), this, SLOT(alterTree(const QString)));
  controls_layout->addWidget(expand_controls);

  // Spacer
  QWidget* spacer = new QWidget(this);
  spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
  controls_layout->addWidget(spacer);

  // Save
  QPushButton* btn_save = new QPushButton("&Save", this);
  btn_save->setMaximumWidth(200);
  connect(btn_save, SIGNAL(clicked()), this, SIGNAL(doneEditing()));
  controls_layout->addWidget(btn_save);
  controls_layout->setAlignment(btn_save, Qt::AlignRight);

  // Cancel
  QPushButton* btn_cancel = new QPushButton("&Cancel", this);
  btn_cancel->setMaximumWidth(200);
  connect(btn_cancel, SIGNAL(clicked()), this, SIGNAL(cancelEditing()));
  controls_layout->addWidget(btn_cancel);
  controls_layout->setAlignment(btn_cancel, Qt::AlignRight);

  // Add layout
  layout->addLayout(controls_layout);

  // Finish Layout --------------------------------------------------
  this->setLayout(layout);

  // Remember that we have no loaded the chains yet
  kinematic_chain_loaded_ = false;
}

// ******************************************************************************************
// Load robot links into tree
// ******************************************************************************************
void KinematicChainWidget::setAvailable()
{
  // Only load the kinematic chain once
  if (kinematic_chain_loaded_)
    return;

  // Retrieve pointer to the shared kinematic model
  const robot_model::RobotModelConstPtr& model = config_data_->getRobotModel();

  // Get the root joint
  const robot_model::JointModel* root_joint = model->getRootJoint();

  addLinktoTreeRecursive(root_joint->getChildLinkModel(), NULL);

  // Remember that we have loaded the chain
  kinematic_chain_loaded_ = true;
}

// ******************************************************************************************
//
// ******************************************************************************************
void KinematicChainWidget::addLinktoTreeRecursive(const robot_model::LinkModel* link,
                                                  const robot_model::LinkModel* parent)
{
  // Create new tree item
  QTreeWidgetItem* new_item = new QTreeWidgetItem(link_tree_);

  // Add item to tree
  if (parent == NULL)
  {
    new_item->setText(0, link->getName().c_str());
    link_tree_->addTopLevelItem(new_item);
  }
  else
  {
    for (int i = 0; i < link_tree_->topLevelItemCount(); i++)
    {
      if (addLinkChildRecursive(link_tree_->topLevelItem(i), link, parent->getName()))
      {
        break;
      }
    }
  }
  for (size_t i = 0; i < link->getChildJointModels().size(); i++)
  {
    addLinktoTreeRecursive(link->getChildJointModels()[i]->getChildLinkModel(), link);
  }
}

// ******************************************************************************************
//
// ******************************************************************************************
bool KinematicChainWidget::addLinkChildRecursive(QTreeWidgetItem* parent, const robot_model::LinkModel* link,
                                                 const std::string& parent_name)
{
  if (parent->text(0).toStdString() == parent_name)
  {
    QTreeWidgetItem* new_item = new QTreeWidgetItem(parent);
    new_item->setText(0, link->getName().c_str());

    parent->addChild(new_item);
    return true;
  }
  else
  {
    for (int i = 0; i < parent->childCount(); i++)
    {
      if (addLinkChildRecursive(parent->child(i), link, parent_name))
      {
        return true;
      }
    }
  }

  return false;
}

// ******************************************************************************************
// Set the link field with previous value
// ******************************************************************************************
void KinematicChainWidget::setSelected(const std::string& base_link, const std::string& tip_link)
{
  base_link_field_->setText(QString(base_link.c_str()));
  tip_link_field_->setText(QString(tip_link.c_str()));
}

// ******************************************************************************************
// Choose the base link
// ******************************************************************************************
void KinematicChainWidget::baseLinkTreeClick()
{
  QTreeWidgetItem* item = link_tree_->currentItem();
  if (item != NULL)
  {
    base_link_field_->setText(item->text(0));
  }
}

// ******************************************************************************************
// Choose the tip link
// ******************************************************************************************
void KinematicChainWidget::tipLinkTreeClick()
{
  QTreeWidgetItem* item = link_tree_->currentItem();
  if (item != NULL)
  {
    tip_link_field_->setText(item->text(0));
  }
}

// ******************************************************************************************
// Expand/Collapse Tree
// ******************************************************************************************
void KinematicChainWidget::alterTree(const QString& link)
{
  if (link.contains("expand"))
    link_tree_->expandAll();
  else
    link_tree_->collapseAll();
}

// ******************************************************************************************
// Highlight the currently selected link
// ******************************************************************************************
void KinematicChainWidget::itemSelected()
{
  QTreeWidgetItem* item = link_tree_->currentItem();
  if (item != NULL)
  {
    Q_EMIT unhighlightAll();

    std::string name = item->text(0).toStdString();

    // Don't try to highlight empty links!
    if (name.empty())
      return;

    // Check that item is not empty
    Q_EMIT highlightLink(item->text(0).toStdString(), QColor(255, 0, 0));
  }
}
}