default_collisions_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 the 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 <QHBoxLayout>
#include <QMessageBox>
#include <QProgressDialog>
#include <QString>
#include <QFont>
#include <QApplication>
#include "default_collisions_widget.h"
#include <boost/unordered_map.hpp>
#include <boost/assign.hpp>
#include <ros/console.h>

namespace moveit_setup_assistant
{

const boost::unordered_map<moveit_setup_assistant::DisabledReason, const char*> longReasonsToString =
  boost::assign::map_list_of
  ( moveit_setup_assistant::NEVER, "Never in Collision" )
  ( moveit_setup_assistant::DEFAULT, "Collision by Default" )
  ( moveit_setup_assistant::ADJACENT, "Adjacent Links" )
  ( moveit_setup_assistant::ALWAYS, "Always in Collision" )
  ( moveit_setup_assistant::USER, "User Disabled" )
  ( moveit_setup_assistant::NOT_DISABLED, "");

class CheckboxSortWidgetItem : public QTableWidgetItem {
public:
  bool operator <(const QTableWidgetItem &other) const
  {
    return checkState() < other.checkState();
  }
};

// ******************************************************************************************
// User interface for editing the default collision matrix list in an SRDF
// ******************************************************************************************
DefaultCollisionsWidget::DefaultCollisionsWidget( QWidget *parent,
                                                  MoveItConfigDataPtr config_data )
  : SetupScreenWidget( parent ), config_data_(config_data)
{
  // Basic widget container
  layout_ = new QVBoxLayout( this );

  // Top Label Area ------------------------------------------------
  HeaderWidget *header = new HeaderWidget( "Optimize Self-Collision Checking",
                                           "The Default Self-Collision Matrix Generator will search for pairs of links on the robot that can safely be disabled from collision checking, decreasing motion planning processing time. These pairs of links are disabled when they are always in collision, never in collision, in collision in the robot's default position or when the links are adjacent to each other on the kinematic chain. Sampling density specifies how many random robot positions to check for self collision. Higher densities require more computation time.",
                                           this);
  layout_->addWidget( header );

  // Top Button Area -----------------------------------------------
  controls_box_ = new QGroupBox( this );
  layout_->addWidget( controls_box_ );
  QHBoxLayout *controls_box_layout = new QHBoxLayout( controls_box_ );
  controls_box_layout->setAlignment(Qt::AlignTop | Qt::AlignLeft);

  // Slider Label
  QLabel *density_left_label = new QLabel( this );
  density_left_label->setText("Sampling Density: Low");
  controls_box_layout->addWidget(density_left_label);

  // Slider
  density_slider_ = new QSlider( this );
  density_slider_->setTickPosition(QSlider::TicksBelow);
  density_slider_->setMinimum( 0 );
  density_slider_->setMaximum( 99 );
  density_slider_->setSingleStep( 10 );
  density_slider_->setPageStep( 50 );
  density_slider_->setSliderPosition( 9 ); // 10,000 is default
  density_slider_->setTickInterval( 10 );
  density_slider_->setOrientation( Qt::Horizontal );
  controls_box_layout->addWidget(density_slider_);
  connect(density_slider_, SIGNAL(valueChanged(int)), this, SLOT(changeDensityLabel(int)));

  // Slider Right Label
  QLabel *density_right_label = new QLabel( this );
  density_right_label->setText("High   ");
  controls_box_layout->addWidget(density_right_label);

  // Slider Value Label
  density_value_label_ = new QLabel( this );
  density_value_label_->setMinimumWidth(50);
  controls_box_layout->addWidget(density_value_label_);
  changeDensityLabel( density_slider_->value() ); // initialize label with value


  // Generate Button
  btn_generate_ = new QPushButton( this );
  btn_generate_->setText("&Generate Collision Matrix");
  btn_generate_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred );
  connect(btn_generate_, SIGNAL(clicked()), this, SLOT(generateCollisionTable()));
  layout_->addWidget(btn_generate_);
  layout_->setAlignment( btn_generate_, Qt::AlignRight );

