motion_planning_frame_objects.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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: Ioan Sucan, Mario Prats */
36 
38 
43 
45 
46 #include <rviz/display_context.h>
47 #include <rviz/frame_manager.h>
49 
52 
53 #include <QMessageBox>
54 #include <QInputDialog>
55 #include <QFileDialog>
56 
57 #include "ui_motion_planning_rviz_plugin_frame.h"
58 
59 namespace moveit_rviz_plugin
60 {
62 {
63  QString path = QFileDialog::getOpenFileName(this, tr("Import Object"));
64  if (!path.isEmpty())
65  importResource("file://" + path.toStdString());
66 }
67 
69 {
70  bool ok = false;
71  QString url = QInputDialog::getText(this, tr("Import Object"), tr("URL for file to import:"), QLineEdit::Normal,
72  QString("http://"), &ok);
73  if (ok && !url.isEmpty())
74  importResource(url.toStdString());
75 }
76 
78 {
80  if (ps)
81  {
82  ps->getWorldNonConst()->clearObjects();
83  ps->getCurrentStateNonConst().clearAttachedBodies();
84  moveit_msgs::PlanningScene msg;
85  ps->getPlanningSceneMsg(msg);
89  }
90 }
91 
93 {
94  if (scaled_object_)
95  {
97  if (ps)
98  {
99  if (ps->getWorld()->hasObject(scaled_object_->id_))
100  {
101  ps->getWorldNonConst()->removeObject(scaled_object_->id_);
102  for (std::size_t i = 0; i < scaled_object_->shapes_.size(); ++i)
103  {
104  shapes::Shape* s = scaled_object_->shapes_[i]->clone();
105  s->scale((double)value / 100.0);
106  ps->getWorldNonConst()->addToObject(scaled_object_->id_, shapes::ShapeConstPtr(s),
107  scaled_object_->shape_poses_[i]);
108  }
110  }
111  else
112  scaled_object_.reset();
113  }
114  else
115  scaled_object_.reset();
116  }
117 }
118 
120 {
121  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
122  if (sel.empty())
123  return;
124  if (planning_display_->getPlanningSceneMonitor() && sel[0]->checkState() == Qt::Unchecked)
125  {
127  if (ps)
128  {
129  scaled_object_ = ps->getWorld()->getObject(sel[0]->text().toStdString());
130  }
131  }
132 }
133 
135 {
136  scaled_object_.reset();
137  ui_->scene_scale->setSliderPosition(100);
138 }
139 
141 {
142  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
143  if (sel.empty())
144  return;
146  if (ps)
147  {
148  for (int i = 0; i < sel.count(); ++i)
149  if (sel[i]->checkState() == Qt::Unchecked)
150  ps->getWorldNonConst()->removeObject(sel[i]->text().toStdString());
151  else
152  ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString());
153  scene_marker_.reset();
156  }
157 }
158 
160 {
161  QString status_text = "'" + QString::fromStdString(obj->id_) + "' is a collision object with ";
162  if (obj->shapes_.empty())
163  status_text += "no geometry";
164  else
165  {
166  std::vector<QString> shape_names;
167  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
168  shape_names.push_back(QString::fromStdString(shapes::shapeStringName(obj->shapes_[i].get())));
169  if (shape_names.size() == 1)
170  status_text += "one " + shape_names[0];
171  else
172  {
173  status_text += QString::fromStdString(boost::lexical_cast<std::string>(shape_names.size())) + " shapes:";
174  for (std::size_t i = 0; i < shape_names.size(); ++i)
175  status_text += " " + shape_names[i];
176  }
177  }
178  return status_text;
179 }
180 
181 static QString decideStatusText(const robot_state::AttachedBody* attached_body)
182 {
183  QString status_text = "'" + QString::fromStdString(attached_body->getName()) + "' is attached to '" +
184  QString::fromStdString(attached_body->getAttachedLinkName()) + "'";
185  return status_text;
186 }
187 
189 {
190  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
191  if (sel.empty())
192  {
193  bool oldState = ui_->object_x->blockSignals(true);
194  ui_->object_x->setValue(0.0);
195  ui_->object_x->blockSignals(oldState);
196 
197  oldState = ui_->object_y->blockSignals(true);
198  ui_->object_y->setValue(0.0);
199  ui_->object_y->blockSignals(oldState);
200 
201  oldState = ui_->object_z->blockSignals(true);
202  ui_->object_z->setValue(0.0);
203  ui_->object_z->blockSignals(oldState);
204 
205  oldState = ui_->object_rx->blockSignals(true);
206  ui_->object_rx->setValue(0.0);
207  ui_->object_rx->blockSignals(oldState);
208 
209  oldState = ui_->object_ry->blockSignals(true);
210  ui_->object_ry->setValue(0.0);
211  ui_->object_ry->blockSignals(oldState);
212 
213  oldState = ui_->object_rz->blockSignals(true);
214  ui_->object_rz->setValue(0.0);
215  ui_->object_rz->blockSignals(oldState);
216 
217  ui_->object_status->setText("");
218  scene_marker_.reset();
219  ui_->scene_scale->setEnabled(false);
220  }
222  {
223  // if this is a CollisionWorld element
224  if (sel[0]->checkState() == Qt::Unchecked)
225  {
226  ui_->scene_scale->setEnabled(true);
227  bool update_scene_marker = false;
228  Eigen::Affine3d obj_pose;
229  {
232  ps->getWorld()->getObject(sel[0]->text().toStdString());
233  if (obj)
234  {
235  ui_->object_status->setText(decideStatusText(obj));
236 
237  if (obj->shapes_.size() == 1)
238  {
239  obj_pose = obj->shape_poses_[0];
240  Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2);
241  update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock
242 
243  bool oldState = ui_->object_x->blockSignals(true);
244  ui_->object_x->setValue(obj_pose.translation()[0]);
245  ui_->object_x->blockSignals(oldState);
246 
247  oldState = ui_->object_y->blockSignals(true);
248  ui_->object_y->setValue(obj_pose.translation()[1]);
249  ui_->object_y->blockSignals(oldState);
250 
251  oldState = ui_->object_z->blockSignals(true);
252  ui_->object_z->setValue(obj_pose.translation()[2]);
253  ui_->object_z->blockSignals(oldState);
254 
255  oldState = ui_->object_rx->blockSignals(true);
256  ui_->object_rx->setValue(xyz[0]);
257  ui_->object_rx->blockSignals(oldState);
258 
259  oldState = ui_->object_ry->blockSignals(true);
260  ui_->object_ry->setValue(xyz[1]);
261  ui_->object_ry->blockSignals(oldState);
262 
263  oldState = ui_->object_rz->blockSignals(true);
264  ui_->object_rz->setValue(xyz[2]);
265  ui_->object_rz->blockSignals(oldState);
266  }
267  }
268  else
269  ui_->object_status->setText("ERROR: '" + sel[0]->text() + "' should be a collision object but it is not");
270  }
271  if (update_scene_marker && ui_->tabWidget->tabText(ui_->tabWidget->currentIndex()).toStdString() == TAB_OBJECTS)
272  {
274  }
275  }
276  else
277  {
278  ui_->scene_scale->setEnabled(false);
279  // if it is an attached object
280  scene_marker_.reset();
282  const robot_state::AttachedBody* attached_body =
283  ps->getCurrentState().getAttachedBody(sel[0]->text().toStdString());
284  if (attached_body)
285  ui_->object_status->setText(decideStatusText(attached_body));
286  else
287  ui_->object_status->setText("ERROR: '" + sel[0]->text() + "' should be an attached object but it is not");
288  }
289  }
290 }
291 
293 {
295 }
296 
297 void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position)
298 {
299  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
300  if (sel.empty())
301  return;
303  if (ps)
304  {
305  collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
306  if (obj && obj->shapes_.size() == 1)
307  {
308  Eigen::Affine3d p;
309  p.translation()[0] = ui_->object_x->value();
310  p.translation()[1] = ui_->object_y->value();
311  p.translation()[2] = ui_->object_z->value();
312 
313  p = Eigen::Translation3d(p.translation()) *
314  (Eigen::AngleAxisd(ui_->object_rx->value(), Eigen::Vector3d::UnitX()) *
315  Eigen::AngleAxisd(ui_->object_ry->value(), Eigen::Vector3d::UnitY()) *
316  Eigen::AngleAxisd(ui_->object_rz->value(), Eigen::Vector3d::UnitZ()));
317 
318  ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p);
320 
321  // Update the interactive marker pose to match the manually introduced one
322  if (update_marker_position && scene_marker_)
323  {
324  Eigen::Quaterniond eq(p.linear());
325  scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()),
326  Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), "");
327  }
328  }
329  }
330 }
331 
333 {
334  if (item->type() < (int)known_collision_objects_.size() && planning_display_->getPlanningSceneMonitor())
335  {
336  // if we have a name change
337  if (known_collision_objects_[item->type()].first != item->text().toStdString())
338  renameCollisionObject(item);
339  else
340  {
341  bool checked = item->checkState() == Qt::Checked;
342  if (known_collision_objects_[item->type()].second != checked)
344  }
345  }
346 }
347 
348 /* Receives feedback from the interactive marker and updates the shape pose in the world accordingly */
349 void MotionPlanningFrame::imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback& feedback)
350 {
351  bool oldState = ui_->object_x->blockSignals(true);
352  ui_->object_x->setValue(feedback.pose.position.x);
353  ui_->object_x->blockSignals(oldState);
354 
355  oldState = ui_->object_y->blockSignals(true);
356  ui_->object_y->setValue(feedback.pose.position.y);
357  ui_->object_y->blockSignals(oldState);
358 
359  oldState = ui_->object_z->blockSignals(true);
360  ui_->object_z->setValue(feedback.pose.position.z);
361  ui_->object_z->blockSignals(oldState);
362 
363  Eigen::Quaterniond q;
364  tf::quaternionMsgToEigen(feedback.pose.orientation, q);
365  Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2);
366 
367  oldState = ui_->object_rx->blockSignals(true);
368  ui_->object_rx->setValue(xyz[0]);
369  ui_->object_rx->blockSignals(oldState);
370 
371  oldState = ui_->object_ry->blockSignals(true);
372  ui_->object_ry->setValue(xyz[1]);
373  ui_->object_ry->blockSignals(oldState);
374 
375  oldState = ui_->object_rz->blockSignals(true);
376  ui_->object_rz->setValue(xyz[2]);
377  ui_->object_rz->blockSignals(oldState);
378 
380 }
381 
383 {
384  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
385  if (sel.empty())
386  return;
387 
389  if (!ps)
390  return;
391 
392  for (int i = 0; i < sel.size(); ++i)
393  {
394  std::string name = sel[i]->text().toStdString();
395  collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(name);
396  if (!obj)
397  continue;
398 
399  // find a name for the copy
400  name = "Copy of " + name;
401  if (ps->getWorld()->hasObject(name))
402  {
403  name += " ";
404  unsigned int n = 1;
405  while (ps->getWorld()->hasObject(name + boost::lexical_cast<std::string>(n)))
406  n++;
407  name += boost::lexical_cast<std::string>(n);
408  }
409  ps->getWorldNonConst()->addToObject(name, obj->shapes_, obj->shape_poses_);
410  ROS_DEBUG("Copied collision object to '%s'", name.c_str());
411  }
413 }
414 
416 {
418  {
419  moveit_msgs::PlanningScene msg;
420  planning_display_->getPlanningSceneRO()->getPlanningSceneMsg(msg);
421  try
422  {
423  planning_scene_storage_->removePlanningScene(msg.name);
424  planning_scene_storage_->addPlanningScene(msg);
425  }
426  catch (std::exception& ex)
427  {
428  ROS_ERROR("%s", ex.what());
429  }
430 
432  }
433 }
434 
435 void MotionPlanningFrame::computeSaveQueryButtonClicked(const std::string& scene, const std::string& query_name)
436 {
437  moveit_msgs::MotionPlanRequest mreq;
440  {
441  try
442  {
443  if (!query_name.empty())
444  planning_scene_storage_->removePlanningQuery(scene, query_name);
445  planning_scene_storage_->addPlanningQuery(mreq, scene, query_name);
446  }
447  catch (std::exception& ex)
448  {
449  ROS_ERROR("%s", ex.what());
450  }
451 
453  }
454 }
455 
457 {
459  {
460  QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
461  if (!sel.empty())
462  {
463  QTreeWidgetItem* s = sel.front();
464  if (s->type() == ITEM_TYPE_SCENE)
465  {
466  std::string scene = s->text(0).toStdString();
467  try
468  {
469  planning_scene_storage_->removePlanningScene(scene);
470  }
471  catch (std::exception& ex)
472  {
473  ROS_ERROR("%s", ex.what());
474  }
475  }
476  else
477  {
478  // if we selected a query name, then we overwrite that query
479  std::string scene = s->parent()->text(0).toStdString();
480  try
481  {
482  planning_scene_storage_->removePlanningScene(scene);
483  }
484  catch (std::exception& ex)
485  {
486  ROS_ERROR("%s", ex.what());
487  }
488  }
490  }
491  }
492 }
493 
495 {
497  {
498  QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
499  if (!sel.empty())
500  {
501  QTreeWidgetItem* s = sel.front();
502  if (s->type() == ITEM_TYPE_QUERY)
503  {
504  std::string scene = s->parent()->text(0).toStdString();
505  std::string query_name = s->text(0).toStdString();
506  try
507  {
508  planning_scene_storage_->removePlanningQuery(scene, query_name);
509  }
510  catch (std::exception& ex)
511  {
512  ROS_ERROR("%s", ex.what());
513  }
516  }
517  }
518  }
519 }
520 
522 {
523  ui_->planning_scene_tree->setUpdatesEnabled(false);
524  s->parent()->removeChild(s);
525  ui_->planning_scene_tree->setUpdatesEnabled(true);
526 }
527 
529 {
530  QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
531  if (sel.empty())
532  {
533  ui_->load_scene_button->setEnabled(false);
534  ui_->load_query_button->setEnabled(false);
535  ui_->save_query_button->setEnabled(false);
536  ui_->delete_scene_button->setEnabled(false);
537  }
538  else
539  {
540  ui_->save_query_button->setEnabled(true);
541 
542  QTreeWidgetItem* s = sel.front();
543 
544  // if the item is a PlanningScene
545  if (s->type() == ITEM_TYPE_SCENE)
546  {
547  ui_->load_scene_button->setEnabled(true);
548  ui_->load_query_button->setEnabled(false);
549  ui_->delete_scene_button->setEnabled(true);
550  ui_->delete_query_button->setEnabled(false);
551  ui_->save_query_button->setEnabled(true);
552  }
553  else
554  {
555  // if the item is a query
556  ui_->load_scene_button->setEnabled(false);
557  ui_->load_query_button->setEnabled(true);
558  ui_->delete_scene_button->setEnabled(false);
559  ui_->delete_query_button->setEnabled(true);
560  }
561  }
562 }
563 
565 {
567  {
568  QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
569  if (!sel.empty())
570  {
571  QTreeWidgetItem* s = sel.front();
572  if (s->type() == ITEM_TYPE_SCENE)
573  {
574  std::string scene = s->text(0).toStdString();
575  ROS_DEBUG("Attempting to load scene '%s'", scene.c_str());
577  bool got_ps = false;
578  try
579  {
580  got_ps = planning_scene_storage_->getPlanningScene(scene_m, scene);
581  }
582  catch (std::exception& ex)
583  {
584  ROS_ERROR("%s", ex.what());
585  }
586 
587  if (got_ps)
588  {
589  ROS_INFO("Loaded scene '%s'", scene.c_str());
591  {
592  if (scene_m->robot_model_name != planning_display_->getRobotModel()->getName())
593  {
594  ROS_INFO("Scene '%s' was saved for robot '%s' but we are using robot '%s'. Using scene geometry only",
595  scene.c_str(), scene_m->robot_model_name.c_str(),
596  planning_display_->getRobotModel()->getName().c_str());
598  // publish the parts that are not in the world
599  moveit_msgs::PlanningScene diff;
600  diff.is_diff = true;
601  diff.name = scene_m->name;
603  }
604  else
605  planning_scene_publisher_.publish(static_cast<const moveit_msgs::PlanningScene&>(*scene_m));
606  }
607  else
608  planning_scene_publisher_.publish(static_cast<const moveit_msgs::PlanningScene&>(*scene_m));
609  }
610  else
611  ROS_WARN("Failed to load scene '%s'. Has the message format changed since the scene was saved?",
612  scene.c_str());
613  }
614  }
615  }
616 }
617 
619 {
621  {
622  QList<QTreeWidgetItem*> sel = ui_->planning_scene_tree->selectedItems();
623  if (!sel.empty())
624  {
625  QTreeWidgetItem* s = sel.front();
626  if (s->type() == ITEM_TYPE_QUERY)
627  {
628  std::string scene = s->parent()->text(0).toStdString();
629  std::string query_name = s->text(0).toStdString();
631  bool got_q = false;
632  try
633  {
634  got_q = planning_scene_storage_->getPlanningQuery(mp, scene, query_name);
635  }
636  catch (std::exception& ex)
637  {
638  ROS_ERROR("%s", ex.what());
639  }
640 
641  if (got_q)
642  {
643  robot_state::RobotStatePtr start_state(new robot_state::RobotState(*planning_display_->getQueryStartState()));
644  robot_state::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(),
645  mp->start_state, *start_state);
646  planning_display_->setQueryStartState(*start_state);
647 
648  robot_state::RobotStatePtr goal_state(new robot_state::RobotState(*planning_display_->getQueryGoalState()));
649  for (std::size_t i = 0; i < mp->goal_constraints.size(); ++i)
650  if (mp->goal_constraints[i].joint_constraints.size() > 0)
651  {
652  std::map<std::string, double> vals;
653  for (std::size_t j = 0; j < mp->goal_constraints[i].joint_constraints.size(); ++j)
654  vals[mp->goal_constraints[i].joint_constraints[j].joint_name] =
655  mp->goal_constraints[i].joint_constraints[j].position;
656  goal_state->setVariablePositions(vals);
657  break;
658  }
659  planning_display_->setQueryGoalState(*goal_state);
660  }
661  else
662  ROS_ERROR("Failed to load planning query '%s'. Has the message format changed since the query was saved?",
663  query_name.c_str());
664  }
665  }
666  }
667 }
668 
669 void MotionPlanningFrame::addObject(const collision_detection::WorldPtr& world, const std::string& id,
670  const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose)
671 {
672  world->addToObject(id, shape, pose);
673 
675 
676  // Automatically select the inserted object so that its IM is displayed
678  boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, id, true, ui_->collision_objects_list));
679 
681 }
682 
684 {
685  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
686  if (sel.empty())
687  return;
688 
690  if (!ps)
691  return;
692 
694  ps->getWorld()->getObject(sel[0]->text().toStdString());
695  if (obj && obj->shapes_.size() == 1)
696  {
697  Eigen::Quaterniond eq(obj->shape_poses_[0].linear());
698  geometry_msgs::PoseStamped shape_pose;
699  shape_pose.pose.position.x = obj->shape_poses_[0].translation()[0];
700  shape_pose.pose.position.y = obj->shape_poses_[0].translation()[1];
701  shape_pose.pose.position.z = obj->shape_poses_[0].translation()[2];
702  shape_pose.pose.orientation.x = eq.x();
703  shape_pose.pose.orientation.y = eq.y();
704  shape_pose.pose.orientation.z = eq.z();
705  shape_pose.pose.orientation.w = eq.w();
706 
707  // create an interactive marker for moving the shape in the world
708  visualization_msgs::InteractiveMarker int_marker =
709  robot_interaction::make6DOFMarker(std::string("marker_") + sel[0]->text().toStdString(), shape_pose, 1.0);
710  int_marker.header.frame_id = planning_display_->getRobotModel()->getModelFrame();
711  int_marker.description = sel[0]->text().toStdString();
712 
715  imarker->processMessage(int_marker);
716  imarker->setShowAxes(false);
717  scene_marker_.reset(imarker);
718 
719  // Connect signals
720  connect(imarker, SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)), this,
721  SLOT(imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
722  }
723  else
724  {
725  scene_marker_.reset();
726  }
727 }
728 
730 {
731  long unsigned int version = known_collision_objects_version_;
732  if (item->text().isEmpty())
733  {
734  QMessageBox::warning(this, "Invalid object name", "Cannot set an empty object name.");
735  if (version == known_collision_objects_version_)
736  item->setText(QString::fromStdString(known_collision_objects_[item->type()].first));
737  return;
738  }
739 
740  std::string item_text = item->text().toStdString();
741  bool already_exists = planning_display_->getPlanningSceneRO()->getWorld()->hasObject(item_text);
742  if (!already_exists)
743  already_exists = planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(item_text);
744  if (already_exists)
745  {
746  QMessageBox::warning(this, "Duplicate object name",
747  QString("The name '")
748  .append(item->text())
749  .append("' already exists. Not renaming object ")
750  .append((known_collision_objects_[item->type()].first.c_str())));
751  if (version == known_collision_objects_version_)
752  item->setText(QString::fromStdString(known_collision_objects_[item->type()].first));
753  return;
754  }
755 
756  if (item->checkState() == Qt::Unchecked)
757  {
760  ps->getWorld()->getObject(known_collision_objects_[item->type()].first);
761  if (obj)
762  {
763  known_collision_objects_[item->type()].first = item_text;
764  ps->getWorldNonConst()->removeObject(obj->id_);
765  ps->getWorldNonConst()->addToObject(known_collision_objects_[item->type()].first, obj->shapes_,
766  obj->shape_poses_);
767  if (scene_marker_)
768  {
769  scene_marker_.reset();
771  }
772  }
773  }
774  else
775  {
776  // rename attached body
778  robot_state::RobotState& cs = ps->getCurrentStateNonConst();
779  const robot_state::AttachedBody* ab = cs.getAttachedBody(known_collision_objects_[item->type()].first);
780  if (ab)
781  {
782  known_collision_objects_[item->type()].first = item_text;
783  robot_state::AttachedBody* new_ab = new robot_state::AttachedBody(
784  ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getShapes(),
785  ab->getFixedTransforms(), ab->getTouchLinks(), ab->getDetachPosture());
786  cs.clearAttachedBody(ab->getName());
787  cs.attachBody(new_ab);
788  }
789  }
790 }
791 
793 {
794  long unsigned int version = known_collision_objects_version_;
795  bool checked = item->checkState() == Qt::Checked;
796  std::pair<std::string, bool> data = known_collision_objects_[item->type()];
797  moveit_msgs::AttachedCollisionObject aco;
798 
799  if (checked) // we need to attach a known collision object
800  {
801  QStringList links;
802  const std::vector<std::string>& links_std = planning_display_->getRobotModel()->getLinkModelNames();
803  for (std::size_t i = 0; i < links_std.size(); ++i)
804  links.append(QString::fromStdString(links_std[i]));
805  bool ok = false;
806  QString response =
807  QInputDialog::getItem(this, tr("Select Link Name"), tr("Choose the link to attach to:"), links, 0, false, &ok);
808  if (!ok)
809  {
810  if (version == known_collision_objects_version_)
811  item->setCheckState(Qt::Unchecked);
812  return;
813  }
814  aco.link_name = response.toStdString();
815  aco.object.id = data.first;
816  aco.object.operation = moveit_msgs::CollisionObject::ADD;
817  }
818  else // we need to detach an attached object
819  {
821  const robot_state::AttachedBody* attached_body = ps->getCurrentState().getAttachedBody(data.first);
822  if (attached_body)
823  {
824  aco.link_name = attached_body->getAttachedLinkName();
825  aco.object.id = attached_body->getName();
826  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
827  }
828  }
829  {
831  // we loop through the list in case updates were received since the start of the function
832  for (std::size_t i = 0; i < known_collision_objects_.size(); ++i)
833  if (known_collision_objects_[i].first == data.first)
834  {
835  known_collision_objects_[i].second = checked;
836  break;
837  }
838  ps->processAttachedCollisionObjectMsg(aco);
839  }
840 
843 }
844 
846 {
847  ui_->collision_objects_list->setUpdatesEnabled(false);
848  bool oldState = ui_->collision_objects_list->blockSignals(true);
849 
850  {
851  QList<QListWidgetItem*> sel = ui_->collision_objects_list->selectedItems();
852  std::set<std::string> to_select;
853  for (int i = 0; i < sel.size(); ++i)
854  to_select.insert(sel[i]->text().toStdString());
855  ui_->collision_objects_list->clear();
856  known_collision_objects_.clear();
858 
860  if (ps)
861  {
862  const std::vector<std::string>& collision_object_names = ps->getWorld()->getObjectIds();
863  for (std::size_t i = 0; i < collision_object_names.size(); ++i)
864  {
865  if (collision_object_names[i] == planning_scene::PlanningScene::OCTOMAP_NS)
866  continue;
867 
868  QListWidgetItem* item =
869  new QListWidgetItem(QString::fromStdString(collision_object_names[i]), ui_->collision_objects_list, (int)i);
870  item->setFlags(item->flags() | Qt::ItemIsEditable);
871  item->setToolTip(item->text());
872  item->setCheckState(Qt::Unchecked);
873  if (to_select.find(collision_object_names[i]) != to_select.end())
874  item->setSelected(true);
875  ui_->collision_objects_list->addItem(item);
876  known_collision_objects_.push_back(std::make_pair(collision_object_names[i], false));
877  }
878 
879  const robot_state::RobotState& cs = ps->getCurrentState();
880  std::vector<const robot_state::AttachedBody*> attached_bodies;
881  cs.getAttachedBodies(attached_bodies);
882  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
883  {
884  QListWidgetItem* item =
885  new QListWidgetItem(QString::fromStdString(attached_bodies[i]->getName()), ui_->collision_objects_list,
886  (int)(i + collision_object_names.size()));
887  item->setFlags(item->flags() | Qt::ItemIsEditable);
888  item->setToolTip(item->text());
889  item->setCheckState(Qt::Checked);
890  if (to_select.find(attached_bodies[i]->getName()) != to_select.end())
891  item->setSelected(true);
892  ui_->collision_objects_list->addItem(item);
893  known_collision_objects_.push_back(std::make_pair(attached_bodies[i]->getName(), true));
894  }
895  }
896  }
897 
898  ui_->collision_objects_list->blockSignals(oldState);
899  ui_->collision_objects_list->setUpdatesEnabled(true);
901 }
902 
904 {
905  QString path =
906  QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
907  if (!path.isEmpty())
909  boost::bind(&MotionPlanningFrame::computeExportAsText, this, path.toStdString()), "export as text");
910 }
911 
912 void MotionPlanningFrame::computeExportAsText(const std::string& path)
913 {
915  if (ps)
916  {
917  std::string p = (path.length() < 7 || path.substr(path.length() - 6) != ".scene") ? path + ".scene" : path;
918  std::ofstream fout(p.c_str());
919  if (fout.good())
920  {
921  ps->saveGeometryToStream(fout);
922  fout.close();
923  ROS_INFO("Saved current scene geometry to '%s'", p.c_str());
924  }
925  else
926  ROS_WARN("Unable to save current scene geometry to '%s'", p.c_str());
927  }
928 }
929 
930 void MotionPlanningFrame::computeImportFromText(const std::string& path)
931 {
933  if (ps)
934  {
935  std::ifstream fin(path.c_str());
936  if (fin.good())
937  {
938  ps->loadGeometryFromStream(fin);
939  fin.close();
940  ROS_INFO("Loaded scene geometry from '%s'", path.c_str());
943  }
944  else
945  ROS_WARN("Unable to load scene geometry from '%s'", path.c_str());
946  }
947 }
948 
950 {
951  QString path =
952  QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
953  if (!path.isEmpty())
955  boost::bind(&MotionPlanningFrame::computeImportFromText, this, path.toStdString()), "import from text");
956 }
957 }
void updateCollisionObjectPose(bool update_marker_position)
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void scale(double scale)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
q
void publish(const boost::shared_ptr< M > &message) const
static QString decideStatusText(const collision_detection::CollisionWorld::ObjectConstPtr &obj)
Ogre::SceneNode * getSceneNode() const
static const std::string TAB_OBJECTS
XmlRpcServer s
void computeSaveQueryButtonClicked(const std::string &scene, const std::string &query_name)
std::string getName(void *handle)
name
robot_state::RobotStateConstPtr getQueryStartState() const
void constructPlanningRequest(moveit_msgs::MotionPlanRequest &mreq)
INTERACTIVE_MARKERS_PUBLIC void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
void setQueryStartState(const robot_state::RobotState &start)
text
static const std::string OCTOMAP_NS
data
#define ROS_WARN(...)
void setQueryGoalState(const robot_state::RobotState &goal)
World::ObjectConstPtr ObjectConstPtr
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
void setItemSelectionInList(const std::string &item_name, bool selection, QListWidget *list)
#define ROS_INFO(...)
void importResource(const std::string &path)
bool processMessage(const visualization_msgs::InteractiveMarker &message)
void imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
version
robot_state::RobotStateConstPtr getQueryGoalState() const
const std::string & shapeStringName(const Shape *shape)
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
const robot_model::RobotModelConstPtr & getRobotModel() const
std::vector< std::pair< std::string, bool > > known_collision_objects_
std::shared_ptr< rviz::InteractiveMarker > scene_marker_
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
void setShowAxes(bool show)
collision_detection::CollisionWorld::ObjectConstPtr scaled_object_
#define ROS_ERROR(...)
moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_
visualization_msgs::InteractiveMarker make6DOFMarker(const std::string &name, const geometry_msgs::PoseStamped &stamped, double scale, bool orientation_fixed=false)
void addObject(const collision_detection::WorldPtr &world, const std::string &id, const shapes::ShapeConstPtr &shape, const Eigen::Affine3d &pose)
planning_scene_monitor::LockedPlanningSceneRO getPlanningSceneRO() const
get read-only access to planning scene
#define ROS_DEBUG(...)
std::shared_ptr< const Shape > ShapeConstPtr


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:08