tab_states_and_goals.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: Mario Prats, Ioan Sucan */

#include <main_window.h>
#include <job_processing.h>
#include <benchmark_processing_thread.h>

#include <moveit/kinematic_constraints/utils.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/warehouse/state_storage.h>
#include <moveit/benchmarks/benchmark_execution.h>
#include <moveit/planning_interface/planning_interface.h>

#include <pluginlib/class_loader.h>

#include <rviz/display_context.h>
#include <rviz/window_manager_interface.h>

#include <eigen_conversions/eigen_msg.h>

#include <QMessageBox>
#include <QInputDialog>
#include <QFileDialog>
#include <QProgressDialog>

#include <boost/math/constants/constants.hpp>

namespace benchmark_tool
{

void MainWindow::createGoalAtPose(const std::string &name, const Eigen::Affine3d &pose)
{
  goals_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(name, pose));

  geometry_msgs::Pose marker_pose;
  tf::poseEigenToMsg(pose * goal_offset_, marker_pose);
  static const float marker_scale = 0.15;

  GripperMarkerPtr goal_pose(new GripperMarker(scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_, name, scene_display_->getRobotModel()->getModelFrame(),
                                               robot_interaction_->getActiveEndEffectors()[0], marker_pose, marker_scale, GripperMarker::NOT_TESTED));
  goal_poses_.insert(GoalPosePair(name,  goal_pose));

  // Connect signals
  goal_pose->connect(this, SLOT( goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &) ));

  //If connected to a database, save all the goals back to the database
  if (constraints_storage_)
  {
    saveGoalsToDB();
  }
}