  // Progress Bar Area ---------------------------------------------

  // Progress Label
  progress_label_ = new QLabel( this );
  progress_label_->setText("Generating Default Collision Matrix");
  progress_label_->hide();
  layout_->addWidget(progress_label_);

  // Progress Bar
  progress_bar_ = new QProgressBar( this );
  progress_bar_->setMaximum(100);
  progress_bar_->setMinimum(0);
  progress_bar_->hide(); // only show when computation begins
  layout_->addWidget(progress_bar_); //,Qt::AlignCenter);

  // Table Area --------------------------------------------

  // Table
  collision_table_ = new QTableWidget( this );
  collision_table_->setColumnCount(4);
  collision_table_->setSortingEnabled(true);
  collision_table_->setSelectionBehavior( QAbstractItemView::SelectRows );
  connect( collision_table_, SIGNAL( cellClicked( int, int ) ), this, SLOT( previewClicked( int, int ) ) );
  layout_->addWidget(collision_table_);

  QStringList header_list;
  header_list.append("Link A");
  header_list.append("Link B");
  header_list.append("Disabled");
  header_list.append("Reason To Disable");
  collision_table_->setHorizontalHeaderLabels(header_list);

  // Resize headers
  collision_table_->resizeColumnToContents(0);
  collision_table_->resizeColumnToContents(1);
  collision_table_->resizeColumnToContents(2);
  collision_table_->resizeColumnToContents(3);

  // Bottom Area ----------------------------------------
  controls_box_bottom_ = new QGroupBox( this );
  layout_->addWidget( controls_box_bottom_ );
  QHBoxLayout *controls_box_bottom_layout = new QHBoxLayout( controls_box_bottom_ );

  // Checkbox
  collision_checkbox_ = new QCheckBox( this );
  collision_checkbox_->setText("Show Non-Disabled Link Pairs");
  connect(collision_checkbox_, SIGNAL(toggled(bool)), this, SLOT(collisionCheckboxToggle()));
  controls_box_bottom_layout->addWidget(collision_checkbox_);

  fraction_label_ = new QLabel(this);
  fraction_label_->setText("Min. collisions for \"always\"-colliding pairs:");

  controls_box_bottom_layout->addWidget(fraction_label_);

  fraction_spinbox_ = new QSpinBox(this);
  fraction_spinbox_->setRange(1, 100);
  fraction_spinbox_->setValue(95);
  fraction_spinbox_->setSuffix("%");
  controls_box_bottom_layout->addWidget(fraction_spinbox_);

  controls_box_bottom_layout->setAlignment(collision_checkbox_, Qt::AlignLeft);

  setLayout(layout_);

  setWindowTitle("Default Collision Matrix");
}

// ******************************************************************************************
// Qt close event function for reminding user to save
// ******************************************************************************************
void DefaultCollisionsWidget::generateCollisionTable()
{
  // Confirm the user wants to overwrite the current disabled collisions
  if( !config_data_->srdf_->disabled_collisions_.empty() )
  {
    if( QMessageBox::question( this, "Confirm Disabled Collision Overwrite",
                               "Are you sure you want to overwrite the current default collisions matrix with a newly generated one?",
                               QMessageBox::Ok | QMessageBox::Cancel)
        == QMessageBox::Cancel )
    {
      return; // abort
    }
  }
  QApplication::processEvents(); // allow the progress bar to be shown
  progress_label_->setText("Computing default collision matrix for robot model...");

  // Disable controls on form
  disableControls(true);

  // Create a progress variable that will be shared with the compute_default_collisions tool and its threads
  // NOTE: be sure not to delete this variable until the subprograms have finished using it. Because of the simple
  // use case of this variable (1 thread writes to it, the parent process reads it) it was decided a boost shared
  // pointer is not necessary.
  unsigned int collision_progress = 0;
  progress_bar_->setValue(collision_progress);

  QApplication::processEvents(); // allow the progress bar to be shown

  // Create thread to do actual work
  boost::thread workerThread( boost::bind( &DefaultCollisionsWidget::generateCollisionTableThread,
                                           this, &collision_progress ));
  // Check interval
  boost::posix_time::seconds check_interval(1);

  // Continually loop until threaded computation is finished
  while( collision_progress < 100 )
  {
    // Set updated progress value.
    progress_bar_->setValue(collision_progress);

    // Allow GUI thread to do its stuff
    QApplication::processEvents();

    // 1 second sleep
    boost::this_thread::sleep(check_interval);
    //usleep(1000 * 1000);
  }

  // Wait for thread to finish
  workerThread.join();

  // Load the results into the GUI
  loadCollisionTable();

  // Hide the progress bar
  disableControls(false); // enable everything else
}

