motion_planning_frame_states.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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 <moveit/warehouse/state_storage.h>
00038 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
00039 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
00040 #include <moveit/robot_state/conversions.h>
00041 
00042 #include <QMessageBox>
00043 #include <QInputDialog>
00044 
00045 #include "ui_motion_planning_rviz_plugin_frame.h"
00046 
00047 namespace moveit_rviz_plugin
00048 {
00049 void MotionPlanningFrame::populateRobotStatesList(void)
00050 {
00051   ui_->list_states->clear();
00052   for (RobotStateMap::iterator it = robot_states_.begin(); it != robot_states_.end(); ++it)
00053   {
00054     QListWidgetItem* item = new QListWidgetItem(QString(it->first.c_str()));
00055     ui_->list_states->addItem(item);
00056   }
00057 }
00058 
00059 void MotionPlanningFrame::loadStateButtonClicked()
00060 {
00061   if (robot_state_storage_)
00062   {
00063     bool ok;
00064 
00065     QString text =
00066         QInputDialog::getText(this, tr("Robot states to load"), tr("Pattern:"), QLineEdit::Normal, ".*", &ok);
00067     if (ok && !text.isEmpty())
00068     {
00069       loadStoredStates(text.toStdString());
00070     }
00071   }
00072   else
00073   {
00074     QMessageBox::warning(this, "Warning", "Not connected to a database.");
00075   }
00076 }
00077 
00078 void MotionPlanningFrame::loadStoredStates(const std::string& pattern)
00079 {
00080   std::vector<std::string> names;
00081   try
00082   {
00083     robot_state_storage_->getKnownRobotStates(pattern, names);
00084   }
00085   catch (std::runtime_error& ex)
00086   {
00087     QMessageBox::warning(this, "Cannot query the database",
00088                          QString("Wrongly formatted regular expression for robot states: ").append(ex.what()));
00089     return;
00090   }
00091 
00092   // Clear the current list
00093   clearStatesButtonClicked();
00094 
00095   for (std::size_t i = 0; i < names.size(); ++i)
00096   {
00097     moveit_warehouse::RobotStateWithMetadata rs;
00098     bool got_state = false;
00099     try
00100     {
00101       got_state = robot_state_storage_->getRobotState(rs, names[i]);
00102     }
00103     catch (std::runtime_error& ex)
00104     {
00105       ROS_ERROR("%s", ex.what());
00106     }
00107     if (!got_state)
00108       continue;
00109 
00110     // Overwrite if exists.
00111     if (robot_states_.find(names[i]) != robot_states_.end())
00112     {
00113       robot_states_.erase(names[i]);
00114     }
00115 
00116     // Store the current start state
00117     robot_states_.insert(RobotStatePair(names[i], *rs));
00118   }
00119   populateRobotStatesList();
00120 }
00121 
00122 void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotState& state)
00123 {
00124   bool ok = false;
00125 
00126   std::stringstream ss;
00127   ss << planning_display_->getRobotModel()->getName().c_str() << "_state_" << std::setfill('0') << std::setw(4)
00128      << robot_states_.size();
00129 
00130   QString text = QInputDialog::getText(this, tr("Choose a name"), tr("State name:"), QLineEdit::Normal,
00131                                        QString(ss.str().c_str()), &ok);
00132 
00133   std::string name;
00134   if (ok)
00135   {
00136     if (!text.isEmpty())
00137     {
00138       name = text.toStdString();
00139       if (robot_states_.find(name) != robot_states_.end())
00140         QMessageBox::warning(
00141             this, "Name already exists",
00142             QString("The name '").append(name.c_str()).append("' already exists. Not creating state."));
00143       else
00144       {
00145         // Store the current start state
00146         moveit_msgs::RobotState msg;
00147         robot_state::robotStateToRobotStateMsg(state, msg);
00148         robot_states_.insert(RobotStatePair(name, msg));
00149 
00150         // Save to the database if connected
00151         if (robot_state_storage_)
00152         {
00153           try
00154           {
00155             robot_state_storage_->addRobotState(msg, name, planning_display_->getRobotModel()->getName());
00156           }
00157           catch (std::runtime_error& ex)
00158           {
00159             ROS_ERROR("Cannot save robot state on the database: %s", ex.what());
00160           }
00161         }
00162         else
00163         {
00164           QMessageBox::warning(this, "Warning", "Not connected to a database. The state will be created but not "
00165                                                 "stored");
00166         }
00167       }
00168     }
00169     else
00170       QMessageBox::warning(this, "Start state not saved", "Cannot use an empty name for a new start state.");
00171   }
00172   populateRobotStatesList();
00173 }
00174 
00175 void MotionPlanningFrame::saveStartStateButtonClicked()
00176 {
00177   saveRobotStateButtonClicked(*planning_display_->getQueryStartState());
00178 }
00179 
00180 void MotionPlanningFrame::saveGoalStateButtonClicked()
00181 {
00182   saveRobotStateButtonClicked(*planning_display_->getQueryGoalState());
00183 }
00184 
00185 void MotionPlanningFrame::setAsStartStateButtonClicked()
00186 {
00187   QListWidgetItem* item = ui_->list_states->currentItem();
00188 
00189   if (item)
00190   {
00191     robot_state::RobotState robot_state(*planning_display_->getQueryStartState());
00192     robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state);
00193     planning_display_->setQueryStartState(robot_state);
00194   }
00195 }
00196 
00197 void MotionPlanningFrame::setAsGoalStateButtonClicked()
00198 {
00199   QListWidgetItem* item = ui_->list_states->currentItem();
00200 
00201   if (item)
00202   {
00203     robot_state::RobotState robot_state(*planning_display_->getQueryGoalState());
00204     robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state);
00205     planning_display_->setQueryGoalState(robot_state);
00206   }
00207 }
00208 
00209 void MotionPlanningFrame::removeStateButtonClicked()
00210 {
00211   if (robot_state_storage_)
00212   {
00213     // Warn the user
00214     QMessageBox msgBox;
00215     msgBox.setText("All the selected states will be removed from the database");
00216     msgBox.setInformativeText("Do you want to continue?");
00217     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
00218     msgBox.setDefaultButton(QMessageBox::No);
00219     int ret = msgBox.exec();
00220 
00221     switch (ret)
00222     {
00223       case QMessageBox::Yes:
00224       {
00225         QList<QListWidgetItem*> found_items = ui_->list_states->selectedItems();
00226         for (std::size_t i = 0; i < found_items.size(); ++i)
00227         {
00228           const std::string& name = found_items[i]->text().toStdString();
00229           try
00230           {
00231             robot_state_storage_->removeRobotState(name);
00232             robot_states_.erase(name);
00233           }
00234           catch (std::runtime_error& ex)
00235           {
00236             ROS_ERROR("%s", ex.what());
00237           }
00238         }
00239         break;
00240       }
00241     }
00242   }
00243   populateRobotStatesList();
00244 }
00245 
00246 void MotionPlanningFrame::clearStatesButtonClicked()
00247 {
00248   robot_states_.clear();
00249   populateRobotStatesList();
00250 }
00251 
00252 }  // namespace


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