void MainWindow::saveGoalsToDB()
{
  if (constraints_storage_)
  {
    //Convert all goal pose markers into constraints and store them
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      moveit_msgs::Constraints constraints;
      constraints.name = it->first;

      shape_msgs::SolidPrimitive sp;
      sp.type = sp.BOX;
      sp.dimensions.resize(3, std::numeric_limits<float>::epsilon() * 10.0);

      moveit_msgs::PositionConstraint pc;
      pc.header.frame_id = scene_display_->getRobotModel()->getModelFrame();
      pc.link_name = robot_interaction_->getActiveEndEffectors()[0].parent_link; // TODO this is hacky i think
      pc.constraint_region.primitives.push_back(sp);
      geometry_msgs::Pose posemsg;
      it->second->getPosition(posemsg.position);
      posemsg.orientation.x = 0.0;
      posemsg.orientation.y = 0.0;
      posemsg.orientation.z = 0.0;
      posemsg.orientation.w = 1.0;
      pc.constraint_region.primitive_poses.push_back(posemsg);
      pc.weight = 1.0;
      constraints.position_constraints.push_back(pc);

      moveit_msgs::OrientationConstraint oc;
      oc.header.frame_id = scene_display_->getRobotModel()->getModelFrame();
      oc.link_name = robot_interaction_->getActiveEndEffectors()[0].parent_link; // TODO this is hacky i think
      it->second->getOrientation(oc.orientation);
      oc.absolute_x_axis_tolerance = oc.absolute_y_axis_tolerance =
        oc.absolute_z_axis_tolerance = std::numeric_limits<float>::epsilon() * 10.0;
      oc.weight = 1.0;
      constraints.orientation_constraints.push_back(oc);

      try
      {
        constraints_storage_->addConstraints(constraints);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("Cannot save constraint: %s", ex.what());
      }
    }
  }
  else
  {
    QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::createGoalPoseButtonClicked(void)
{
  std::stringstream ss;

  {
    const planning_scene_monitor::LockedPlanningSceneRO &ps = scene_display_->getPlanningSceneRO();
    if ( ! ps || robot_interaction_->getActiveEndEffectors().empty() )
    {
      if ( ! ps ) ROS_ERROR("Not planning scene");
      if ( robot_interaction_->getActiveEndEffectors().empty() ) ROS_ERROR("No end effector");
      return;
    }
    else
      ss << ps->getName().c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();
  }

  bool ok = false;
  QString text = QInputDialog::getText(this, tr("Choose a name"),
                                       tr("Goal pose name:"), QLineEdit::Normal,
                                       QString(ss.str().c_str()), &ok);

  std::string name;
  if (ok)
  {
    if ( ! text.isEmpty() )
    {
      name = text.toStdString();
      if (goal_poses_.find(name) != goal_poses_.end())
        QMessageBox::warning(this, "Name already exists", QString("The name '").append(name.c_str()).
                             append("' already exists. Not creating goal."));
      else
      {
        //Create the new goal pose at the current eef pose, and attach an interactive marker to it
        scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
        Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(robot_interaction_->getActiveEndEffectors()[0].parent_link);
        createGoalAtPose(name, tip_pose);
      }
    }
    else
      QMessageBox::warning(this, "Goal not created", "Cannot use an empty name for a new goal pose.");
  }

  populateGoalPosesList();
}

void MainWindow::showBBoxGoalsDialog()
{
  std::string goals_base_name;
  {
    const planning_scene_monitor::LockedPlanningSceneRO &ps = scene_display_->getPlanningSceneRO();
    if ( ! ps || robot_interaction_->getActiveEndEffectors().empty() )
    {
      if ( ! ps ) ROS_ERROR("No planning scene");
      if ( robot_interaction_->getActiveEndEffectors().empty() ) ROS_ERROR("No end effector");
      return;
    }
    goals_base_name = ps->getName() + "_pose_";
  }

  bbox_dialog_ui_.base_name_text->setText(QString(goals_base_name.c_str()));

  bbox_dialog_->exec();
}

void MainWindow::createBBoxGoalsButtonClicked(void)
{
  bbox_dialog_->close();

  double minx = bbox_dialog_ui_.center_x_text->text().toDouble() - bbox_dialog_ui_.size_x_text->text().toDouble() / 2.0;
  double maxx = bbox_dialog_ui_.center_x_text->text().toDouble() + bbox_dialog_ui_.size_x_text->text().toDouble() / 2.0;
  double stepx = (maxx - minx) / std::max<double>(bbox_dialog_ui_.ngoals_x_text->text().toDouble() - 1, 1);
  double miny = bbox_dialog_ui_.center_y_text->text().toDouble() - bbox_dialog_ui_.size_y_text->text().toDouble() / 2.0;
  double maxy = bbox_dialog_ui_.center_y_text->text().toDouble() + bbox_dialog_ui_.size_y_text->text().toDouble() / 2.0;
  double stepy = (maxy - miny) / std::max<double>(bbox_dialog_ui_.ngoals_y_text->text().toDouble() - 1, 1);
  double minz = bbox_dialog_ui_.center_z_text->text().toDouble() - bbox_dialog_ui_.size_z_text->text().toDouble() / 2.0;
  double maxz = bbox_dialog_ui_.center_z_text->text().toDouble() + bbox_dialog_ui_.size_z_text->text().toDouble() / 2.0;
  double stepz = (maxz - minz) / std::max<double>(bbox_dialog_ui_.ngoals_z_text->text().toDouble() - 1, 1);

  Eigen::Affine3d goal_pose;
  goal_pose.setIdentity();
  for (std::size_t x = 0; x < bbox_dialog_ui_.ngoals_x_text->text().toShort(); ++x)
    for (std::size_t y = 0; y < bbox_dialog_ui_.ngoals_y_text->text().toShort(); ++y)
      for (std::size_t z = 0; z < bbox_dialog_ui_.ngoals_z_text->text().toShort(); ++z)
      {
        goal_pose(0,3) = minx + x * stepx;
        goal_pose(1,3) = miny + y * stepy;
        goal_pose(2,3) = minz + z * stepz;

        std::stringstream ss;
        ss << bbox_dialog_ui_.base_name_text->text().toStdString() << std::setfill('0') << std::setw(4) << goal_poses_.size();

        createGoalAtPose(ss.str(), goal_pose);
      }

  populateGoalPosesList();
}

void MainWindow::removeSelectedGoalsButtonClicked(void)
{
  QList<QListWidgetItem*> found_items = ui_.goal_poses_list->selectedItems();
  for ( unsigned int i = 0 ; i < found_items.size() ; i++ )
  {
    goal_poses_.erase(found_items[i]->text().toStdString());
  }
  populateGoalPosesList();
}

void MainWindow::removeAllGoalsButtonClicked(void)
{
  goal_poses_.clear();
  goals_initial_pose_.clear();
  populateGoalPosesList();
}

void MainWindow::loadGoalsFromDBButtonClicked(void)
{
  //Get all the constraints from the database, convert to goal pose markers
  if (constraints_storage_ && !robot_interaction_->getActiveEndEffectors().empty())
  {
    //First clear the current list
    removeAllGoalsButtonClicked();

    std::vector<std::string> names;
    try
    {
      constraints_storage_->getKnownConstraints(ui_.load_poses_filter_text->text().toStdString(), names);
    }
    catch (std::runtime_error &ex)
    {
      QMessageBox::warning(this, "Cannot query the database", QString("Wrongly formatted regular expression for goal poses: ").append(ex.what()));
      return;
    }

    for (unsigned int i = 0 ; i < names.size() ; i++)
    {
      //Create a goal pose marker
      moveit_warehouse::ConstraintsWithMetadata c;
      bool got_constraint = false;
      try
      {
        got_constraint = constraints_storage_->getConstraints(c, names[i]);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("%s", ex.what());
      }
      if (!got_constraint)
        continue;

      if ( c->position_constraints.size() > 0 && c->position_constraints[0].constraint_region.primitive_poses.size() > 0 && c->orientation_constraints.size() > 0 )
      {
        //Overwrite if exists. TODO: Ask the user before overwriting? copy the existing one with another name before?
        if ( goal_poses_.find(c->name) != goal_poses_.end() )
        {
          goal_poses_.erase(c->name);
        }
        geometry_msgs::Pose shape_pose;
        shape_pose.position = c->position_constraints[0].constraint_region.primitive_poses[0].position;
        shape_pose.orientation = c->orientation_constraints[0].orientation;

        Eigen::Affine3d shape_pose_eigen;
        tf::poseMsgToEigen(shape_pose, shape_pose_eigen);
        goals_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(c->name, shape_pose_eigen));

        tf::poseEigenToMsg(shape_pose_eigen * goal_offset_, shape_pose);

        static const float marker_scale = 0.15;
        GripperMarkerPtr goal_pose(new GripperMarker(scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_, c->name, scene_display_->getRobotModel()->getModelFrame(),
                                robot_interaction_->getActiveEndEffectors()[0], shape_pose, marker_scale, GripperMarker::NOT_TESTED, false,
                                ui_.show_x_checkbox->isChecked(), ui_.show_y_checkbox->isChecked(), ui_.show_z_checkbox->isChecked()));
        // Connect signals
        goal_pose->connect(this, SLOT( goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &) ));

        goal_poses_.insert(GoalPosePair(c->name, goal_pose));
      }
    }
    populateGoalPosesList();
   }
  else
  {
    if (!constraints_storage_)
      QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::saveGoalsOnDBButtonClicked(void)
{
  saveGoalsToDB();
}

void MainWindow::deleteGoalsOnDBButtonClicked(void)
{
  if (constraints_storage_)
  {
    //Warn the user
    QMessageBox msgBox;
    msgBox.setText("All the selected items will be removed from the database");
    msgBox.setInformativeText("Do you want to continue?");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::No);
    int ret = msgBox.exec();

    switch (ret)
    {
      case QMessageBox::Yes:
      {
        //Go through the list of goal poses, and delete those selected
        QList<QListWidgetItem*> found_items = ui_.goal_poses_list->selectedItems();
        for ( std::size_t i = 0 ; i < found_items.size() ; i++ )
        {
          try
          {
            constraints_storage_->removeConstraints(found_items[i]->text().toStdString());
          }
          catch (std::runtime_error &ex)
          {
            ROS_ERROR("%s", ex.what());
          }
        }
        break;
      }
    }
  }
  removeSelectedGoalsButtonClicked();
}