// ******************************************************************************************
// The thread that is called to allow the GUI to update. Calls an external function to do calcs
// ******************************************************************************************
void DefaultCollisionsWidget::generateCollisionTableThread( unsigned int *collision_progress )
{
  unsigned int num_trials = density_slider_->value() * 1000 + 1000; // scale to trials amount
  double min_frac = (double)fraction_spinbox_->value() / 100.0;

  const bool verbose = true; // Output benchmarking and statistics
  const bool include_never_colliding = true;

  // clear previously loaded collision matrix entries
  config_data_->getPlanningScene()->getAllowedCollisionMatrixNonConst().clear();

  // Find the default collision matrix - all links that are allowed to collide
  link_pairs_ =
    moveit_setup_assistant::computeDefaultCollisions( config_data_->getPlanningScene(),
                                                      collision_progress, include_never_colliding, num_trials,
                                                      min_frac, verbose);

  // Copy data changes to srdf_writer object
  linkPairsToSRDF();

  // End the progress bar loop
  *collision_progress = 100;

  ROS_INFO_STREAM("Thread complete " << link_pairs_.size());
}

// ******************************************************************************************
// Displays data in the link_pairs_ data structure into a QtTableWidget
// ******************************************************************************************
void DefaultCollisionsWidget::loadCollisionTable()
{
  int row = 0;
  int progress_counter = 0;

  // Show Progress Bar
  progress_bar_->setValue(0);

  QApplication::processEvents(); // allow the progress bar to be shown
  progress_label_->setText("Loading table...");

  // Setup Collision Table
  collision_table_->setUpdatesEnabled(false); // prevent table from updating until we are completely done
  collision_table_->setDisabled(true); // make sure we disable it so that the cellChanged event is not called
  collision_table_->clearContents();

  // Check if there are no disabled collisions (unprobable?)
  if(link_pairs_.empty())
  {
    collision_table_->setRowCount(1);
    QTableWidgetItem* no_collide = new QTableWidgetItem("No Link Pairs Of This Kind");
    collision_table_->setItem(0, 0, no_collide);
  }
  else
  {
    // The table will be populated, so indicate it on the button
    btn_generate_->setText("Regenerate Default Collision Matrix");
  }


  // Intially set the table to be worst-case scenario of every possible element pair
  collision_table_->setRowCount( link_pairs_.size() );

  for ( moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin();
        pair_it != link_pairs_.end();
        ++pair_it)
  {
    // Add link pair row if 1) it is disabled from collision checking or 2) the SHOW ALL LINK PAIRS checkbox is checked
    if( pair_it->second.disable_check || collision_checkbox_->isChecked() )
    {

      // Create row elements
      QTableWidgetItem* linkA = new QTableWidgetItem( pair_it->first.first.c_str() );
      linkA->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);

      QTableWidgetItem* linkB = new QTableWidgetItem( pair_it->first.second.c_str() );
      linkB->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);

      CheckboxSortWidgetItem* disable_check = new CheckboxSortWidgetItem( );
      disable_check->setFlags(Qt::ItemIsEnabled | Qt::ItemIsUserCheckable | Qt::ItemIsSelectable);
      if( pair_it->second.disable_check ) // Checked means no collision checking
        disable_check->setCheckState(Qt::Checked);
      else
        disable_check->setCheckState(Qt::Unchecked);

      QTableWidgetItem* reason = new QTableWidgetItem( longReasonsToString.at( pair_it->second.reason ) );
      reason->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);

      // Insert row elements into collision table
      collision_table_->setItem( row, 0, linkA);
      collision_table_->setItem( row, 1, linkB);
      collision_table_->setItem( row, 2, disable_check);
      collision_table_->setItem( row, 3, reason);

      // Increment row count
      ++row;
    }

    ++progress_counter; // for calculating progress bar

    if( progress_counter % 200 == 0 )
    {
      // Update Progress Bar
      progress_bar_->setValue( progress_counter * 100 / link_pairs_.size() );
      QApplication::processEvents(); // allow the progress bar to be shown
    }

  }

  // Reduce the table size to only the number of used rows.
  collision_table_->setRowCount( row );

  // Resize headers. The hiding is a hack so that it resizes correctly
  collision_table_->setVisible(false);
  collision_table_->resizeColumnToContents(0);
  collision_table_->resizeColumnToContents(1);
  collision_table_->resizeColumnToContents(2);
  collision_table_->resizeColumnToContents(3);
  collision_table_->setVisible(true);

  collision_table_->setUpdatesEnabled(true); // prevent table from updating until we are completely done
}

