motion_planning_frame.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 */
36 
40 
42 
43 #include <rviz/display_context.h>
44 #include <rviz/frame_manager.h>
45 
46 #include <std_srvs/Empty.h>
47 
48 #include <QMessageBox>
49 #include <QInputDialog>
50 #include <QShortcut>
51 
52 #include "ui_motion_planning_rviz_plugin_frame.h"
53 
54 namespace moveit_rviz_plugin
55 {
57  QWidget* parent)
58  : QWidget(parent)
59  , planning_display_(pdisplay)
60  , context_(context)
61  , ui_(new Ui::MotionPlanningUI())
62  , first_time_(true)
63  , clear_octomap_service_client_(nh_.serviceClient<std_srvs::Empty>(move_group::CLEAR_OCTOMAP_SERVICE_NAME))
64 {
65  // set up the GUI
66  ui_->setupUi(this);
67 
68  // connect bottons to actions; each action usually registers the function pointer for the actual computation,
69  // to keep the GUI more responsive (using the background job processing)
70  connect(ui_->plan_button, SIGNAL(clicked()), this, SLOT(planButtonClicked()));
71  connect(ui_->execute_button, SIGNAL(clicked()), this, SLOT(executeButtonClicked()));
72  connect(ui_->plan_and_execute_button, SIGNAL(clicked()), this, SLOT(planAndExecuteButtonClicked()));
73  connect(ui_->stop_button, SIGNAL(clicked()), this, SLOT(stopButtonClicked()));
74  connect(ui_->use_start_state_button, SIGNAL(clicked()), this, SLOT(useStartStateButtonClicked()));
75  connect(ui_->use_goal_state_button, SIGNAL(clicked()), this, SLOT(useGoalStateButtonClicked()));
76  connect(ui_->database_connect_button, SIGNAL(clicked()), this, SLOT(databaseConnectButtonClicked()));
77  connect(ui_->save_scene_button, SIGNAL(clicked()), this, SLOT(saveSceneButtonClicked()));
78  connect(ui_->save_query_button, SIGNAL(clicked()), this, SLOT(saveQueryButtonClicked()));
79  connect(ui_->delete_scene_button, SIGNAL(clicked()), this, SLOT(deleteSceneButtonClicked()));
80  connect(ui_->delete_query_button, SIGNAL(clicked()), this, SLOT(deleteQueryButtonClicked()));
81  connect(ui_->planning_scene_tree, SIGNAL(itemSelectionChanged()), this, SLOT(planningSceneItemClicked()));
82  connect(ui_->load_scene_button, SIGNAL(clicked()), this, SLOT(loadSceneButtonClicked()));
83  connect(ui_->load_query_button, SIGNAL(clicked()), this, SLOT(loadQueryButtonClicked()));
84  connect(ui_->allow_looking, SIGNAL(toggled(bool)), this, SLOT(allowLookingToggled(bool)));
85  connect(ui_->allow_replanning, SIGNAL(toggled(bool)), this, SLOT(allowReplanningToggled(bool)));
86  connect(ui_->allow_external_program, SIGNAL(toggled(bool)), this, SLOT(allowExternalProgramCommunication(bool)));
87  connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this,
89  connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this,
91  connect(ui_->import_file_button, SIGNAL(clicked()), this, SLOT(importFileButtonClicked()));
92  connect(ui_->import_url_button, SIGNAL(clicked()), this, SLOT(importUrlButtonClicked()));
93  connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearSceneButtonClicked()));
94  connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int)));
95  connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange()));
96  connect(ui_->scene_scale, SIGNAL(sliderReleased()), this, SLOT(sceneScaleEndChange()));
97  connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeObjectButtonClicked()));
98  connect(ui_->object_x, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
99  connect(ui_->object_y, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
100  connect(ui_->object_z, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
101  connect(ui_->object_rx, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
102  connect(ui_->object_ry, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
103  connect(ui_->object_rz, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
104  connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishSceneButtonClicked()));
105  connect(ui_->collision_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedCollisionObjectChanged()));
106  connect(ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
107  SLOT(collisionObjectChanged(QListWidgetItem*)));
108  connect(ui_->path_constraints_combo_box, SIGNAL(currentIndexChanged(int)), this,
109  SLOT(pathConstraintsIndexChanged(int)));
110  connect(ui_->clear_octomap_button, SIGNAL(clicked()), this, SLOT(onClearOctomapClicked()));
111  connect(ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this,
112  SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int)));
113  connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked()));
114  connect(ui_->export_scene_text_button, SIGNAL(clicked()), this, SLOT(exportAsTextButtonClicked()));
115  connect(ui_->import_scene_text_button, SIGNAL(clicked()), this, SLOT(importFromTextButtonClicked()));
116  connect(ui_->load_state_button, SIGNAL(clicked()), this, SLOT(loadStateButtonClicked()));
117  connect(ui_->save_start_state_button, SIGNAL(clicked()), this, SLOT(saveStartStateButtonClicked()));
118  connect(ui_->save_goal_state_button, SIGNAL(clicked()), this, SLOT(saveGoalStateButtonClicked()));
119  connect(ui_->set_as_start_state_button, SIGNAL(clicked()), this, SLOT(setAsStartStateButtonClicked()));
120  connect(ui_->set_as_goal_state_button, SIGNAL(clicked()), this, SLOT(setAsGoalStateButtonClicked()));
121  connect(ui_->remove_state_button, SIGNAL(clicked()), this, SLOT(removeStateButtonClicked()));
122  connect(ui_->clear_states_button, SIGNAL(clicked()), this, SLOT(clearStatesButtonClicked()));
123  connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SLOT(approximateIKChanged(int)));
124 
125  connect(ui_->detect_objects_button, SIGNAL(clicked()), this, SLOT(detectObjectsButtonClicked()));
126  connect(ui_->pick_button, SIGNAL(clicked()), this, SLOT(pickObjectButtonClicked()));
127  connect(ui_->place_button, SIGNAL(clicked()), this, SLOT(placeObjectButtonClicked()));
128  connect(ui_->detected_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedDetectedObjectChanged()));
129  connect(ui_->detected_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
130  SLOT(detectedObjectChanged(QListWidgetItem*)));
131  connect(ui_->support_surfaces_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedSupportSurfaceChanged()));
132 
133  connect(ui_->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabChanged(int)));
134 
135  QShortcut* copy_object_shortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_C), ui_->collision_objects_list);
136  connect(copy_object_shortcut, SIGNAL(activated()), this, SLOT(copySelectedCollisionObject()));
137 
138  ui_->reset_db_button->hide();
139  ui_->background_job_progress->hide();
140  ui_->background_job_progress->setMaximum(0);
141 
142  ui_->tabWidget->setCurrentIndex(0);
143 
145 
146  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
147  planning_scene_world_publisher_ = nh_.advertise<moveit_msgs::PlanningSceneWorld>("planning_scene_world", 1);
148 
149  // object_recognition_trigger_publisher_ = nh_.advertise<std_msgs::Bool>("recognize_objects_switch", 1);
151  OBJECT_RECOGNITION_ACTION, false));
153  nh_.subscribe("recognized_object_array", 1, &MotionPlanningFrame::listenDetectedObjects, this);
154 
156  {
157  try
158  {
160  }
161  catch (std::exception& ex)
162  {
163  // ROS_ERROR("Object recognition action: %s", ex.what());
165  }
166  }
167  try
168  {
170  }
171  catch (std::exception& ex)
172  {
173  ROS_ERROR("%s", ex.what());
174  }
175 
176  try
177  {
179  if (ps)
180  {
182  }
183  else
184  semantic_world_.reset();
185  if (semantic_world_)
186  {
187  semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this));
188  }
189  }
190  catch (std::exception& ex)
191  {
192  ROS_ERROR("%s", ex.what());
193  }
194 }
195 
197 {
198 }
199 
201 {
202  planning_display_->useApproximateIK(state == Qt::Checked);
203 }
204 
205 void MotionPlanningFrame::setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list)
206 {
207  QList<QListWidgetItem*> found_items = list->findItems(QString(item_name.c_str()), Qt::MatchExactly);
208  for (int i = 0; i < found_items.size(); ++i)
209  found_items[i]->setSelected(selection);
210 }
211 
213 {
214  // This is needed to prevent UI event (resuming the options) triggered
215  // before getRobotInteraction() is loaded and ready
216  if (first_time_)
217  return;
218 
219  planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(enable);
221  if (enable)
222  {
223  ros::NodeHandle nh;
224  plan_subscriber_ = nh.subscribe("/rviz/moveit/plan", 1, &MotionPlanningFrame::remotePlanCallback, this);
225  execute_subscriber_ = nh.subscribe("/rviz/moveit/execute", 1, &MotionPlanningFrame::remoteExecuteCallback, this);
226  stop_subscriber_ = nh.subscribe("/rviz/moveit/stop", 1, &MotionPlanningFrame::remoteStopCallback, this);
228  nh.subscribe("/rviz/moveit/update_start_state", 1, &MotionPlanningFrame::remoteUpdateStartStateCallback, this);
230  nh.subscribe("/rviz/moveit/update_goal_state", 1, &MotionPlanningFrame::remoteUpdateGoalStateCallback, this);
232  "/rviz/moveit/update_custom_start_state", 1, &MotionPlanningFrame::remoteUpdateCustomStartStateCallback, this);
234  "/rviz/moveit/update_custom_goal_state", 1, &MotionPlanningFrame::remoteUpdateCustomGoalStateCallback, this);
235  }
236  else
237  { // disable
245  }
246 }
247 
249 {
250  ui_->start_state_selection->clear();
251  ui_->goal_state_selection->clear();
252 
254  return;
255 
256  const robot_model::RobotModelConstPtr& kmodel = planning_display_->getRobotModel();
258  if (group.empty())
259  return;
260  const robot_model::JointModelGroup* jmg = kmodel->getJointModelGroup(group);
261  if (jmg)
262  {
263  ui_->start_state_selection->addItem(QString("<random valid>"));
264  ui_->start_state_selection->addItem(QString("<random>"));
265  ui_->start_state_selection->addItem(QString("<current>"));
266  ui_->start_state_selection->addItem(QString("<same as goal>"));
267 
268  ui_->goal_state_selection->addItem(QString("<random valid>"));
269  ui_->goal_state_selection->addItem(QString("<random>"));
270  ui_->goal_state_selection->addItem(QString("<current>"));
271  ui_->goal_state_selection->addItem(QString("<same as start>"));
272 
273  const std::vector<std::string>& known_states = jmg->getDefaultStateNames();
274  if (!known_states.empty())
275  {
276  ui_->start_state_selection->insertSeparator(ui_->start_state_selection->count());
277  ui_->goal_state_selection->insertSeparator(ui_->goal_state_selection->count());
278  for (std::size_t i = 0; i < known_states.size(); ++i)
279  {
280  ui_->start_state_selection->addItem(QString::fromStdString(known_states[i]));
281  ui_->goal_state_selection->addItem(QString::fromStdString(known_states[i]));
282  }
283  }
284  ui_->start_state_selection->setCurrentIndex(2); // default to 'current'
285  ui_->goal_state_selection->setCurrentIndex(0); // default to 'random valid'
286  }
287 }
288 
290 {
292  return;
293 
296  boost::bind(&MotionPlanningFrame::populateConstraintsList, this, std::vector<std::string>()));
297 
298  const robot_model::RobotModelConstPtr& kmodel = planning_display_->getRobotModel();
301  boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group));
302 
303  if (!group.empty() && kmodel)
304  {
305  if (move_group_ && move_group_->getName() == group)
306  return;
307  ROS_INFO("Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(),
308  planning_display_->getMoveGroupNS().c_str());
310  opt.robot_model_ = kmodel;
311  opt.robot_description_.clear();
312  opt.node_handle_ = ros::NodeHandle(planning_display_->getMoveGroupNS());
313  try
314  {
318  move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value());
319  }
320  catch (std::exception& ex)
321  {
322  ROS_ERROR("%s", ex.what());
323  }
325  boost::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_));
326  if (move_group_)
327  {
328  move_group_->allowLooking(ui_->allow_looking->isChecked());
329  move_group_->allowReplanning(ui_->allow_replanning->isChecked());
330  moveit_msgs::PlannerInterfaceDescription desc;
331  if (move_group_->getInterfaceDescription(desc))
334  "populateConstraintsList");
335 
336  if (first_time_)
337  {
338  first_time_ = false;
340  if (ps)
341  {
342  planning_display_->setQueryStartState(ps->getCurrentState());
343  planning_display_->setQueryGoalState(ps->getCurrentState());
344  }
345  // This ensures saved UI settings applied after planning_display_ is ready
346  planning_display_->useApproximateIK(ui_->approximate_ik->isChecked());
347  if (ui_->allow_external_program->isChecked())
350  }
351  }
352  }
353 }
354 
356 {
358  "Frame::changePlanningGroup");
359 }
360 
362 {
365 }
366 
367 void MotionPlanningFrame::importResource(const std::string& path)
368 {
370  {
372  if (mesh)
373  {
374  std::size_t slash = path.find_last_of("/\\");
375  std::string name = path.substr(slash + 1);
376  shapes::ShapeConstPtr shape(mesh);
377  Eigen::Affine3d pose;
378  pose.setIdentity();
379 
380  if (planning_display_->getPlanningSceneRO()->getCurrentState().hasAttachedBody(name))
381  {
382  QMessageBox::warning(this, QString("Duplicate names"), QString("An attached object named '")
383  .append(name.c_str())
384  .append("' already exists. Please rename the "
385  "attached object before importing."));
386  return;
387  }
388 
389  // If the object already exists, ask the user whether to overwrite or rename
390  if (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(name))
391  {
392  QMessageBox msgBox;
393  msgBox.setText("There exists another object with the same name.");
394  msgBox.setInformativeText("Would you like to overwrite it?");
395  msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
396  msgBox.setDefaultButton(QMessageBox::No);
397  int ret = msgBox.exec();
398 
399  switch (ret)
400  {
401  case QMessageBox::Yes:
402  // Overwrite was clicked
403  {
405  if (ps)
406  {
407  ps->getWorldNonConst()->removeObject(name);
408  addObject(ps->getWorldNonConst(), name, shape, pose);
409  }
410  }
411  break;
412  case QMessageBox::No:
413  {
414  // Don't overwrite was clicked. Ask for another name
415  bool ok = false;
416  QString text = QInputDialog::getText(
417  this, tr("Choose a new name"), tr("Import the new object under the name:"), QLineEdit::Normal,
418  QString::fromStdString(name + "-" + boost::lexical_cast<std::string>(
419  planning_display_->getPlanningSceneRO()->getWorld()->size())),
420  &ok);
421  if (ok)
422  {
423  if (!text.isEmpty())
424  {
425  name = text.toStdString();
427  if (ps)
428  {
429  if (ps->getWorld()->hasObject(name))
430  QMessageBox::warning(
431  this, "Name already exists",
432  QString("The name '").append(name.c_str()).append("' already exists. Not importing object."));
433  else
434  addObject(ps->getWorldNonConst(), name, shape, pose);
435  }
436  }
437  else
438  QMessageBox::warning(this, "Object not imported", "Cannot use an empty name for an imported object");
439  }
440  break;
441  }
442  default:
443  // Pressed cancel, do nothing
444  break;
445  }
446  }
447  else
448  {
450  if (ps)
451  addObject(ps->getWorldNonConst(), name, shape, pose);
452  }
453  }
454  else
455  {
456  QMessageBox::warning(this, QString("Import error"), QString("Unable to import scene"));
457  return;
458  }
459  }
460 }
461 
463 {
464  ui_->planning_algorithm_combo_box->clear();
465  ui_->library_label->setText("NO PLANNING LIBRARY LOADED");
466  ui_->library_label->setStyleSheet("QLabel { color : red; font: bold }");
467  ui_->object_status->setText("");
468 
469  // activate the frame
470  parentWidget()->show();
471 }
472 
474 {
475  move_group_.reset();
476  parentWidget()->hide();
477 }
478 
480 {
481  if (scene_marker_ && ui_->tabWidget->tabText(index).toStdString() != TAB_OBJECTS)
482  scene_marker_.reset();
483  else if (ui_->tabWidget->tabText(index).toStdString() == TAB_OBJECTS)
485 }
486 
487 void MotionPlanningFrame::updateSceneMarkers(float wall_dt, float ros_dt)
488 {
489  if (scene_marker_)
490  scene_marker_->update(wall_dt);
491 }
492 
494 {
495  if (ui_->allow_external_program->isChecked())
496  {
497  planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(true);
498  }
499 }
500 
501 } // namespace
void listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr &msg)
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void populatePlannersList(const moveit_msgs::PlannerInterfaceDescription &desc)
planning_scene_monitor::LockedPlanningSceneRW getPlanningSceneRW()
get write access to planning scene
void waitForAction(const T &action, const ros::NodeHandle &node_handle, const ros::Duration &wait_for_server, const std::string &name)
void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr &msg)
desc
void remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr &msg)
ServiceClient serviceClient(ros::NodeHandle n, std::string name)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static const std::string TAB_OBJECTS
void setQueryStartState(const robot_state::RobotState &start)
text
void sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
void setQueryGoalState(const robot_state::RobotState &goal)
void remoteExecuteCallback(const std_msgs::EmptyConstPtr &msg)
group
moveit::planning_interface::MoveGroupInterfacePtr move_group_
void addBackgroundJob(const boost::function< void()> &job, const std::string &name)
void setItemSelectionInList(const std::string &item_name, bool selection, QListWidget *list)
const boost::shared_ptr< tf::TransformListener > & getTFClientPtr()
#define ROS_INFO(...)
void importResource(const std::string &path)
void remotePlanCallback(const std_msgs::EmptyConstPtr &msg)
virtual FrameManager * getFrameManager() const =0
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const std::string OBJECT_RECOGNITION_ACTION
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called ...
void warehouseItemNameChanged(QTreeWidgetItem *item, int column)
const robot_model::RobotModelConstPtr & getRobotModel() const
MotionPlanningFrame(MotionPlanningDisplay *pdisplay, rviz::DisplayContext *context, QWidget *parent=0)
void remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
Mesh * createMeshFromResource(const std::string &resource)
std::shared_ptr< rviz::InteractiveMarker > scene_marker_
ROSCPP_DECL std::string append(const std::string &left, const std::string &right)
void setMoveGroup(const moveit::planning_interface::MoveGroupInterfacePtr &mg)
void remoteStopCallback(const std_msgs::EmptyConstPtr &msg)
void updateSceneMarkers(float wall_dt, float ros_dt)
#define ROS_ERROR(...)
moveit_warehouse::PlanningSceneStoragePtr planning_scene_storage_
std::unique_ptr< actionlib::SimpleActionClient< object_recognition_msgs::ObjectRecognitionAction > > object_recognition_client_
moveit::planning_interface::PlanningSceneInterfacePtr planning_scene_interface_
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
moveit::semantic_world::SemanticWorldPtr semantic_world_
std::shared_ptr< const Shape > ShapeConstPtr


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