void MainWindow::loadStatesFromDBButtonClicked(void)
{
  //Get all the start states from the database
  if (robot_state_storage_)
  {
    //First clear the current list
    removeAllStatesButtonClicked();

    std::vector<std::string> names;
    try
    {
      robot_state_storage_->getKnownRobotStates(ui_.load_states_filter_text->text().toStdString(), names);
    }
    catch (std::runtime_error &ex)
    {
      QMessageBox::warning(this, "Cannot query the database", QString("Wrongly formatted regular expression for goal poses: ").append(ex.what()));
      return;
    }

    for ( unsigned int i = 0 ; i < names.size() ; i++ )
    {
      moveit_warehouse::RobotStateWithMetadata rs;
      bool got_state = false;
      try
      {
        got_state = robot_state_storage_->getRobotState(rs, names[i]);
      }
      catch(std::runtime_error &ex)
      {
        ROS_ERROR("%s", ex.what());
      }
      if (!got_state)
        continue;

      //Overwrite if exists.
      if (start_states_.find(names[i]) != start_states_.end())
      {
        start_states_.erase(names[i]);
      }

      //Store the current start state
      start_states_.insert(StartStatePair(names[i], StartStatePtr(new StartState(*rs))));
    }
    populateStartStatesList();
  }
  else
  {
    QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::saveStatesOnDBButtonClicked(void)
{
  if (robot_state_storage_)
  {
    //Store all start states
    for (StartStateMap::iterator it = start_states_.begin(); it != start_states_.end(); ++it)
      try
      {
        robot_state_storage_->addRobotState(it->second->state_msg, it->first);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("Cannot save robot state: %s", ex.what());
      }
  }
  else
  {
    QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::deleteStatesOnDBButtonClicked(void)
{
  if (robot_state_storage_)
  {
    //Warn the user
    QMessageBox msgBox;
    msgBox.setText("All the selected items will be removed from the database");
    msgBox.setInformativeText("Do you want to continue?");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::No);
    int ret = msgBox.exec();

    switch (ret)
    {
      case QMessageBox::Yes:
      {
        QList<QListWidgetItem*> found_items =  ui_.start_states_list->selectedItems();
        for (unsigned int i = 0; i < found_items.size() ; ++i)
        {
          try
          {
            robot_state_storage_->removeRobotState(found_items[i]->text().toStdString());
          }
          catch (std::runtime_error &ex)
          {
            ROS_ERROR("%s", ex.what());
          }
        }
        break;
      }
    }
  }
  removeSelectedStatesButtonClicked();
}

void MainWindow::visibleAxisChanged(int state)
{
  if ( ! robot_interaction_->getActiveEndEffectors().empty() )
  {
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      if (it->second->isVisible())
      {
        it->second->setAxisVisibility(ui_.show_x_checkbox->isChecked(), ui_.show_y_checkbox->isChecked(), ui_.show_z_checkbox->isChecked());
      }
    }
  }
}

void MainWindow::populateGoalPosesList(void)
{
  ui_.goal_poses_list->clear();
  for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
  {
    QListWidgetItem *item = new QListWidgetItem(QString(it->first.c_str()));
    ui_.goal_poses_list->addItem(item);
    if (! it->second->isVisible())
    {
      item->setBackground(QBrush(Qt::Dense4Pattern));
    }
    else if (it->second->isSelected())
    {
      //If selected, highlight in the list
      item->setSelected(true);
    }
  }
}

void MainWindow::switchGoalVisibilityButtonClicked(void)
{
  QList<QListWidgetItem*> selection = ui_.goal_poses_list->selectedItems();
  for (std::size_t i = 0; i < selection.size() ; ++i)
  {
    std::string name = selection[i]->text().toStdString();
    if (goal_poses_[name]->isVisible())
    {
      //Set invisible
      goal_poses_[name]->hide();
      selection[i]->setBackground(QBrush(Qt::Dense4Pattern));
    }
    else
    {
      //Set visible
      goal_poses_[name]->show(scene_display_->getSceneNode(), visualization_manager_);
      selection[i]->setBackground(QBrush(Qt::NoBrush));
    }
  }
}

void MainWindow::goalPoseSelectionChanged(void)
{
  for (unsigned int i = 0; i < ui_.goal_poses_list->count() ; ++i)
  {
    QListWidgetItem *item = ui_.goal_poses_list->item(i);
    std::string name = item->text().toStdString();
    if ( goal_poses_.find(name) != goal_poses_.end() &&
        ( (item->isSelected() && ! goal_poses_[name]->isSelected() )
            || ( ! item->isSelected() && goal_poses_[name]->isSelected() )))
      switchGoalPoseMarkerSelection(name);
  }
}

void MainWindow::goalPoseDoubleClicked(QListWidgetItem * item)
{
  JobProcessing::addBackgroundJob(boost::bind(&MainWindow::computeGoalPoseDoubleClicked, this, item));
}

void MainWindow::computeGoalPoseDoubleClicked(QListWidgetItem * item)
{
  if ( ! robot_interaction_ || robot_interaction_->getActiveEndEffectors().empty() )
    return;

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

  //Switch the marker color to processing color while processing
  JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, item_text, GripperMarker::PROCESSING));

  checkIfGoalReachable(item_text, true);
}