// ******************************************************************************************
// GUI func for showing sampling density amount
// ******************************************************************************************
void DefaultCollisionsWidget::changeDensityLabel(int value)
{
  density_value_label_->setText( QString::number( value*1000 + 1000) ); //.append(" samples") );
}

// ******************************************************************************************
// Helper function to disable parts of GUI during computation
// ******************************************************************************************
void DefaultCollisionsWidget::disableControls(bool disable)
{
  controls_box_->setDisabled( disable );
  collision_table_->setDisabled( disable );
  collision_checkbox_->setDisabled( disable );

  if( disable )
  {
    progress_bar_->show(); // only show when computation begins
    progress_label_->show();
  }
  else
  {
    progress_label_->hide();
    progress_bar_->hide();
  }

  QApplication::processEvents(); // allow the progress bar to be shown
}

// ******************************************************************************************
// Changes the table to show or hide collisions that are not disabled (that have collision checking enabled)
// ******************************************************************************************
void DefaultCollisionsWidget::collisionCheckboxToggle()
{
  // Show Progress bar
  disableControls( true );

  // Now update collision table with updates
  loadCollisionTable();

  // Hide Progress bar
  disableControls( false );
}

// ******************************************************************************************
// Called when user changes data in table, really just the checkbox
// ******************************************************************************************
void DefaultCollisionsWidget::toggleCheckBox(int j, int i) // these are flipped on purpose
{
  // Only accept cell changes if table is enabled, otherwise it is this program making changes
  if( collision_table_->isEnabled() )
  {
    // Make sure change is the checkbox column
    if( i == 2 )
    {

      // Convert row to pair
      std::pair<std::string, std::string> link_pair;
      link_pair.first = collision_table_->item(j, 0)->text().toStdString();
      link_pair.second = collision_table_->item(j, 1)->text().toStdString();

      // Get the state of checkbox
      bool check_state = collision_table_->item(j, 2)->checkState();


      // Check if the checkbox state has changed from original value
      if( link_pairs_[ link_pair ].disable_check != check_state )
      {

        // Save the change
        link_pairs_[ link_pair ].disable_check = check_state;

        // Handle USER Reasons: 1) pair is disabled by user
        if( link_pairs_[ link_pair ].disable_check == true &&
            link_pairs_[ link_pair ].reason == moveit_setup_assistant::NOT_DISABLED )
        {
          link_pairs_[ link_pair ].reason = moveit_setup_assistant::USER;

          // Change Reason in Table
          collision_table_->item(j, 3)->setText( longReasonsToString.at( link_pairs_[ link_pair ].reason ) );
        }
        // Handle USER Reasons: 2) pair was disabled by user and now is enabled (not checked)
        else if( link_pairs_[ link_pair ].disable_check == false &&
                 link_pairs_[ link_pair ].reason == moveit_setup_assistant::USER )
        {
          link_pairs_[ link_pair ].reason = moveit_setup_assistant::NOT_DISABLED;

          // Change Reason in Table
          collision_table_->item(j, 3)->setText( "" );
        }

      }

      // Copy data changes to srdf_writer object
      linkPairsToSRDF();
    }
  }
}

