motion_planning_frame_scenes.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: Ioan Sucan */
00036 
00037 #include <moveit/warehouse/planning_scene_storage.h>
00038 #include <moveit/warehouse/constraints_storage.h>
00039 #include <moveit/warehouse/state_storage.h>
00040 
00041 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00042 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
00043 #include <moveit/kinematic_constraints/utils.h>
00044 #include <moveit/robot_state/conversions.h>
00045 #include <moveit/robot_interaction/interactive_marker_helpers.h>
00046 
00047 #include <interactive_markers/tools.h>
00048 
00049 #include <rviz/display_context.h>
00050 #include <rviz/window_manager_interface.h>
00051 
00052 #include <eigen_conversions/eigen_msg.h>
00053 
00054 #include <QMessageBox>
00055 #include <QInputDialog>
00056 
00057 #include "ui_motion_planning_rviz_plugin_frame.h"
00058 
00059 #include <boost/math/constants/constants.hpp>
00060 
00061 namespace moveit_rviz_plugin
00062 {
00063 void MotionPlanningFrame::saveSceneButtonClicked()
00064 {
00065   if (planning_scene_storage_)
00066   {
00067     const std::string& name = planning_display_->getPlanningSceneRO()->getName();
00068     if (name.empty() || planning_scene_storage_->hasPlanningScene(name))
00069     {
00070       boost::scoped_ptr<QMessageBox> q;
00071       if (name.empty())
00072         q.reset(new QMessageBox(QMessageBox::Question, "Change Planning Scene Name",
00073                                 QString("The name for the planning scene should not be empty. Would you like to rename "
00074                                         "the planning scene?'"),
00075                                 QMessageBox::Cancel, this));
00076       else
00077         q.reset(new QMessageBox(QMessageBox::Question, "Confirm Planning Scene Overwrite",
00078                                 QString("A planning scene named '")
00079                                     .append(name.c_str())
00080                                     .append("' already exists. Do you wish to "
00081                                             "overwrite that scene?"),
00082                                 QMessageBox::Yes | QMessageBox::No, this));
00083       boost::scoped_ptr<QPushButton> rename(q->addButton("&Rename", QMessageBox::AcceptRole));
00084       if (q->exec() != QMessageBox::Yes)
00085       {
00086         if (q->clickedButton() == rename.get())
00087         {
00088           bool ok = false;
00089           QString new_name = QInputDialog::getText(this, "Rename Planning Scene", "New name for the planning scene:",
00090                                                    QLineEdit::Normal, QString::fromStdString(name), &ok);
00091           if (ok)
00092           {
00093             planning_display_->getPlanningSceneRW()->setName(new_name.toStdString());
00094             rviz::Property* prop = planning_display_->subProp("Scene Geometry")->subProp("Scene Name");
00095             if (prop)
00096             {
00097               bool old = prop->blockSignals(true);
00098               prop->setValue(new_name);
00099               prop->blockSignals(old);
00100             }
00101             saveSceneButtonClicked();
00102           }
00103           return;
00104         }
00105         return;
00106       }
00107     }
00108 
00109     planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this),
00110                                         "save scene");
00111   }
00112 }
00113 
00114 void MotionPlanningFrame::planningSceneItemClicked()
00115 {
00116   checkPlanningSceneTreeEnabledButtons();
00117 }
00118 
00119 void MotionPlanningFrame::saveQueryButtonClicked()
00120 {
00121   if (planning_scene_storage_)
00122   {
00123     QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
00124     if (!sel.empty())
00125     {
00126       QTreeWidgetItem* s = sel.front();
00127 
00128       // if we have selected a PlanningScene, add the query as a new one, under that planning scene
00129       if (s->type() == ITEM_TYPE_SCENE)
00130       {
00131         std::string scene = s->text(0).toStdString();
00132         planning_display_->addBackgroundJob(
00133             boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query");
00134       }
00135       else
00136       {
00137         // if we selected a query name, then we overwrite that query
00138         std::string scene = s->parent()->text(0).toStdString();
00139         std::string query_name = s->text(0).toStdString();
00140 
00141         while (query_name.empty() || planning_scene_storage_->hasPlanningQuery(scene, query_name))
00142         {
00143           boost::scoped_ptr<QMessageBox> q;
00144           if (query_name.empty())
00145             q.reset(new QMessageBox(QMessageBox::Question, "Change Planning Query Name",
00146                                     QString("The name for the planning query should not be empty. Would you like to "
00147                                             "rename the planning query?'"),
00148                                     QMessageBox::Cancel, this));
00149           else
00150             q.reset(new QMessageBox(QMessageBox::Question, "Confirm Planning Query Overwrite",
00151                                     QString("A planning query named '")
00152                                         .append(query_name.c_str())
00153                                         .append("' already exists. Do you wish "
00154                                                 "to overwrite that query?"),
00155                                     QMessageBox::Yes | QMessageBox::No, this));
00156           boost::scoped_ptr<QPushButton> rename(q->addButton("&Rename", QMessageBox::AcceptRole));
00157           if (q->exec() == QMessageBox::Yes)
00158             break;
00159           else
00160           {
00161             if (q->clickedButton() == rename.get())
00162             {
00163               bool ok = false;
00164               QString new_name =
00165                   QInputDialog::getText(this, "Rename Planning Query", "New name for the planning query:",
00166                                         QLineEdit::Normal, QString::fromStdString(query_name), &ok);
00167               if (ok)
00168                 query_name = new_name.toStdString();
00169               else
00170                 return;
00171             }
00172             else
00173               return;
00174           }
00175         }
00176         planning_display_->addBackgroundJob(
00177             boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query");
00178       }
00179     }
00180   }
00181 }
00182 
00183 void MotionPlanningFrame::deleteSceneButtonClicked()
00184 {
00185   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this),
00186                                       "delete scene");
00187 }
00188 
00189 void MotionPlanningFrame::deleteQueryButtonClicked()
00190 {
00191   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this),
00192                                       "delete query");
00193 }
00194 
00195 void MotionPlanningFrame::loadSceneButtonClicked()
00196 {
00197   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this), "load "
00198                                                                                                               "scene");
00199 }
00200 
00201 void MotionPlanningFrame::loadQueryButtonClicked()
00202 {
00203   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this), "load "
00204                                                                                                               "query");
00205 }
00206 
00207 void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int column)
00208 {
00209   if (item->text(column) == item->toolTip(column) || item->toolTip(column).length() == 0)
00210     return;
00211   moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage = planning_scene_storage_;
00212   if (!planning_scene_storage)
00213     return;
00214 
00215   if (item->type() == ITEM_TYPE_SCENE)
00216   {
00217     std::string new_name = item->text(column).toStdString();
00218 
00219     if (planning_scene_storage->hasPlanningScene(new_name))
00220     {
00221       planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
00222       QMessageBox::warning(this, "Scene not renamed",
00223                            QString("The scene name '").append(item->text(column)).append("' already exists"));
00224       return;
00225     }
00226     else
00227     {
00228       std::string old_name = item->toolTip(column).toStdString();
00229       planning_scene_storage->renamePlanningScene(old_name, new_name);
00230       item->setToolTip(column, item->text(column));
00231     }
00232   }
00233   else
00234   {
00235     std::string scene = item->parent()->text(0).toStdString();
00236     std::string new_name = item->text(column).toStdString();
00237     if (planning_scene_storage->hasPlanningQuery(scene, new_name))
00238     {
00239       planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
00240       QMessageBox::warning(this, "Query not renamed", QString("The query name '")
00241                                                           .append(item->text(column))
00242                                                           .append("' already exists for scene ")
00243                                                           .append(item->parent()->text(0)));
00244       return;
00245     }
00246     else
00247     {
00248       std::string old_name = item->toolTip(column).toStdString();
00249       planning_scene_storage->renamePlanningQuery(scene, old_name, new_name);
00250       item->setToolTip(column, item->text(column));
00251     }
00252   }
00253 }
00254 
00255 void MotionPlanningFrame::populatePlanningSceneTreeView()
00256 {
00257   moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage = planning_scene_storage_;
00258   if (!planning_scene_storage)
00259     return;
00260 
00261   ui_->planning_scene_tree->setUpdatesEnabled(false);
00262 
00263   // remember which items were expanded
00264   std::set<std::string> expanded;
00265   for (int i = 0; i < ui_->planning_scene_tree->topLevelItemCount(); ++i)
00266   {
00267     QTreeWidgetItem* it = ui_->planning_scene_tree->topLevelItem(i);
00268     if (it->isExpanded())
00269       expanded.insert(it->text(0).toStdString());
00270   }
00271 
00272   ui_->planning_scene_tree->clear();
00273   std::vector<std::string> names;
00274   planning_scene_storage->getPlanningSceneNames(names);
00275 
00276   for (std::size_t i = 0; i < names.size(); ++i)
00277   {
00278     std::vector<std::string> query_names;
00279     planning_scene_storage->getPlanningQueriesNames(query_names, names[i]);
00280     QTreeWidgetItem* item =
00281         new QTreeWidgetItem(ui_->planning_scene_tree, QStringList(QString::fromStdString(names[i])), ITEM_TYPE_SCENE);
00282     item->setFlags(item->flags() | Qt::ItemIsEditable);
00283     item->setToolTip(0, item->text(0));  // we use the tool tip as a backup of the old name when renaming
00284     for (std::size_t j = 0; j < query_names.size(); ++j)
00285     {
00286       QTreeWidgetItem* subitem =
00287           new QTreeWidgetItem(item, QStringList(QString::fromStdString(query_names[j])), ITEM_TYPE_QUERY);
00288       subitem->setFlags(subitem->flags() | Qt::ItemIsEditable);
00289       subitem->setToolTip(0, subitem->text(0));
00290       item->addChild(subitem);
00291     }
00292 
00293     ui_->planning_scene_tree->insertTopLevelItem(ui_->planning_scene_tree->topLevelItemCount(), item);
00294     if (expanded.find(names[i]) != expanded.end())
00295       ui_->planning_scene_tree->expandItem(item);
00296   }
00297   ui_->planning_scene_tree->sortItems(0, Qt::AscendingOrder);
00298   ui_->planning_scene_tree->setUpdatesEnabled(true);
00299   checkPlanningSceneTreeEnabledButtons();
00300 }
00301 }


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:25:00