/* Receives feedback from the interactive marker attached to a goal pose */
void MainWindow::goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
{
  if (feedback.event_type == feedback.BUTTON_CLICK)
  {
    //Unselect all but the clicked one. Needs to be in order, first unselect, then select.
    QListWidgetItem *item = 0;
    for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
    {
      item = ui_.goal_poses_list->item(i);
      if ( item->text().toStdString() != feedback.marker_name)
        JobProcessing::addMainLoopJob(boost::bind(&MainWindow::selectItemJob, this, item, false));
    }

    for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
    {
      item = ui_.goal_poses_list->item(i);
      if (item->text().toStdString() == feedback.marker_name)
        JobProcessing::addMainLoopJob(boost::bind(&MainWindow::selectItemJob, this, item, true));
    }
  }
  else if (feedback.event_type == feedback.MOUSE_DOWN)
  {
    // First check to see if this mouse_down is happening on an
    // already-selected goal pose.  A mouse_down event is sent even if
    // the gesture turns out to be nothing but a click.
    bool this_goal_already_selected = false;
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      if (it->second->isSelected() &&
          it->second->isVisible() &&
          it->second->imarker->getName() == feedback.marker_name)
      {
        this_goal_already_selected = true;
      }
    }

    // If this is a mouse-down on an already-selected goal pose, we
    // want to initialize dragging all selected goal poses at the same
    // time.
    if(this_goal_already_selected) {
      //Store current poses
      goals_dragging_initial_pose_.clear();
      for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
      {
        if (it->second->isSelected() && it->second->isVisible())
        {
          Eigen::Affine3d pose(Eigen::Quaterniond(it->second->imarker->getOrientation().w, it->second->imarker->getOrientation().x,
                                                  it->second->imarker->getOrientation().y, it->second->imarker->getOrientation().z));
          pose(0,3) = it->second->imarker->getPosition().x;
          pose(1,3) = it->second->imarker->getPosition().y;
          pose(2,3) = it->second->imarker->getPosition().z;
          goals_dragging_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(it->second->imarker->getName(), pose));

          if (it->second->imarker->getName() == feedback.marker_name)
            drag_initial_pose_ = pose;
        }
      }
      goal_pose_dragging_=true;
    }
  }
  else if (feedback.event_type == feedback.POSE_UPDATE && goal_pose_dragging_)
  {
    //Compute displacement from stored pose, and apply to the rest of selected markers
    Eigen::Affine3d current_pose_eigen;
    tf::poseMsgToEigen(feedback.pose, current_pose_eigen);

    Eigen::Affine3d current_wrt_initial = drag_initial_pose_.inverse() * current_pose_eigen;

    //Display the pose in the ui
    Eigen::Vector3d v = current_pose_eigen.linear().eulerAngles(0,1,2);
    setStatusFromBackground(STATUS_INFO, QString().sprintf(
        "%.2f %.2f %.2f    %.2f %.2f %.2f",
        current_pose_eigen(0,3),
        current_pose_eigen(1,3),
        current_pose_eigen(2,3),
        v(0) * 180.0 / boost::math::constants::pi<double>(),
        v(1) * 180.0 / boost::math::constants::pi<double>(),
        v(2) * 180.0 / boost::math::constants::pi<double>()));

    //Update the rest of selected markers
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end() ; ++it)
    {
      if (it->second->isVisible() && it->second->imarker->getName() != feedback.marker_name && it->second->isSelected())
      {
        visualization_msgs::InteractiveMarkerPose impose;

        Eigen::Affine3d newpose = drag_initial_pose_ * current_wrt_initial * drag_initial_pose_.inverse() * goals_dragging_initial_pose_[it->second->imarker->getName()];
        tf::poseEigenToMsg(newpose, impose.pose);

        it->second->imarker->setPose(Ogre::Vector3(impose.pose.position.x, impose.pose.position.y, impose.pose.position.z),
                                      Ogre::Quaternion(impose.pose.orientation.w, impose.pose.orientation.x, impose.pose.orientation.y, impose.pose.orientation.z), "");
      }
    }
  } else if (feedback.event_type == feedback.MOUSE_UP)
  {
    goal_pose_dragging_ = false;
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalInCollision, this, feedback.marker_name));
  }
}

void MainWindow::checkGoalsReachable(void)
{
  for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalReachable, this, it->first, false));
}

void MainWindow::checkGoalsInCollision(void)
{
  for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalInCollision, this, it->first));
}