// ******************************************************************************************
// Output Link Pairs to SRDF Format and update the collision matrix
// ******************************************************************************************
void DefaultCollisionsWidget::linkPairsToSRDF()
{
  // reset the data in the SRDF Writer class
  config_data_->srdf_->disabled_collisions_.clear();

  // Create temp disabled collision
  srdf::Model::DisabledCollision dc;

  // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
  for ( moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin();
        pair_it != link_pairs_.end(); ++pair_it)
  {
    // Only copy those that are actually disabled
    if( pair_it->second.disable_check )
    {
      dc.link1_ = pair_it->first.first;
      dc.link2_ = pair_it->first.second;
      dc.reason_ = moveit_setup_assistant::disabledReasonToString( pair_it->second.reason );
      config_data_->srdf_->disabled_collisions_.push_back( dc );
    }
  }

  // Update collision_matrix for robot pose's use
  config_data_->loadAllowedCollisionMatrix();

}

// ******************************************************************************************
// Load Link Pairs from SRDF Format
// ******************************************************************************************
void DefaultCollisionsWidget::linkPairsFromSRDF()
{
  // Clear all the previous data in the compute_default_collisions tool
  link_pairs_.clear();

  // Create new instance of planning scene using pointer
  planning_scene::PlanningScenePtr scene = config_data_->getPlanningScene()->diff();

  // Populate link_pairs_ list with every possible n choose 2 combination of links
  moveit_setup_assistant::computeLinkPairs(*scene, link_pairs_);

  // Create temp link pair data struct
  moveit_setup_assistant::LinkPairData link_pair_data;
  std::pair<std::string, std::string> link_pair;

  // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created
  for( std::vector<srdf::Model::DisabledCollision>::const_iterator collision_it =
         config_data_->srdf_->disabled_collisions_.begin();
       collision_it != config_data_->srdf_->disabled_collisions_.end(); ++collision_it )
  {
    // Set the link names
    link_pair.first = collision_it->link1_;
    link_pair.second = collision_it->link2_;

    // Set the link meta data
    link_pair_data.reason = moveit_setup_assistant::disabledReasonFromString( collision_it->reason_ );
    link_pair_data.disable_check = true; // disable checking the collision btw the 2 links

    // Insert into map
    link_pairs_[ link_pair ] = link_pair_data;
  }
}

// ******************************************************************************************
// Preview whatever element is selected
// ******************************************************************************************
void DefaultCollisionsWidget::previewClicked( int row, int column )
{
  // Get list of all selected items
  QList<QTableWidgetItem*> selected = collision_table_->selectedItems();

  // Check that an element was selected
  if( !selected.size() )
    return;

  // Unhighlight all links
  Q_EMIT unhighlightAll();

  // Highlight link
  Q_EMIT highlightLink( selected[0]->text().toStdString() );
  Q_EMIT highlightLink( selected[1]->text().toStdString() );
}

// ******************************************************************************************
// Called when setup assistant navigation switches to this screen
// ******************************************************************************************
void DefaultCollisionsWidget::focusGiven()
{
  // Convert the SRDF data to LinkPairData format
  linkPairsFromSRDF();

  // Load the data to the table
  loadCollisionTable();

  // Enable the table
  disableControls( false );
}


} // namespace