tab_states_and_goals.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: Mario Prats, Ioan Sucan */
00036 
00037 #include <main_window.h>
00038 #include <job_processing.h>
00039 #include <benchmark_processing_thread.h>
00040 
00041 #include <moveit/kinematic_constraints/utils.h>
00042 #include <moveit/robot_state/conversions.h>
00043 #include <moveit/warehouse/constraints_storage.h>
00044 #include <moveit/warehouse/state_storage.h>
00045 #include <moveit/benchmarks/benchmark_execution.h>
00046 #include <moveit/planning_interface/planning_interface.h>
00047 
00048 #include <pluginlib/class_loader.h>
00049 
00050 #include <rviz/display_context.h>
00051 #include <rviz/window_manager_interface.h>
00052 
00053 #include <eigen_conversions/eigen_msg.h>
00054 
00055 #include <QMessageBox>
00056 #include <QInputDialog>
00057 #include <QFileDialog>
00058 #include <QProgressDialog>
00059 
00060 #include <boost/math/constants/constants.hpp>
00061 
00062 namespace benchmark_tool
00063 {
00064 void MainWindow::createGoalAtPose(const std::string& name, const Eigen::Affine3d& pose)
00065 {
00066   goals_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(name, pose));
00067 
00068   geometry_msgs::Pose marker_pose;
00069   tf::poseEigenToMsg(pose * goal_offset_, marker_pose);
00070   static const float marker_scale = 0.15;
00071 
00072   GripperMarkerPtr goal_pose(new GripperMarker(
00073       scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_,
00074       name, scene_display_->getRobotModel()->getModelFrame(), robot_interaction_->getActiveEndEffectors()[0],
00075       marker_pose, marker_scale, GripperMarker::NOT_TESTED));
00076   goal_poses_.insert(GoalPosePair(name, goal_pose));
00077 
00078   // Connect signals
00079   goal_pose->connect(this, SLOT(goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00080 
00081   // If connected to a database, save all the goals back to the database
00082   if (constraints_storage_)
00083   {
00084     saveGoalsToDB();
00085   }
00086 }
00087 
00088 void MainWindow::saveGoalsToDB()
00089 {
00090   if (constraints_storage_)
00091   {
00092     // Convert all goal pose markers into constraints and store them
00093     for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00094     {
00095       moveit_msgs::Constraints constraints;
00096       constraints.name = it->first;
00097 
00098       shape_msgs::SolidPrimitive sp;
00099       sp.type = sp.BOX;
00100       sp.dimensions.resize(3, std::numeric_limits<float>::epsilon() * 10.0);
00101 
00102       moveit_msgs::PositionConstraint pc;
00103       pc.header.frame_id = scene_display_->getRobotModel()->getModelFrame();
00104       pc.link_name = robot_interaction_->getActiveEndEffectors()[0].parent_link;  // TODO this is hacky i think
00105       pc.constraint_region.primitives.push_back(sp);
00106       geometry_msgs::Pose posemsg;
00107       it->second->getPosition(posemsg.position);
00108       posemsg.orientation.x = 0.0;
00109       posemsg.orientation.y = 0.0;
00110       posemsg.orientation.z = 0.0;
00111       posemsg.orientation.w = 1.0;
00112       pc.constraint_region.primitive_poses.push_back(posemsg);
00113       pc.weight = 1.0;
00114       constraints.position_constraints.push_back(pc);
00115 
00116       moveit_msgs::OrientationConstraint oc;
00117       oc.header.frame_id = scene_display_->getRobotModel()->getModelFrame();
00118       oc.link_name = robot_interaction_->getActiveEndEffectors()[0].parent_link;  // TODO this is hacky i think
00119       it->second->getOrientation(oc.orientation);
00120       oc.absolute_x_axis_tolerance = oc.absolute_y_axis_tolerance = oc.absolute_z_axis_tolerance =
00121           std::numeric_limits<float>::epsilon() * 10.0;
00122       oc.weight = 1.0;
00123       constraints.orientation_constraints.push_back(oc);
00124 
00125       try
00126       {
00127         constraints_storage_->addConstraints(constraints);
00128       }
00129       catch (std::runtime_error& ex)
00130       {
00131         ROS_ERROR("Cannot save constraint: %s", ex.what());
00132       }
00133     }
00134   }
00135   else
00136   {
00137     QMessageBox::warning(this, "Warning", "Not connected to a database.");
00138   }
00139 }
00140 
00141 void MainWindow::createGoalPoseButtonClicked(void)
00142 {
00143   std::stringstream ss;
00144 
00145   {
00146     const planning_scene_monitor::LockedPlanningSceneRO& ps = scene_display_->getPlanningSceneRO();
00147     if (!ps || robot_interaction_->getActiveEndEffectors().empty())
00148     {
00149       if (!ps)
00150         ROS_ERROR("Not planning scene");
00151       if (robot_interaction_->getActiveEndEffectors().empty())
00152         ROS_ERROR("No end effector");
00153       return;
00154     }
00155     else
00156       ss << ps->getName().c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();
00157   }
00158 
00159   bool ok = false;
00160   QString text = QInputDialog::getText(this, tr("Choose a name"), tr("Goal pose name:"), QLineEdit::Normal,
00161                                        QString(ss.str().c_str()), &ok);
00162 
00163   std::string name;
00164   if (ok)
00165   {
00166     if (!text.isEmpty())
00167     {
00168       name = text.toStdString();
00169       if (goal_poses_.find(name) != goal_poses_.end())
00170         QMessageBox::warning(this, "Name already exists",
00171                              QString("The name '").append(name.c_str()).append("' already exists. Not creating goal."));
00172       else
00173       {
00174         // Create the new goal pose at the current eef pose, and attach an interactive marker to it
00175         scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
00176         Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(
00177             robot_interaction_->getActiveEndEffectors()[0].parent_link);
00178         createGoalAtPose(name, tip_pose);
00179       }
00180     }
00181     else
00182       QMessageBox::warning(this, "Goal not created", "Cannot use an empty name for a new goal pose.");
00183   }
00184 
00185   populateGoalPosesList();
00186 }
00187 
00188 void MainWindow::showBBoxGoalsDialog()
00189 {
00190   std::string goals_base_name;
00191   {
00192     const planning_scene_monitor::LockedPlanningSceneRO& ps = scene_display_->getPlanningSceneRO();
00193     if (!ps || robot_interaction_->getActiveEndEffectors().empty())
00194     {
00195       if (!ps)
00196         ROS_ERROR("No planning scene");
00197       if (robot_interaction_->getActiveEndEffectors().empty())
00198         ROS_ERROR("No end effector");
00199       return;
00200     }
00201     goals_base_name = ps->getName() + "_pose_";
00202   }
00203 
00204   bbox_dialog_ui_.base_name_text->setText(QString(goals_base_name.c_str()));
00205 
00206   bbox_dialog_->exec();
00207 }
00208 
00209 void MainWindow::createBBoxGoalsButtonClicked(void)
00210 {
00211   bbox_dialog_->close();
00212 
00213   double minx = bbox_dialog_ui_.center_x_text->text().toDouble() - bbox_dialog_ui_.size_x_text->text().toDouble() / 2.0;
00214   double maxx = bbox_dialog_ui_.center_x_text->text().toDouble() + bbox_dialog_ui_.size_x_text->text().toDouble() / 2.0;
00215   double stepx = (maxx - minx) / std::max<double>(bbox_dialog_ui_.ngoals_x_text->text().toDouble() - 1, 1);
00216   double miny = bbox_dialog_ui_.center_y_text->text().toDouble() - bbox_dialog_ui_.size_y_text->text().toDouble() / 2.0;
00217   double maxy = bbox_dialog_ui_.center_y_text->text().toDouble() + bbox_dialog_ui_.size_y_text->text().toDouble() / 2.0;
00218   double stepy = (maxy - miny) / std::max<double>(bbox_dialog_ui_.ngoals_y_text->text().toDouble() - 1, 1);
00219   double minz = bbox_dialog_ui_.center_z_text->text().toDouble() - bbox_dialog_ui_.size_z_text->text().toDouble() / 2.0;
00220   double maxz = bbox_dialog_ui_.center_z_text->text().toDouble() + bbox_dialog_ui_.size_z_text->text().toDouble() / 2.0;
00221   double stepz = (maxz - minz) / std::max<double>(bbox_dialog_ui_.ngoals_z_text->text().toDouble() - 1, 1);
00222 
00223   Eigen::Affine3d goal_pose;
00224   goal_pose.setIdentity();
00225   for (std::size_t x = 0; x < bbox_dialog_ui_.ngoals_x_text->text().toShort(); ++x)
00226     for (std::size_t y = 0; y < bbox_dialog_ui_.ngoals_y_text->text().toShort(); ++y)
00227       for (std::size_t z = 0; z < bbox_dialog_ui_.ngoals_z_text->text().toShort(); ++z)
00228       {
00229         goal_pose(0, 3) = minx + x * stepx;
00230         goal_pose(1, 3) = miny + y * stepy;
00231         goal_pose(2, 3) = minz + z * stepz;
00232 
00233         std::stringstream ss;
00234         ss << bbox_dialog_ui_.base_name_text->text().toStdString() << std::setfill('0') << std::setw(4)
00235            << goal_poses_.size();
00236 
00237         createGoalAtPose(ss.str(), goal_pose);
00238       }
00239 
00240   populateGoalPosesList();
00241 }
00242 
00243 void MainWindow::removeSelectedGoalsButtonClicked(void)
00244 {
00245   QList<QListWidgetItem*> found_items = ui_.goal_poses_list->selectedItems();
00246   for (unsigned int i = 0; i < found_items.size(); i++)
00247   {
00248     goal_poses_.erase(found_items[i]->text().toStdString());
00249   }
00250   populateGoalPosesList();
00251 }
00252 
00253 void MainWindow::removeAllGoalsButtonClicked(void)
00254 {
00255   goal_poses_.clear();
00256   goals_initial_pose_.clear();
00257   populateGoalPosesList();
00258 }
00259 
00260 void MainWindow::loadGoalsFromDBButtonClicked(void)
00261 {
00262   // Get all the constraints from the database, convert to goal pose markers
00263   if (constraints_storage_ && !robot_interaction_->getActiveEndEffectors().empty())
00264   {
00265     // First clear the current list
00266     removeAllGoalsButtonClicked();
00267 
00268     std::vector<std::string> names;
00269     try
00270     {
00271       constraints_storage_->getKnownConstraints(ui_.load_poses_filter_text->text().toStdString(), names);
00272     }
00273     catch (std::runtime_error& ex)
00274     {
00275       QMessageBox::warning(this, "Cannot query the database",
00276                            QString("Wrongly formatted regular expression for goal poses: ").append(ex.what()));
00277       return;
00278     }
00279 
00280     for (unsigned int i = 0; i < names.size(); i++)
00281     {
00282       // Create a goal pose marker
00283       moveit_warehouse::ConstraintsWithMetadata c;
00284       bool got_constraint = false;
00285       try
00286       {
00287         got_constraint = constraints_storage_->getConstraints(c, names[i]);
00288       }
00289       catch (std::runtime_error& ex)
00290       {
00291         ROS_ERROR("%s", ex.what());
00292       }
00293       if (!got_constraint)
00294         continue;
00295 
00296       if (c->position_constraints.size() > 0 &&
00297           c->position_constraints[0].constraint_region.primitive_poses.size() > 0 &&
00298           c->orientation_constraints.size() > 0)
00299       {
00300         // Overwrite if exists. TODO: Ask the user before overwriting? copy the existing one with another name before?
00301         if (goal_poses_.find(c->name) != goal_poses_.end())
00302         {
00303           goal_poses_.erase(c->name);
00304         }
00305         geometry_msgs::Pose shape_pose;
00306         shape_pose.position = c->position_constraints[0].constraint_region.primitive_poses[0].position;
00307         shape_pose.orientation = c->orientation_constraints[0].orientation;
00308 
00309         Eigen::Affine3d shape_pose_eigen;
00310         tf::poseMsgToEigen(shape_pose, shape_pose_eigen);
00311         goals_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(c->name, shape_pose_eigen));
00312 
00313         tf::poseEigenToMsg(shape_pose_eigen * goal_offset_, shape_pose);
00314 
00315         static const float marker_scale = 0.15;
00316         GripperMarkerPtr goal_pose(new GripperMarker(
00317             scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(),
00318             visualization_manager_, c->name, scene_display_->getRobotModel()->getModelFrame(),
00319             robot_interaction_->getActiveEndEffectors()[0], shape_pose, marker_scale, GripperMarker::NOT_TESTED, false,
00320             ui_.show_x_checkbox->isChecked(), ui_.show_y_checkbox->isChecked(), ui_.show_z_checkbox->isChecked()));
00321         // Connect signals
00322         goal_pose->connect(this, SLOT(goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00323 
00324         goal_poses_.insert(GoalPosePair(c->name, goal_pose));
00325       }
00326     }
00327     populateGoalPosesList();
00328   }
00329   else
00330   {
00331     if (!constraints_storage_)
00332       QMessageBox::warning(this, "Warning", "Not connected to a database.");
00333   }
00334 }
00335 
00336 void MainWindow::saveGoalsOnDBButtonClicked(void)
00337 {
00338   saveGoalsToDB();
00339 }
00340 
00341 void MainWindow::deleteGoalsOnDBButtonClicked(void)
00342 {
00343   if (constraints_storage_)
00344   {
00345     // Warn the user
00346     QMessageBox msgBox;
00347     msgBox.setText("All the selected items will be removed from the database");
00348     msgBox.setInformativeText("Do you want to continue?");
00349     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
00350     msgBox.setDefaultButton(QMessageBox::No);
00351     int ret = msgBox.exec();
00352 
00353     switch (ret)
00354     {
00355       case QMessageBox::Yes:
00356       {
00357         // Go through the list of goal poses, and delete those selected
00358         QList<QListWidgetItem*> found_items = ui_.goal_poses_list->selectedItems();
00359         for (std::size_t i = 0; i < found_items.size(); i++)
00360         {
00361           try
00362           {
00363             constraints_storage_->removeConstraints(found_items[i]->text().toStdString());
00364           }
00365           catch (std::runtime_error& ex)
00366           {
00367             ROS_ERROR("%s", ex.what());
00368           }
00369         }
00370         break;
00371       }
00372     }
00373   }
00374   removeSelectedGoalsButtonClicked();
00375 }
00376 
00377 void MainWindow::loadStatesFromDBButtonClicked(void)
00378 {
00379   // Get all the start states from the database
00380   if (robot_state_storage_)
00381   {
00382     // First clear the current list
00383     removeAllStatesButtonClicked();
00384 
00385     std::vector<std::string> names;
00386     try
00387     {
00388       robot_state_storage_->getKnownRobotStates(ui_.load_states_filter_text->text().toStdString(), names);
00389     }
00390     catch (std::runtime_error& ex)
00391     {
00392       QMessageBox::warning(this, "Cannot query the database",
00393                            QString("Wrongly formatted regular expression for goal poses: ").append(ex.what()));
00394       return;
00395     }
00396 
00397     for (unsigned int i = 0; i < names.size(); i++)
00398     {
00399       moveit_warehouse::RobotStateWithMetadata rs;
00400       bool got_state = false;
00401       try
00402       {
00403         got_state = robot_state_storage_->getRobotState(rs, names[i]);
00404       }
00405       catch (std::runtime_error& ex)
00406       {
00407         ROS_ERROR("%s", ex.what());
00408       }
00409       if (!got_state)
00410         continue;
00411 
00412       // Overwrite if exists.
00413       if (start_states_.find(names[i]) != start_states_.end())
00414       {
00415         start_states_.erase(names[i]);
00416       }
00417 
00418       // Store the current start state
00419       start_states_.insert(StartStatePair(names[i], StartStatePtr(new StartState(*rs))));
00420     }
00421     populateStartStatesList();
00422   }
00423   else
00424   {
00425     QMessageBox::warning(this, "Warning", "Not connected to a database.");
00426   }
00427 }
00428 
00429 void MainWindow::saveStatesOnDBButtonClicked(void)
00430 {
00431   if (robot_state_storage_)
00432   {
00433     // Store all start states
00434     for (StartStateMap::iterator it = start_states_.begin(); it != start_states_.end(); ++it)
00435       try
00436       {
00437         robot_state_storage_->addRobotState(it->second->state_msg, it->first);
00438       }
00439       catch (std::runtime_error& ex)
00440       {
00441         ROS_ERROR("Cannot save robot state: %s", ex.what());
00442       }
00443   }
00444   else
00445   {
00446     QMessageBox::warning(this, "Warning", "Not connected to a database.");
00447   }
00448 }
00449 
00450 void MainWindow::deleteStatesOnDBButtonClicked(void)
00451 {
00452   if (robot_state_storage_)
00453   {
00454     // Warn the user
00455     QMessageBox msgBox;
00456     msgBox.setText("All the selected items will be removed from the database");
00457     msgBox.setInformativeText("Do you want to continue?");
00458     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
00459     msgBox.setDefaultButton(QMessageBox::No);
00460     int ret = msgBox.exec();
00461 
00462     switch (ret)
00463     {
00464       case QMessageBox::Yes:
00465       {
00466         QList<QListWidgetItem*> found_items = ui_.start_states_list->selectedItems();
00467         for (unsigned int i = 0; i < found_items.size(); ++i)
00468         {
00469           try
00470           {
00471             robot_state_storage_->removeRobotState(found_items[i]->text().toStdString());
00472           }
00473           catch (std::runtime_error& ex)
00474           {
00475             ROS_ERROR("%s", ex.what());
00476           }
00477         }
00478         break;
00479       }
00480     }
00481   }
00482   removeSelectedStatesButtonClicked();
00483 }
00484 
00485 void MainWindow::visibleAxisChanged(int state)
00486 {
00487   if (!robot_interaction_->getActiveEndEffectors().empty())
00488   {
00489     for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00490     {
00491       if (it->second->isVisible())
00492       {
00493         it->second->setAxisVisibility(ui_.show_x_checkbox->isChecked(), ui_.show_y_checkbox->isChecked(),
00494                                       ui_.show_z_checkbox->isChecked());
00495       }
00496     }
00497   }
00498 }
00499 
00500 void MainWindow::populateGoalPosesList(void)
00501 {
00502   ui_.goal_poses_list->clear();
00503   for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00504   {
00505     QListWidgetItem* item = new QListWidgetItem(QString(it->first.c_str()));
00506     ui_.goal_poses_list->addItem(item);
00507     if (!it->second->isVisible())
00508     {
00509       item->setBackground(QBrush(Qt::Dense4Pattern));
00510     }
00511     else if (it->second->isSelected())
00512     {
00513       // If selected, highlight in the list
00514       item->setSelected(true);
00515     }
00516   }
00517 }
00518 
00519 void MainWindow::switchGoalVisibilityButtonClicked(void)
00520 {
00521   QList<QListWidgetItem*> selection = ui_.goal_poses_list->selectedItems();
00522   for (std::size_t i = 0; i < selection.size(); ++i)
00523   {
00524     std::string name = selection[i]->text().toStdString();
00525     if (goal_poses_[name]->isVisible())
00526     {
00527       // Set invisible
00528       goal_poses_[name]->hide();
00529       selection[i]->setBackground(QBrush(Qt::Dense4Pattern));
00530     }
00531     else
00532     {
00533       // Set visible
00534       goal_poses_[name]->show(scene_display_->getSceneNode(), visualization_manager_);
00535       selection[i]->setBackground(QBrush(Qt::NoBrush));
00536     }
00537   }
00538 }
00539 
00540 void MainWindow::goalPoseSelectionChanged(void)
00541 {
00542   for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
00543   {
00544     QListWidgetItem* item = ui_.goal_poses_list->item(i);
00545     std::string name = item->text().toStdString();
00546     if (goal_poses_.find(name) != goal_poses_.end() && ((item->isSelected() && !goal_poses_[name]->isSelected()) ||
00547                                                         (!item->isSelected() && goal_poses_[name]->isSelected())))
00548       switchGoalPoseMarkerSelection(name);
00549   }
00550 }
00551 
00552 void MainWindow::goalPoseDoubleClicked(QListWidgetItem* item)
00553 {
00554   JobProcessing::addBackgroundJob(boost::bind(&MainWindow::computeGoalPoseDoubleClicked, this, item));
00555 }
00556 
00557 void MainWindow::computeGoalPoseDoubleClicked(QListWidgetItem* item)
00558 {
00559   if (!robot_interaction_ || robot_interaction_->getActiveEndEffectors().empty())
00560     return;
00561 
00562   std::string item_text = item->text().toStdString();
00563 
00564   // Switch the marker color to processing color while processing
00565   JobProcessing::addMainLoopJob(
00566       boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, item_text, GripperMarker::PROCESSING));
00567 
00568   checkIfGoalReachable(item_text, true);
00569 }
00570 
00571 /* Receives feedback from the interactive marker attached to a goal pose */
00572 void MainWindow::goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
00573 {
00574   if (feedback.event_type == feedback.BUTTON_CLICK)
00575   {
00576     // Unselect all but the clicked one. Needs to be in order, first unselect, then select.
00577     QListWidgetItem* item = 0;
00578     for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
00579     {
00580       item = ui_.goal_poses_list->item(i);
00581       if (item->text().toStdString() != feedback.marker_name)
00582         JobProcessing::addMainLoopJob(boost::bind(&MainWindow::selectItemJob, this, item, false));
00583     }
00584 
00585     for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
00586     {
00587       item = ui_.goal_poses_list->item(i);
00588       if (item->text().toStdString() == feedback.marker_name)
00589         JobProcessing::addMainLoopJob(boost::bind(&MainWindow::selectItemJob, this, item, true));
00590     }
00591   }
00592   else if (feedback.event_type == feedback.MOUSE_DOWN)
00593   {
00594     // First check to see if this mouse_down is happening on an
00595     // already-selected goal pose.  A mouse_down event is sent even if
00596     // the gesture turns out to be nothing but a click.
00597     bool this_goal_already_selected = false;
00598     for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00599     {
00600       if (it->second->isSelected() && it->second->isVisible() && it->second->imarker->getName() == feedback.marker_name)
00601       {
00602         this_goal_already_selected = true;
00603       }
00604     }
00605 
00606     // If this is a mouse-down on an already-selected goal pose, we
00607     // want to initialize dragging all selected goal poses at the same
00608     // time.
00609     if (this_goal_already_selected)
00610     {
00611       // Store current poses
00612       goals_dragging_initial_pose_.clear();
00613       for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00614       {
00615         if (it->second->isSelected() && it->second->isVisible())
00616         {
00617           Eigen::Affine3d pose(
00618               Eigen::Quaterniond(it->second->imarker->getOrientation().w, it->second->imarker->getOrientation().x,
00619                                  it->second->imarker->getOrientation().y, it->second->imarker->getOrientation().z));
00620           pose(0, 3) = it->second->imarker->getPosition().x;
00621           pose(1, 3) = it->second->imarker->getPosition().y;
00622           pose(2, 3) = it->second->imarker->getPosition().z;
00623           goals_dragging_initial_pose_.insert(
00624               std::pair<std::string, Eigen::Affine3d>(it->second->imarker->getName(), pose));
00625 
00626           if (it->second->imarker->getName() == feedback.marker_name)
00627             drag_initial_pose_ = pose;
00628         }
00629       }
00630       goal_pose_dragging_ = true;
00631     }
00632   }
00633   else if (feedback.event_type == feedback.POSE_UPDATE && goal_pose_dragging_)
00634   {
00635     // Compute displacement from stored pose, and apply to the rest of selected markers
00636     Eigen::Affine3d current_pose_eigen;
00637     tf::poseMsgToEigen(feedback.pose, current_pose_eigen);
00638 
00639     Eigen::Affine3d current_wrt_initial = drag_initial_pose_.inverse() * current_pose_eigen;
00640 
00641     // Display the pose in the ui
00642     Eigen::Vector3d v = current_pose_eigen.linear().eulerAngles(0, 1, 2);
00643     setStatusFromBackground(STATUS_INFO, QString().sprintf("%.2f %.2f %.2f    %.2f %.2f %.2f", current_pose_eigen(0, 3),
00644                                                            current_pose_eigen(1, 3), current_pose_eigen(2, 3),
00645                                                            v(0) * 180.0 / boost::math::constants::pi<double>(),
00646                                                            v(1) * 180.0 / boost::math::constants::pi<double>(),
00647                                                            v(2) * 180.0 / boost::math::constants::pi<double>()));
00648 
00649     // Update the rest of selected markers
00650     for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00651     {
00652       if (it->second->isVisible() && it->second->imarker->getName() != feedback.marker_name && it->second->isSelected())
00653       {
00654         visualization_msgs::InteractiveMarkerPose impose;
00655 
00656         Eigen::Affine3d newpose = drag_initial_pose_ * current_wrt_initial * drag_initial_pose_.inverse() *
00657                                   goals_dragging_initial_pose_[it->second->imarker->getName()];
00658         tf::poseEigenToMsg(newpose, impose.pose);
00659 
00660         it->second->imarker->setPose(
00661             Ogre::Vector3(impose.pose.position.x, impose.pose.position.y, impose.pose.position.z),
00662             Ogre::Quaternion(impose.pose.orientation.w, impose.pose.orientation.x, impose.pose.orientation.y,
00663                              impose.pose.orientation.z),
00664             "");
00665       }
00666     }
00667   }
00668   else if (feedback.event_type == feedback.MOUSE_UP)
00669   {
00670     goal_pose_dragging_ = false;
00671     JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalInCollision, this, feedback.marker_name));
00672   }
00673 }
00674 
00675 void MainWindow::checkGoalsReachable(void)
00676 {
00677   for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00678     JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalReachable, this, it->first, false));
00679 }
00680 
00681 void MainWindow::checkGoalsInCollision(void)
00682 {
00683   for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00684     JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalInCollision, this, it->first));
00685 }
00686 
00687 void MainWindow::checkIfGoalReachable(const std::string& goal_name, bool update_if_reachable)
00688 {
00689   if (goal_poses_.find(goal_name) == goal_poses_.end())
00690     return;
00691 
00692   if (!goal_poses_[goal_name]->isVisible())
00693     return;
00694 
00695   const boost::shared_ptr<rviz::InteractiveMarker>& imarker = goal_poses_[goal_name]->imarker;
00696 
00697   geometry_msgs::Pose current_pose_msg;
00698   current_pose_msg.position.x = imarker->getPosition().x;
00699   current_pose_msg.position.y = imarker->getPosition().y;
00700   current_pose_msg.position.z = imarker->getPosition().z;
00701   current_pose_msg.orientation.x = imarker->getOrientation().x;
00702   current_pose_msg.orientation.y = imarker->getOrientation().y;
00703   current_pose_msg.orientation.z = imarker->getOrientation().z;
00704   current_pose_msg.orientation.w = imarker->getOrientation().w;
00705 
00706   // Call to IK
00707   setStatusFromBackground(STATUS_INFO, "Computing inverse kinematics...");
00708   robot_state::RobotState ks(scene_display_->getPlanningSceneRO()->getCurrentState());
00709   static const int ik_attempts = 5;
00710   static const float ik_timeout = 0.2;
00711   bool feasible = robot_interaction_->updateState(ks, robot_interaction_->getActiveEndEffectors()[0], current_pose_msg,
00712                                                   ik_attempts, ik_timeout);
00713   if (feasible)
00714   {
00715     setStatusFromBackground(STATUS_INFO, "Updating state...");
00716     if (update_if_reachable)
00717     {
00718       scene_display_->getPlanningSceneRW()->setCurrentState(ks);
00719       scene_display_->queueRenderSceneGeometry();
00720     }
00721 
00722     setStatusFromBackground(STATUS_INFO, "Updating marker...");
00723     // Switch the marker color to reachable
00724     JobProcessing::addMainLoopJob(
00725         boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::REACHABLE));
00726   }
00727   else
00728   {
00729     // Switch the marker color to not-reachable
00730     JobProcessing::addMainLoopJob(
00731         boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::NOT_REACHABLE));
00732   }
00733   setStatusFromBackground(STATUS_INFO, "");
00734 }
00735 
00736 bool MainWindow::isGroupCollidingWithWorld(robot_state::RobotState& robot_state, const std::string& group_name)
00737 {
00738   collision_detection::AllowedCollisionMatrix acm(scene_display_->getPlanningSceneRO()->getAllowedCollisionMatrix());
00739   // get link names in group_name
00740   const std::vector<std::string>& group_link_names =
00741       scene_display_->getRobotModel()->getJointModelGroup(group_name)->getLinkModelNamesWithCollisionGeometry();
00742 
00743   // Create a set of links which is all links minus links in the group.
00744   const std::vector<std::string>& all_links = scene_display_->getRobotModel()->getLinkModelNames();
00745   std::set<std::string> link_set(all_links.begin(), all_links.end());
00746   for (size_t i = 0; i < group_link_names.size(); i++)
00747   {
00748     link_set.erase(group_link_names[i]);
00749   }
00750 
00751   // for each link name in the set,
00752   for (std::set<std::string>::const_iterator it = link_set.begin(); it != link_set.end(); it++)
00753   {
00754     // allow collisions with link.
00755     acm.setEntry(*it, true);
00756   }
00757 
00758   // call checkCollision();
00759   collision_detection::CollisionRequest req;
00760   req.verbose = false;
00761   collision_detection::CollisionResult res;
00762   scene_display_->getPlanningSceneRO()->checkCollision(req, res, robot_state, acm);
00763 
00764   // return result boolean.
00765   return res.collision;
00766 }
00767 
00768 void MainWindow::checkIfGoalInCollision(const std::string& goal_name)
00769 {
00770   if (goal_poses_.find(goal_name) == goal_poses_.end())
00771     return;
00772 
00773   // Check if the end-effector is in collision at the current pose
00774   if (!goal_poses_[goal_name]->isVisible())
00775     return;
00776 
00777   const robot_interaction::RobotInteraction::EndEffector& eef = robot_interaction_->getActiveEndEffectors()[0];
00778 
00779   const boost::shared_ptr<rviz::InteractiveMarker>& im = goal_poses_[goal_name]->imarker;
00780   Eigen::Affine3d marker_pose_eigen;
00781   goal_poses_[goal_name]->getPose(marker_pose_eigen);
00782 
00783   robot_state::RobotState ks(scene_display_->getPlanningSceneRO()->getCurrentState());
00784   ks.updateStateWithLinkAt(eef.parent_link, marker_pose_eigen);
00785   bool in_collision = isGroupCollidingWithWorld(ks, eef.eef_group);
00786 
00787   if (in_collision)
00788   {
00789     JobProcessing::addMainLoopJob(
00790         boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::IN_COLLISION));
00791   }
00792   else
00793   {
00794     JobProcessing::addMainLoopJob(
00795         boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::NOT_TESTED));
00796   }
00797 }
00798 
00799 void MainWindow::switchGoalPoseMarkerSelection(const std::string& marker_name)
00800 {
00801   if (robot_interaction_->getActiveEndEffectors().empty() || !goal_poses_[marker_name]->isVisible())
00802     return;
00803 
00804   if (goal_poses_[marker_name]->isSelected())
00805   {
00806     // If selected, unselect
00807     goal_poses_[marker_name]->unselect();
00808     setItemSelectionInList(marker_name, false, ui_.goal_poses_list);
00809   }
00810   else
00811   {
00812     // If unselected, select. Only display the gripper mesh for one
00813     if (ui_.goal_poses_list->selectedItems().size() == 1)
00814       goal_poses_[marker_name]->select(true);
00815     else
00816       goal_poses_[marker_name]->select(false);
00817     setItemSelectionInList(marker_name, true, ui_.goal_poses_list);
00818   }
00819 }
00820 
00821 void MainWindow::copySelectedGoalPoses(void)
00822 {
00823   QList<QListWidgetItem*> sel = ui_.goal_poses_list->selectedItems();
00824   if (sel.empty() || robot_interaction_->getActiveEndEffectors().empty())
00825     return;
00826 
00827   std::string scene_name;
00828   {
00829     const planning_scene_monitor::LockedPlanningSceneRO& ps = scene_display_->getPlanningSceneRO();
00830     if (!ps)
00831       return;
00832     else
00833       scene_name = ps->getName();
00834   }
00835 
00836   for (int i = 0; i < sel.size(); ++i)
00837   {
00838     std::string name = sel[i]->text().toStdString();
00839     if (!goal_poses_[name]->isVisible())
00840       continue;
00841 
00842     std::stringstream ss;
00843     ss << scene_name.c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();
00844 
00845     scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
00846     Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(
00847         robot_interaction_->getActiveEndEffectors()[0].parent_link);
00848     geometry_msgs::Pose marker_pose;
00849     marker_pose.position.x = goal_poses_[name]->imarker->getPosition().x;
00850     marker_pose.position.y = goal_poses_[name]->imarker->getPosition().y;
00851     marker_pose.position.z = goal_poses_[name]->imarker->getPosition().z;
00852     marker_pose.orientation.x = goal_poses_[name]->imarker->getOrientation().x;
00853     marker_pose.orientation.y = goal_poses_[name]->imarker->getOrientation().y;
00854     marker_pose.orientation.z = goal_poses_[name]->imarker->getOrientation().z;
00855     marker_pose.orientation.w = goal_poses_[name]->imarker->getOrientation().w;
00856 
00857     static const float marker_scale = 0.15;
00858     GripperMarkerPtr goal_pose(new GripperMarker(
00859         scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_,
00860         ss.str(), scene_display_->getRobotModel()->getModelFrame(), robot_interaction_->getActiveEndEffectors()[0],
00861         marker_pose, marker_scale, GripperMarker::NOT_TESTED, true));
00862 
00863     goal_poses_.insert(GoalPosePair(ss.str(), goal_pose));
00864 
00865     // Connect signals
00866     goal_pose->connect(this, SLOT(goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00867 
00868     // Unselect the marker source of the copy
00869     switchGoalPoseMarkerSelection(name);
00870   }
00871 
00872   JobProcessing::addMainLoopJob(boost::bind(&MainWindow::populateGoalPosesList, this));
00873 }
00874 
00875 void MainWindow::saveStartStateButtonClicked(void)
00876 {
00877   bool ok = false;
00878 
00879   std::stringstream ss;
00880   ss << scene_display_->getRobotModel()->getName().c_str() << "_state_" << std::setfill('0') << std::setw(4)
00881      << start_states_.size();
00882 
00883   QString text = QInputDialog::getText(this, tr("Choose a name"), tr("Start state name:"), QLineEdit::Normal,
00884                                        QString(ss.str().c_str()), &ok);
00885 
00886   std::string name;
00887   if (ok)
00888   {
00889     if (!text.isEmpty())
00890     {
00891       name = text.toStdString();
00892       if (start_states_.find(name) != start_states_.end())
00893         QMessageBox::warning(
00894             this, "Name already exists",
00895             QString("The name '").append(name.c_str()).append("' already exists. Not creating state."));
00896       else
00897       {
00898         // Store the current start state
00899         moveit_msgs::RobotState msg;
00900         robot_state::robotStateToRobotStateMsg(scene_display_->getPlanningSceneRO()->getCurrentState(), msg);
00901         start_states_.insert(StartStatePair(name, StartStatePtr(new StartState(msg))));
00902 
00903         // Save to the database if connected
00904         if (robot_state_storage_)
00905         {
00906           try
00907           {
00908             robot_state_storage_->addRobotState(msg, name);
00909           }
00910           catch (std::runtime_error& ex)
00911           {
00912             ROS_ERROR("Cannot save robot state on the database: %s", ex.what());
00913           }
00914         }
00915       }
00916     }
00917     else
00918       QMessageBox::warning(this, "Start state not saved", "Cannot use an empty name for a new start state.");
00919   }
00920   populateStartStatesList();
00921 }
00922 
00923 void MainWindow::removeSelectedStatesButtonClicked(void)
00924 {
00925   QList<QListWidgetItem*> found_items = ui_.start_states_list->selectedItems();
00926   for (unsigned int i = 0; i < found_items.size(); i++)
00927   {
00928     start_states_.erase(found_items[i]->text().toStdString());
00929   }
00930   populateStartStatesList();
00931 }
00932 
00933 void MainWindow::removeAllStatesButtonClicked(void)
00934 {
00935   start_states_.clear();
00936   populateStartStatesList();
00937 }
00938 
00939 void MainWindow::populateStartStatesList(void)
00940 {
00941   ui_.start_states_list->clear();
00942   for (StartStateMap::iterator it = start_states_.begin(); it != start_states_.end(); ++it)
00943   {
00944     QListWidgetItem* item = new QListWidgetItem(QString(it->first.c_str()));
00945     ui_.start_states_list->addItem(item);
00946     if (it->second->selected)
00947     {
00948       // If selected, highlight in the list
00949       item->setSelected(true);
00950     }
00951   }
00952 }
00953 
00954 void MainWindow::startStateItemDoubleClicked(QListWidgetItem* item)
00955 {
00956   scene_display_->getPlanningSceneRW()->setCurrentState(start_states_[item->text().toStdString()]->state_msg);
00957   scene_display_->queueRenderSceneGeometry();
00958 }
00959 
00960 void MainWindow::runBenchmark(void)
00961 {
00962   if (!robot_interaction_ || robot_interaction_->getActiveEndEffectors().size() == 0)
00963   {
00964     QMessageBox::warning(this, "Error", "Please make sure a planning group with an end-effector is active");
00965     return;
00966   }
00967 
00968   run_benchmark_ui_.benchmark_goal_text->setText(ui_.load_poses_filter_text->text());
00969   run_benchmark_ui_.benchmark_start_state_text->setText(ui_.load_states_filter_text->text());
00970 
00971   // Load available planners
00972   boost::shared_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
00973   std::map<std::string, planning_interface::PlannerManagerPtr> planner_interfaces;
00974   try
00975   {
00976     planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
00977         "moveit_core", "planning_interface::PlannerManager"));
00978   }
00979   catch (pluginlib::PluginlibException& ex)
00980   {
00981     ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
00982   }
00983 
00984   if (planner_plugin_loader)
00985   {
00986     // load the planning plugins
00987     const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
00988     for (std::size_t i = 0; i < classes.size(); ++i)
00989     {
00990       ROS_DEBUG("Attempting to load and configure %s", classes[i].c_str());
00991       try
00992       {
00993         planning_interface::PlannerManagerPtr p = planner_plugin_loader->createInstance(classes[i]);
00994         p->initialize(scene_display_->getPlanningSceneRO()->getRobotModel(), "");
00995         planner_interfaces[classes[i]] = p;
00996       }
00997       catch (pluginlib::PluginlibException& ex)
00998       {
00999         ROS_ERROR_STREAM("Exception while loading planner '" << classes[i] << "': " << ex.what());
01000       }
01001     }
01002 
01003     // Get the list of planning interfaces and planning algorithms
01004     if (!planner_interfaces.empty())
01005     {
01006       std::stringstream interfaces_ss, algorithms_ss;
01007       for (std::map<std::string, planning_interface::PlannerManagerPtr>::const_iterator it = planner_interfaces.begin();
01008            it != planner_interfaces.end(); ++it)
01009       {
01010         interfaces_ss << it->first << " ";
01011 
01012         std::vector<std::string> known;
01013         it->second->getPlanningAlgorithms(known);
01014         for (std::size_t i = 0; i < known.size(); ++i)
01015           algorithms_ss << known[i] << " ";
01016       }
01017       ROS_DEBUG("Available planner instances: %s. Available algorithms: %s", interfaces_ss.str().c_str(),
01018                 algorithms_ss.str().c_str());
01019       run_benchmark_ui_.planning_interfaces_text->setText(QString(interfaces_ss.str().c_str()));
01020       run_benchmark_ui_.planning_algorithms_text->setText(QString(algorithms_ss.str().c_str()));
01021     }
01022   }
01023 
01024   run_benchmark_dialog_->show();
01025 }
01026 
01027 void MainWindow::benchmarkFolderButtonClicked(void)
01028 {
01029   QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"), "/home",
01030                                                   QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
01031   run_benchmark_ui_.benchmark_output_folder_text->setText(dir);
01032 }
01033 
01034 void MainWindow::cancelBenchmarkButtonClicked(void)
01035 {
01036   run_benchmark_dialog_->close();
01037 }
01038 
01039 bool MainWindow::saveBenchmarkConfigButtonClicked(void)
01040 {
01041   if (run_benchmark_ui_.benchmark_output_folder_text->text().isEmpty())
01042   {
01043     QMessageBox::warning(this, "Missing data", "Must specify an output folder");
01044     return false;
01045   }
01046 
01047   QString outfilename = run_benchmark_ui_.benchmark_output_folder_text->text().append("/config.cfg");
01048   std::ofstream outfile(outfilename.toUtf8());
01049   if (outfile)
01050   {
01051     outfile << "[scene]" << std::endl;
01052     outfile << "group=" << ui_.planning_group_combo->currentText().toStdString() << std::endl;
01053     outfile << "default_constrained_link=" << robot_interaction_->getActiveEndEffectors()[0].parent_link << std::endl;
01054     outfile << "planning_frame=" << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getModelFrame()
01055             << std::endl;
01056     outfile << "name=" << scene_display_->getPlanningSceneRO()->getName() << std::endl;
01057 
01058     outfile << "timeout=" << run_benchmark_ui_.timeout_spin->value() << std::endl;
01059     outfile << "runs=" << run_benchmark_ui_.number_of_runs_spin->value() << std::endl;
01060     outfile << "output=" << run_benchmark_ui_.benchmark_output_folder_text->text().toStdString() << "/"
01061             << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getName() << "_"
01062             << scene_display_->getPlanningSceneRO()->getName() << "_" << ros::Time::now() << std::endl;
01063     outfile << "start=" << run_benchmark_ui_.benchmark_start_state_text->text().toStdString() << std::endl;
01064     outfile << "query=" << std::endl;
01065     outfile << "goal=" << run_benchmark_ui_.benchmark_goal_text->text().toStdString() << std::endl;
01066     outfile << "goal_offset_roll=" << ui_.goal_offset_roll->value() << std::endl;
01067     outfile << "goal_offset_pitch=" << ui_.goal_offset_pitch->value() << std::endl;
01068     outfile << "goal_offset_yaw=" << ui_.goal_offset_yaw->value() << std::endl << std::endl;
01069 
01070     outfile << "[plugin]" << std::endl;
01071     outfile << "name=" << run_benchmark_ui_.planning_interfaces_text->text().toStdString() << std::endl;
01072     outfile << "planners=" << run_benchmark_ui_.planning_algorithms_text->text().toStdString() << std::endl;
01073 
01074     outfile.close();
01075 
01076     run_benchmark_dialog_->close();
01077   }
01078   else
01079   {
01080     QMessageBox::warning(this, "Error", QString("Cannot open file ").append(outfilename).append(" for writing"));
01081     return false;
01082   }
01083 
01084   return true;
01085 }
01086 
01087 void MainWindow::runBenchmarkButtonClicked(void)
01088 {
01089   if (!saveBenchmarkConfigButtonClicked())
01090     return;
01091 
01092   QMessageBox msgBox;
01093   msgBox.setText("This will run the benchmark pipeline. The process will take several minutes during which you will "
01094                  "not be able to interact with the interface. You can follow the progress in the console output");
01095   msgBox.setInformativeText("Do you want to continue?");
01096   msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
01097   msgBox.setDefaultButton(QMessageBox::Yes);
01098   int ret = msgBox.exec();
01099 
01100   switch (ret)
01101   {
01102     case QMessageBox::Yes:
01103     {
01104       // Set up db
01105       warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase();
01106       conn->setParams(database_host_, database_port_, 10.0);
01107       if (!conn->connect())
01108       {
01109         QMessageBox::warning(this, "Error", QString("Unable to connect to Database"));
01110         break;
01111       }
01112       QString outfilename = run_benchmark_ui_.benchmark_output_folder_text->text().append("/config.cfg");
01113       moveit_benchmarks::BenchmarkType btype = 0;
01114       moveit_benchmarks::BenchmarkExecution be(scene_display_->getPlanningSceneMonitor()->getPlanningScene(), conn);
01115       if (run_benchmark_ui_.benchmark_include_planners_checkbox->isChecked())
01116         btype += moveit_benchmarks::BENCHMARK_PLANNERS;
01117       if (run_benchmark_ui_.benchmark_check_reachability_checkbox->isChecked())
01118         btype += moveit_benchmarks::BENCHMARK_GOAL_EXISTANCE;
01119 
01120       if (be.readOptions(outfilename.toStdString()))
01121       {
01122         std::stringstream ss;
01123         be.printOptions(ss);
01124         ROS_INFO_STREAM("Calling benchmark with options:" << std::endl << ss.str() << std::endl);
01125 
01126         BenchmarkProcessingThread benchmark_thread(be, btype, this);
01127         benchmark_thread.startAndShow();
01128 
01129         if (benchmark_thread.isRunning())
01130         {
01131           benchmark_thread.terminate();
01132           benchmark_thread.wait();
01133           QMessageBox::warning(this, "", "Benchmark computation canceled");
01134         }
01135         else
01136         {
01137           QMessageBox::information(
01138               this, "Benchmark computation finished",
01139               QString("The results were logged into '").append(run_benchmark_ui_.benchmark_output_folder_text->text()));
01140         }
01141       }
01142 
01143       break;
01144     }
01145   }
01146 }
01147 
01148 void MainWindow::loadBenchmarkResults(void)
01149 {
01150   // Select a log file of the set.
01151   QString path =
01152       QFileDialog::getOpenFileName(this, tr("Select a log file in the set"), tr(""), tr("Log files (*.log)"));
01153   if (!path.isEmpty())
01154   {
01155     JobProcessing::addBackgroundJob(boost::bind(&MainWindow::computeLoadBenchmarkResults, this, path.toStdString()));
01156   }
01157 }
01158 
01159 void MainWindow::computeLoadBenchmarkResults(const std::string& file)
01160 {
01161   std::string logid, basename, logid_text;
01162   try
01163   {
01164     logid = file.substr(file.find_last_of(".", file.length() - 5) + 1,
01165                         file.find_last_of(".") - file.find_last_of(".", file.length() - 5) - 1);
01166     basename = file.substr(0, file.find_last_of(".", file.length() - 5));
01167     logid_text = logid.substr(logid.find_first_of("_"), logid.length() - logid.find_first_of("_"));
01168   }
01169   catch (...)
01170   {
01171     ROS_ERROR("Invalid benchmark log file. Cannot load results.");
01172     return;
01173   }
01174 
01175   int count = 1;
01176   bool more_files = true;
01177   // Parse all the log files in the set
01178   while (more_files)
01179   {
01180     std::ifstream ifile;
01181     std::stringstream file_to_load;
01182     file_to_load << basename << "." << count << logid_text << ".log";
01183 
01184     ifile.open(file_to_load.str().c_str());
01185     if (ifile.good())
01186     {
01187       // Parse results
01188       char text_line[512];
01189       static std::streamsize text_line_length = 512;
01190       bool valid_file = false;
01191       ifile.getline(text_line, text_line_length);
01192 
01193       // Check if the file is valid. The first line must contain "Experiment <scene>". There must be a "total_time"
01194       // line, followed by the results
01195       if (ifile.good() && strstr(text_line, "Experiment") != NULL && strlen(text_line) > 11 &&
01196           strncmp(&text_line[11], scene_display_->getPlanningSceneRO()->getName().c_str(),
01197                   scene_display_->getPlanningSceneRO()->getName().length()) == 0)
01198       {
01199         while (ifile.good())
01200         {
01201           if (strstr(text_line, "total_time REAL") != NULL)
01202           {
01203             valid_file = true;
01204             break;
01205           }
01206 
01207           ifile.getline(text_line, text_line_length);
01208         }
01209       }
01210       else
01211       {
01212         ROS_ERROR("Not a valid log file, or a different planning scene loaded");
01213       }
01214 
01215       if (valid_file)
01216       {
01217         if (basename.find(".trajectory") != std::string::npos)
01218         {
01219           // Trajectory results may contain multiple goals
01220           int nwaypoint = 0;
01221           while (ifile.getline(text_line, text_line_length) && ifile.good() && strlen(text_line) > 6)
01222           {
01223             // This should be a reachability results line composed of reachable, collision-free and time fields (bool;
01224             // bool; float)
01225             try
01226             {
01227               bool reachable = boost::lexical_cast<bool>(text_line[0]);
01228               bool collision_free = boost::lexical_cast<bool>(text_line[3]);
01229 
01230               // Update colors accordingly
01231               if (count <= trajectories_.size())
01232               {
01233                 std::string trajectory_name = ui_.trajectory_list->item(count - 1)->text().toStdString();
01234                 if (nwaypoint < trajectories_[trajectory_name]->waypoint_markers.size())
01235                 {
01236                   if (reachable)
01237                   {
01238                     if (collision_free)
01239                     {
01240                       // Reachable and collision-free
01241                       JobProcessing::addMainLoopJob(boost::bind(
01242                           &MainWindow::updateMarkerState, this,
01243                           trajectories_[trajectory_name]->waypoint_markers[nwaypoint], GripperMarker::REACHABLE));
01244                     }
01245                     else
01246                     {
01247                       // Reachable, but in collision
01248                       JobProcessing::addMainLoopJob(boost::bind(
01249                           &MainWindow::updateMarkerState, this,
01250                           trajectories_[trajectory_name]->waypoint_markers[nwaypoint], GripperMarker::IN_COLLISION));
01251                     }
01252                   }
01253                   else
01254                   {
01255                     // Not reachable
01256                     JobProcessing::addMainLoopJob(boost::bind(
01257                         &MainWindow::updateMarkerState, this,
01258                         trajectories_[trajectory_name]->waypoint_markers[nwaypoint], GripperMarker::NOT_REACHABLE));
01259                   }
01260                 }
01261               }
01262               nwaypoint++;
01263             }
01264             catch (...)
01265             {
01266               ROS_ERROR("Error parsing the log file");
01267             }
01268           }
01269         }
01270         else
01271         {
01272           ifile.getline(text_line, text_line_length);
01273           if (ifile.good() && strlen(text_line) > 6)
01274           {
01275             // This should be the reachability results line composed of reachable, collision-free and time fields (bool;
01276             // bool; float)
01277             try
01278             {
01279               bool reachable = boost::lexical_cast<bool>(text_line[0]);
01280               bool collision_free = boost::lexical_cast<bool>(text_line[3]);
01281 
01282               // Update colors accordingly
01283               if (count <= goal_poses_.size())
01284               {
01285                 std::string goal_name = ui_.goal_poses_list->item(count - 1)->text().toStdString();
01286                 if (reachable)
01287                 {
01288                   if (collision_free)
01289                   {
01290                     // Reachable and collision-free
01291                     JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this,
01292                                                               goal_name, GripperMarker::REACHABLE));
01293                   }
01294                   else
01295                   {
01296                     // Reachable, but in collision
01297                     JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this,
01298                                                               goal_name, GripperMarker::IN_COLLISION));
01299                   }
01300                 }
01301                 else
01302                 {
01303                   // Not reachable
01304                   JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
01305                                                             GripperMarker::NOT_REACHABLE));
01306                 }
01307               }
01308             }
01309             catch (...)
01310             {
01311               ROS_ERROR("Error parsing the log file");
01312             }
01313           }
01314         }
01315       }
01316       else
01317       {
01318         ROS_ERROR("Invalid benchmark log file. Cannot load results.");
01319       }
01320 
01321       ifile.close();
01322     }
01323     else
01324     {
01325       more_files = false;
01326     }
01327     count++;
01328   }
01329 }
01330 
01331 void MainWindow::updateGoalMarkerStateFromName(const std::string& name, const GripperMarker::GripperMarkerState& state)
01332 {
01333   if (goal_poses_.find(name) != goal_poses_.end())
01334     goal_poses_[name]->setState(state);
01335 }
01336 
01337 void MainWindow::updateMarkerState(GripperMarkerPtr marker, const GripperMarker::GripperMarkerState& state)
01338 {
01339   marker->setState(state);
01340 }
01341 
01342 void MainWindow::goalOffsetChanged()
01343 {
01344   goal_offset_.setIdentity();
01345   goal_offset_ = Eigen::AngleAxisd(ui_.goal_offset_roll->value() * boost::math::constants::pi<double>() / 180.0,
01346                                    Eigen::Vector3d::UnitX()) *
01347                  Eigen::AngleAxisd(ui_.goal_offset_pitch->value() * boost::math::constants::pi<double>() / 180.0,
01348                                    Eigen::Vector3d::UnitY()) *
01349                  Eigen::AngleAxisd(ui_.goal_offset_yaw->value() * boost::math::constants::pi<double>() / 180.0,
01350                                    Eigen::Vector3d::UnitZ());
01351 
01352   if (!robot_interaction_->getActiveEndEffectors().empty())
01353   {
01354     Eigen::Affine3d current_pose;
01355     for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
01356     {
01357       current_pose = goals_initial_pose_[it->first] * goal_offset_;
01358       it->second->setPose(current_pose);
01359     }
01360   }
01361 }
01362 
01363 }  // namespace


benchmarks_gui
Author(s): Mario Prats
autogenerated on Mon Jul 24 2017 02:22:21