void MainWindow::checkIfGoalReachable(const std::string &goal_name, bool update_if_reachable)
{
  if ( goal_poses_.find(goal_name) == goal_poses_.end())
    return;

  if ( ! goal_poses_[goal_name]->isVisible())
    return;

  const boost::shared_ptr<rviz::InteractiveMarker> &imarker = goal_poses_[goal_name]->imarker;

  geometry_msgs::Pose current_pose_msg;
  current_pose_msg.position.x = imarker->getPosition().x;
  current_pose_msg.position.y = imarker->getPosition().y;
  current_pose_msg.position.z = imarker->getPosition().z;
  current_pose_msg.orientation.x = imarker->getOrientation().x;
  current_pose_msg.orientation.y = imarker->getOrientation().y;
  current_pose_msg.orientation.z = imarker->getOrientation().z;
  current_pose_msg.orientation.w = imarker->getOrientation().w;

  // Call to IK
  setStatusFromBackground(STATUS_INFO, "Computing inverse kinematics...");
  robot_state::RobotState ks(scene_display_->getPlanningSceneRO()->getCurrentState());
  static const int ik_attempts = 5;
  static const float ik_timeout = 0.2;
  bool feasible = robot_interaction_->updateState(ks,
                                                  robot_interaction_->getActiveEndEffectors()[0],
                                                  current_pose_msg, ik_attempts, ik_timeout);
  if (feasible)
  {
    setStatusFromBackground(STATUS_INFO, "Updating state...");
    if (update_if_reachable)
    {
      scene_display_->getPlanningSceneRW()->setCurrentState(ks);
      scene_display_->queueRenderSceneGeometry();
    }

    setStatusFromBackground(STATUS_INFO, "Updating marker...");
    //Switch the marker color to reachable
    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                  GripperMarker::REACHABLE));
  }
  else
  {
    //Switch the marker color to not-reachable
    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                  GripperMarker::NOT_REACHABLE));
  }
  setStatusFromBackground(STATUS_INFO, "");
}

bool MainWindow::isGroupCollidingWithWorld(robot_state::RobotState& robot_state, const std::string& group_name)
{
  collision_detection::AllowedCollisionMatrix acm( scene_display_->getPlanningSceneRO()->getAllowedCollisionMatrix() );
  // get link names in group_name
  const std::vector<std::string>& group_link_names =
    scene_display_->getRobotModel()->getJointModelGroup(group_name)->getLinkModelNamesWithCollisionGeometry();

  // Create a set of links which is all links minus links in the group.
  const std::vector<std::string>& all_links = scene_display_->getRobotModel()->getLinkModelNames();
  std::set<std::string> link_set(all_links.begin(), all_links.end());
  for(size_t i = 0; i < group_link_names.size(); i++ )
  {
    link_set.erase(group_link_names[i]);
  }

  // for each link name in the set,
  for(std::set<std::string>::const_iterator it = link_set.begin(); it != link_set.end(); it++ )
  {
    // allow collisions with link.
    acm.setEntry(*it, true);
  }

  // call checkCollision();
  collision_detection::CollisionRequest req;
  req.verbose = false;
  collision_detection::CollisionResult  res;
  scene_display_->getPlanningSceneRO()->checkCollision(req, res, robot_state, acm);

  // return result boolean.
  return res.collision;
}

void MainWindow::checkIfGoalInCollision(const std::string & goal_name)
{
  if ( goal_poses_.find(goal_name) == goal_poses_.end())
    return;

  // Check if the end-effector is in collision at the current pose
  if ( ! goal_poses_[goal_name]->isVisible())
    return;

  const robot_interaction::RobotInteraction::EndEffector &eef = robot_interaction_->getActiveEndEffectors()[0];

  const boost::shared_ptr<rviz::InteractiveMarker> &im = goal_poses_[goal_name]->imarker;
  Eigen::Affine3d marker_pose_eigen;
  goal_poses_[goal_name]->getPose(marker_pose_eigen);

  robot_state::RobotState ks(scene_display_->getPlanningSceneRO()->getCurrentState());
  ks.updateStateWithLinkAt(eef.parent_link, marker_pose_eigen);
  bool in_collision = isGroupCollidingWithWorld(ks, eef.eef_group);

  if ( in_collision )
  {
    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                  GripperMarker::IN_COLLISION));
  }
  else
  {
    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                      GripperMarker::NOT_TESTED));
  }
}

void MainWindow::switchGoalPoseMarkerSelection(const std::string &marker_name)
{
  if (robot_interaction_->getActiveEndEffectors().empty() || ! goal_poses_[marker_name]->isVisible())
    return;

  if (goal_poses_[marker_name]->isSelected())
  {
    //If selected, unselect
    goal_poses_[marker_name]->unselect();
    setItemSelectionInList(marker_name, false, ui_.goal_poses_list);
  }
  else
  {
    //If unselected, select. Only display the gripper mesh for one
    if (ui_.goal_poses_list->selectedItems().size() == 1)
      goal_poses_[marker_name]->select(true);
    else
      goal_poses_[marker_name]->select(false);
    setItemSelectionInList(marker_name, true, ui_.goal_poses_list);
  }
}

void MainWindow::copySelectedGoalPoses(void)
{
  QList<QListWidgetItem *> sel = ui_.goal_poses_list->selectedItems();
  if (sel.empty() || robot_interaction_->getActiveEndEffectors().empty())
    return;

  std::string scene_name;
  {
    const planning_scene_monitor::LockedPlanningSceneRO &ps = scene_display_->getPlanningSceneRO();
    if (!ps)
      return;
    else
      scene_name = ps->getName();
  }

  for (int i = 0 ; i < sel.size() ; ++i)
  {
    std::string name = sel[i]->text().toStdString();
    if (! goal_poses_[name]->isVisible())
      continue;

    std::stringstream ss;
    ss << scene_name.c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();

    scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
    Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(robot_interaction_->getActiveEndEffectors()[0].parent_link);
    geometry_msgs::Pose marker_pose;
    marker_pose.position.x = goal_poses_[name]->imarker->getPosition().x;
    marker_pose.position.y = goal_poses_[name]->imarker->getPosition().y;
    marker_pose.position.z = goal_poses_[name]->imarker->getPosition().z;
    marker_pose.orientation.x = goal_poses_[name]->imarker->getOrientation().x;
    marker_pose.orientation.y = goal_poses_[name]->imarker->getOrientation().y;
    marker_pose.orientation.z = goal_poses_[name]->imarker->getOrientation().z;
    marker_pose.orientation.w = goal_poses_[name]->imarker->getOrientation().w;

    static const float marker_scale = 0.15;
    GripperMarkerPtr goal_pose(new GripperMarker(scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_, ss.str(), scene_display_->getRobotModel()->getModelFrame(),
                            robot_interaction_->getActiveEndEffectors()[0], marker_pose, marker_scale, GripperMarker::NOT_TESTED, true));

    goal_poses_.insert(GoalPosePair(ss.str(), goal_pose));

    // Connect signals
    goal_pose->connect(this, SLOT( goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &)));

    //Unselect the marker source of the copy
    switchGoalPoseMarkerSelection(name);
  }

  JobProcessing::addMainLoopJob(boost::bind(&MainWindow::populateGoalPosesList, this));
}

