motion_planning_frame_states.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mario Prats, Ioan Sucan */
36 
41 
42 #include <QMessageBox>
43 #include <QInputDialog>
44 
45 #include "ui_motion_planning_rviz_plugin_frame.h"
46 
47 namespace moveit_rviz_plugin
48 {
50 {
51  ui_->list_states->clear();
52  for (RobotStateMap::iterator it = robot_states_.begin(); it != robot_states_.end(); ++it)
53  {
54  QListWidgetItem* item = new QListWidgetItem(QString(it->first.c_str()));
55  ui_->list_states->addItem(item);
56  }
57 }
58 
60 {
62  {
63  bool ok;
64 
65  QString text =
66  QInputDialog::getText(this, tr("Robot states to load"), tr("Pattern:"), QLineEdit::Normal, ".*", &ok);
67  if (ok && !text.isEmpty())
68  {
69  loadStoredStates(text.toStdString());
70  }
71  }
72  else
73  {
74  QMessageBox::warning(this, "Warning", "Not connected to a database.");
75  }
76 }
77 
78 void MotionPlanningFrame::loadStoredStates(const std::string& pattern)
79 {
80  std::vector<std::string> names;
81  try
82  {
83  robot_state_storage_->getKnownRobotStates(pattern, names);
84  }
85  catch (std::exception& ex)
86  {
87  QMessageBox::warning(this, "Cannot query the database",
88  QString("Wrongly formatted regular expression for robot states: ").append(ex.what()));
89  return;
90  }
91 
92  // Clear the current list
94 
95  for (std::size_t i = 0; i < names.size(); ++i)
96  {
98  bool got_state = false;
99  try
100  {
101  got_state = robot_state_storage_->getRobotState(rs, names[i]);
102  }
103  catch (std::exception& ex)
104  {
105  ROS_ERROR("%s", ex.what());
106  }
107  if (!got_state)
108  continue;
109 
110  // Overwrite if exists.
111  if (robot_states_.find(names[i]) != robot_states_.end())
112  {
113  robot_states_.erase(names[i]);
114  }
115 
116  // Store the current start state
117  robot_states_.insert(RobotStatePair(names[i], *rs));
118  }
120 }
121 
122 void MotionPlanningFrame::saveRobotStateButtonClicked(const robot_state::RobotState& state)
123 {
124  bool ok = false;
125 
126  std::stringstream ss;
127  ss << planning_display_->getRobotModel()->getName().c_str() << "_state_" << std::setfill('0') << std::setw(4)
128  << robot_states_.size();
129 
130  QString text = QInputDialog::getText(this, tr("Choose a name"), tr("State name:"), QLineEdit::Normal,
131  QString(ss.str().c_str()), &ok);
132 
133  std::string name;
134  if (ok)
135  {
136  if (!text.isEmpty())
137  {
138  name = text.toStdString();
139  if (robot_states_.find(name) != robot_states_.end())
140  QMessageBox::warning(
141  this, "Name already exists",
142  QString("The name '").append(name.c_str()).append("' already exists. Not creating state."));
143  else
144  {
145  // Store the current start state
146  moveit_msgs::RobotState msg;
147  robot_state::robotStateToRobotStateMsg(state, msg);
148  robot_states_.insert(RobotStatePair(name, msg));
149 
150  // Save to the database if connected
152  {
153  try
154  {
155  robot_state_storage_->addRobotState(msg, name, planning_display_->getRobotModel()->getName());
156  }
157  catch (std::exception& ex)
158  {
159  ROS_ERROR("Cannot save robot state on the database: %s", ex.what());
160  }
161  }
162  else
163  {
164  QMessageBox::warning(this, "Warning",
165  "Not connected to a database. The state will be created but not stored");
166  }
167  }
168  }
169  else
170  QMessageBox::warning(this, "Start state not saved", "Cannot use an empty name for a new start state.");
171  }
173 }
174 
176 {
178 }
179 
181 {
183 }
184 
186 {
187  QListWidgetItem* item = ui_->list_states->currentItem();
188 
189  if (item)
190  {
191  robot_state::RobotState robot_state(*planning_display_->getQueryStartState());
192  robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state);
194  }
195 }
196 
198 {
199  QListWidgetItem* item = ui_->list_states->currentItem();
200 
201  if (item)
202  {
203  robot_state::RobotState robot_state(*planning_display_->getQueryGoalState());
204  robot_state::robotStateMsgToRobotState(robot_states_[item->text().toStdString()], robot_state);
205  planning_display_->setQueryGoalState(robot_state);
206  }
207 }
208 
210 {
212  {
213  // Warn the user
214  QMessageBox msgBox;
215  msgBox.setText("All the selected states will be removed from the database");
216  msgBox.setInformativeText("Do you want to continue?");
217  msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
218  msgBox.setDefaultButton(QMessageBox::No);
219  int ret = msgBox.exec();
220 
221  switch (ret)
222  {
223  case QMessageBox::Yes:
224  {
225  QList<QListWidgetItem*> found_items = ui_->list_states->selectedItems();
226  for (int i = 0; i < found_items.size(); ++i)
227  {
228  const std::string& name = found_items[i]->text().toStdString();
229  try
230  {
231  robot_state_storage_->removeRobotState(name);
232  robot_states_.erase(name);
233  }
234  catch (std::exception& ex)
235  {
236  ROS_ERROR("%s", ex.what());
237  }
238  }
239  break;
240  }
241  }
242  }
244 }
245 
247 {
248  robot_states_.clear();
250 }
251 
252 } // namespace
robot_state::RobotStateConstPtr getQueryStartState() const
void setQueryStartState(const robot_state::RobotState &start)
text
void setQueryGoalState(const robot_state::RobotState &goal)
moveit_warehouse::RobotStateStoragePtr robot_state_storage_
void saveRobotStateButtonClicked(const robot_state::RobotState &state)
robot_state::RobotStateConstPtr getQueryGoalState() const
const robot_model::RobotModelConstPtr & getRobotModel() const
std::pair< std::string, moveit_msgs::RobotState > RobotStatePair
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
#define ROS_ERROR(...)


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:04:23