void MainWindow::saveStartStateButtonClicked(void)
{
  bool ok = false;

  std::stringstream ss;
  ss << scene_display_->getRobotModel()->getName().c_str() << "_state_" << std::setfill('0') << std::setw(4) << start_states_.size();

  QString text = QInputDialog::getText(this, tr("Choose a name"),
                                       tr("Start state name:"), QLineEdit::Normal,
                                       QString(ss.str().c_str()), &ok);

  std::string name;
  if (ok)
  {
    if (!text.isEmpty())
    {
      name = text.toStdString();
      if (start_states_.find(name) != start_states_.end())
        QMessageBox::warning(this, "Name already exists", QString("The name '").append(name.c_str()).
                             append("' already exists. Not creating state."));
      else
      {
        //Store the current start state
        moveit_msgs::RobotState msg;
        robot_state::robotStateToRobotStateMsg(scene_display_->getPlanningSceneRO()->getCurrentState(), msg);
        start_states_.insert(StartStatePair(name, StartStatePtr(new StartState(msg))));

        //Save to the database if connected
        if (robot_state_storage_)
        {
          try
          {
              robot_state_storage_->addRobotState(msg, name);
          }
          catch (std::runtime_error &ex)
          {
            ROS_ERROR("Cannot save robot state on the database: %s", ex.what());
          }
        }
      }
    }
    else
      QMessageBox::warning(this, "Start state not saved", "Cannot use an empty name for a new start state.");
  }
  populateStartStatesList();
}

void MainWindow::removeSelectedStatesButtonClicked(void)
{
  QList<QListWidgetItem*> found_items = ui_.start_states_list->selectedItems();
  for (unsigned int i = 0; i < found_items.size(); i++)
  {
    start_states_.erase(found_items[i]->text().toStdString());
  }
  populateStartStatesList();
}

void MainWindow::removeAllStatesButtonClicked(void)
{
  start_states_.clear();
  populateStartStatesList();
}

void MainWindow::populateStartStatesList(void)
{
  ui_.start_states_list->clear();
  for (StartStateMap::iterator it = start_states_.begin(); it != start_states_.end(); ++it)
  {
    QListWidgetItem *item = new QListWidgetItem(QString(it->first.c_str()));
    ui_.start_states_list->addItem(item);
    if (it->second->selected)
    {
      //If selected, highlight in the list
      item->setSelected(true);
    }
  }
}

void MainWindow::startStateItemDoubleClicked(QListWidgetItem * item)
{
  scene_display_->getPlanningSceneRW()->setCurrentState(start_states_[item->text().toStdString()]->state_msg);
  scene_display_->queueRenderSceneGeometry();
}

void MainWindow::runBenchmark(void)
{
  if (! robot_interaction_ || robot_interaction_->getActiveEndEffectors().size() == 0)
  {
    QMessageBox::warning(this, "Error", "Please make sure a planning group with an end-effector is active");
    return;
  }

  run_benchmark_ui_.benchmark_goal_text->setText(ui_.load_poses_filter_text->text());
  run_benchmark_ui_.benchmark_start_state_text->setText(ui_.load_states_filter_text->text());

  // Load available planners
  boost::shared_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
  std::map<std::string, planning_interface::PlannerManagerPtr> planner_interfaces;
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }

  if (planner_plugin_loader)
  {
    // load the planning plugins
    const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
    for (std::size_t i = 0 ; i < classes.size() ; ++i)
    {
      ROS_DEBUG("Attempting to load and configure %s", classes[i].c_str());
      try
      {
        boost::shared_ptr<planning_interface::PlannerManager> p = planner_plugin_loader->createInstance(classes[i]);
        p->initialize(scene_display_->getPlanningSceneRO()->getRobotModel(), "");
        planner_interfaces[classes[i]] = p;
      }
      catch (pluginlib::PluginlibException& ex)
      {
        ROS_ERROR_STREAM("Exception while loading planner '" << classes[i] << "': " << ex.what());
      }
    }

    //Get the list of planning interfaces and planning algorithms
    if (! planner_interfaces.empty())
    {
      std::stringstream interfaces_ss, algorithms_ss;
      for (std::map<std::string, boost::shared_ptr<planning_interface::PlannerManager> >::const_iterator it = planner_interfaces.begin() ;
          it != planner_interfaces.end(); ++it)
      {
        interfaces_ss << it->first << " ";

        std::vector<std::string> known;
        it->second->getPlanningAlgorithms(known);
        for (std::size_t i = 0; i < known.size(); ++i)
          algorithms_ss << known[i] << " ";
      }
      ROS_DEBUG("Available planner instances: %s. Available algorithms: %s", interfaces_ss.str().c_str(), algorithms_ss.str().c_str());
      run_benchmark_ui_.planning_interfaces_text->setText(QString(interfaces_ss.str().c_str()));
      run_benchmark_ui_.planning_algorithms_text->setText(QString(algorithms_ss.str().c_str()));
    }
  }

  run_benchmark_dialog_->show();
}

void MainWindow::benchmarkFolderButtonClicked(void)
{
  QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"),
                                                  "/home", QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
  run_benchmark_ui_.benchmark_output_folder_text->setText(dir);
}

void MainWindow::cancelBenchmarkButtonClicked(void)
{
  run_benchmark_dialog_->close();
}

bool MainWindow::saveBenchmarkConfigButtonClicked(void)
{
  if (run_benchmark_ui_.benchmark_output_folder_text->text().isEmpty())
  {
    QMessageBox::warning(this, "Missing data", "Must specify an output folder");
    return false;
  }

  QString outfilename =  run_benchmark_ui_.benchmark_output_folder_text->text().append("/config.cfg");
  std::ofstream outfile(outfilename.toUtf8());
  if (outfile)
  {
    outfile << "[scene]" << std::endl;
    outfile << "group=" << ui_.planning_group_combo->currentText().toStdString() << std::endl;
    outfile << "default_constrained_link=" << robot_interaction_->getActiveEndEffectors()[0].parent_link << std::endl;
    outfile << "planning_frame=" << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getModelFrame() << std::endl;
    outfile << "name=" << scene_display_->getPlanningSceneRO()->getName() << std::endl;

    outfile << "timeout=" << run_benchmark_ui_.timeout_spin->value() << std::endl;
    outfile << "runs=" << run_benchmark_ui_.number_of_runs_spin->value() << std::endl;
    outfile << "output=" << run_benchmark_ui_.benchmark_output_folder_text->text().toStdString() << "/" << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getName() << "_" <<
        scene_display_->getPlanningSceneRO()->getName() << "_" <<
        ros::Time::now() << std::endl;
    outfile << "start=" << run_benchmark_ui_.benchmark_start_state_text->text().toStdString() << std::endl;
    outfile << "query=" << std::endl;
    outfile << "goal=" << run_benchmark_ui_.benchmark_goal_text->text().toStdString() << std::endl;
    outfile << "goal_offset_roll=" << ui_.goal_offset_roll->value() << std::endl;
    outfile << "goal_offset_pitch=" << ui_.goal_offset_pitch->value() << std::endl;
    outfile << "goal_offset_yaw=" << ui_.goal_offset_yaw->value() << std::endl << std::endl;

    outfile << "[plugin]" << std::endl;
    outfile << "name=" << run_benchmark_ui_.planning_interfaces_text->text().toStdString() << std::endl;
    outfile << "planners=" << run_benchmark_ui_.planning_algorithms_text->text().toStdString() << std::endl;

    outfile.close();

    run_benchmark_dialog_->close();
  }
  else
  {
    QMessageBox::warning(this, "Error", QString("Cannot open file ").append(outfilename).append(" for writing"));
    return false;
  }

  return true;
}

void MainWindow::runBenchmarkButtonClicked(void)
{
  if (! saveBenchmarkConfigButtonClicked())
    return;

  QMessageBox msgBox;
  msgBox.setText("This will run the benchmark pipeline. The process will take several minutes during which you will not be able to interact with the interface. You can follow the progress in the console output");
  msgBox.setInformativeText("Do you want to continue?");
  msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
  msgBox.setDefaultButton(QMessageBox::Yes);
  int ret = msgBox.exec();

  switch (ret)
  {
    case QMessageBox::Yes:
    {
      QString outfilename =  run_benchmark_ui_.benchmark_output_folder_text->text().append("/config.cfg");
      moveit_benchmarks::BenchmarkType btype = 0;
      moveit_benchmarks::BenchmarkExecution be(scene_display_->getPlanningSceneMonitor()->getPlanningScene(), database_host_, database_port_);
      if (run_benchmark_ui_.benchmark_include_planners_checkbox->isChecked())
        btype += moveit_benchmarks::BENCHMARK_PLANNERS;
      if (run_benchmark_ui_.benchmark_check_reachability_checkbox->isChecked())
        btype += moveit_benchmarks::BENCHMARK_GOAL_EXISTANCE;

      if (be.readOptions(outfilename.toStdString()))
      {
        std::stringstream ss;
        be.printOptions(ss);
        ROS_INFO_STREAM("Calling benchmark with options:" << std::endl << ss.str() << std::endl);

        BenchmarkProcessingThread benchmark_thread(be, btype, this);
        benchmark_thread.startAndShow();

        if (benchmark_thread.isRunning())
        {
          benchmark_thread.terminate();
          benchmark_thread.wait();
          QMessageBox::warning(this, "", "Benchmark computation canceled");
        }
        else
        {
          QMessageBox::information(this, "Benchmark computation finished", QString("The results were logged into '").append(run_benchmark_ui_.benchmark_output_folder_text->text()));
        }
      }

      break;
    }
  }
}

void MainWindow::loadBenchmarkResults(void)
{
  //Select a log file of the set.
  QString path = QFileDialog::getOpenFileName(this, tr("Select a log file in the set"), tr(""), tr("Log files (*.log)"));
  if (!path.isEmpty())
  {
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::computeLoadBenchmarkResults, this, path.toStdString()));
  }
}

void MainWindow::computeLoadBenchmarkResults(const std::string &file)
{
  std::string logid, basename, logid_text;
  try
  {
    logid = file.substr( file.find_last_of(".", file.length()-5) + 1, file.find_last_of(".") - file.find_last_of(".", file.length()-5) - 1 );
    basename = file.substr(0, file.find_last_of(".", file.length()-5));
    logid_text = logid.substr(logid.find_first_of("_"), logid.length() - logid.find_first_of("_"));
  }
  catch (...)
  {
    ROS_ERROR("Invalid benchmark log file. Cannot load results.");
    return;
  }

  int count = 1;
  bool more_files = true;
  //Parse all the log files in the set
  while (more_files)
  {
    std::ifstream ifile;
    std::stringstream file_to_load;
    file_to_load << basename << "." << count << logid_text << ".log";

    ifile.open(file_to_load.str().c_str());
    if (ifile.good())
    {
      //Parse results
      char text_line[512];
      static std::streamsize text_line_length = 512;
      bool valid_file = false;
      ifile.getline(text_line, text_line_length);

      //Check if the file is valid. The first line must contain "Experiment <scene>". There must be a "total_time" line, followed by the results
      if (ifile.good() && strstr(text_line, "Experiment") != NULL && strlen(text_line) > 11
          && strncmp(&text_line[11], scene_display_->getPlanningSceneRO()->getName().c_str(), scene_display_->getPlanningSceneRO()->getName().length()) == 0)
      {
        while (ifile.good())
        {
          if ( strstr(text_line, "total_time REAL") != NULL)
          {
            valid_file = true;
            break;
          }

          ifile.getline(text_line, text_line_length);
        }
      }
      else
      {
        ROS_ERROR("Not a valid log file, or a different planning scene loaded");
      }

      if (valid_file)
      {
        if (basename.find(".trajectory") != std::string::npos)
        {
          //Trajectory results may contain multiple goals
          int nwaypoint = 0;
          while (ifile.getline(text_line, text_line_length) && ifile.good() && strlen(text_line) > 6)
          {
            //This should be a reachability results line composed of reachable, collision-free and time fields (bool; bool; float)
            try
            {
              bool reachable = boost::lexical_cast<bool>(text_line[0]);
              bool collision_free = boost::lexical_cast<bool>(text_line[3]);


              //Update colors accordingly
              if (count <= trajectories_.size())
              {
                std::string trajectory_name = ui_.trajectory_list->item(count-1)->text().toStdString();
                if ( nwaypoint < trajectories_[trajectory_name]->waypoint_markers.size() )
                {
                  if (reachable)
                  {
                    if (collision_free)
                    {
                      //Reachable and collision-free
                      JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateMarkerState, this, trajectories_[trajectory_name]->waypoint_markers[nwaypoint],
                                                                GripperMarker::REACHABLE));
                    }
                    else
                    {
                      //Reachable, but in collision
                      JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateMarkerState, this, trajectories_[trajectory_name]->waypoint_markers[nwaypoint],
                                                                GripperMarker::IN_COLLISION));
                    }
                  }
                  else
                  {
                    //Not reachable
                    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateMarkerState, this, trajectories_[trajectory_name]->waypoint_markers[nwaypoint],
                                                              GripperMarker::NOT_REACHABLE));
                  }
                }
              }
              nwaypoint++;
            }
            catch (...)
            {
              ROS_ERROR("Error parsing the log file");
            }
          }
        }
        else
        {
          ifile.getline(text_line, text_line_length);
          if (ifile.good() && strlen(text_line) > 6)
          {
            //This should be the reachability results line composed of reachable, collision-free and time fields (bool; bool; float)
            try
            {
              bool reachable = boost::lexical_cast<bool>(text_line[0]);
              bool collision_free = boost::lexical_cast<bool>(text_line[3]);

              //Update colors accordingly
              if (count <= goal_poses_.size())
              {
                std::string goal_name = ui_.goal_poses_list->item(count-1)->text().toStdString();
                if (reachable)
                {
                  if (collision_free)
                  {
                    //Reachable and collision-free
                    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                              GripperMarker::REACHABLE));
                  }
                  else
                  {
                    //Reachable, but in collision
                    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                              GripperMarker::IN_COLLISION));
                  }
                }
                else
                {
                  //Not reachable
                  JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                            GripperMarker::NOT_REACHABLE));
                }
              }
            }
            catch (...)
            {
              ROS_ERROR("Error parsing the log file");
            }
          }
        }
      }
      else
      {
        ROS_ERROR("Invalid benchmark log file. Cannot load results.");
      }

      ifile.close();
    }
    else
    {
      more_files = false;
    }
    count++;
  }
}

void MainWindow::updateGoalMarkerStateFromName(const std::string &name, const GripperMarker::GripperMarkerState &state)
{
  if ( goal_poses_.find(name) != goal_poses_.end())
    goal_poses_[name]->setState(state);
}

void MainWindow::updateMarkerState(GripperMarkerPtr marker, const GripperMarker::GripperMarkerState &state)
{
  marker->setState(state);
}

void MainWindow::goalOffsetChanged()
{
  goal_offset_.setIdentity();
  goal_offset_ = Eigen::AngleAxisd(ui_.goal_offset_roll->value() * boost::math::constants::pi<double>() / 180.0, Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ui_.goal_offset_pitch->value() * boost::math::constants::pi<double>() / 180.0, Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(ui_.goal_offset_yaw->value() * boost::math::constants::pi<double>() / 180.0, Eigen::Vector3d::UnitZ());

  if ( ! robot_interaction_->getActiveEndEffectors().empty() )
  {
    Eigen::Affine3d current_pose;
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      current_pose = goals_initial_pose_[it->first] * goal_offset_;
      it->second->setPose(current_pose);
    }
  }
}

} //namespace