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 
37 #include <functional>
38 
44 
46 
47 #include <rviz/display_context.h>
48 #include <rviz/frame_manager.h>
49 #include <tf2_ros/buffer.h>
50 
51 #include <std_srvs/Empty.h>
52 
53 #include <QMessageBox>
54 #include <QInputDialog>
55 #include <QFileDialog>
56 #include <QComboBox>
57 #include <QShortcut>
58 
59 #include "ui_motion_planning_rviz_plugin_frame.h"
60 
61 namespace moveit_rviz_plugin
62 {
63 MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent)
64  : QWidget(parent), planning_display_(pdisplay), context_(context), ui_(new Ui::MotionPlanningUI()), first_time_(true)
65 {
66  // set up the GUI
67  ui_->setupUi(this);
68  ui_->shapes_combo_box->addItem("Box", shapes::BOX);
69  ui_->shapes_combo_box->addItem("Sphere", shapes::SPHERE);
70  ui_->shapes_combo_box->addItem("Cylinder", shapes::CYLINDER);
71  ui_->shapes_combo_box->addItem("Cone", shapes::CONE);
72  ui_->shapes_combo_box->addItem("Plane", shapes::PLANE);
73  ui_->shapes_combo_box->addItem("Mesh from file", shapes::MESH);
74  ui_->shapes_combo_box->addItem("Mesh from URL", shapes::MESH);
75  setLocalSceneEdited(false);
76 
77  // add more tabs
78  joints_tab_ = new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget);
79  ui_->tabWidget->insertTab(2, joints_tab_, "Joints");
80  connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged()));
81  connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged()));
82 
83  // connect bottons to actions; each action usually registers the function pointer for the actual computation,
84  // to keep the GUI more responsive (using the background job processing)
85  connect(ui_->plan_button, SIGNAL(clicked()), this, SLOT(planButtonClicked()));
86  connect(ui_->execute_button, SIGNAL(clicked()), this, SLOT(executeButtonClicked()));
87  connect(ui_->plan_and_execute_button, SIGNAL(clicked()), this, SLOT(planAndExecuteButtonClicked()));
88  connect(ui_->stop_button, SIGNAL(clicked()), this, SLOT(stopButtonClicked()));
89  connect(ui_->start_state_combo_box, SIGNAL(activated(QString)), this, SLOT(startStateTextChanged(QString)));
90  connect(ui_->goal_state_combo_box, SIGNAL(activated(QString)), this, SLOT(goalStateTextChanged(QString)));
91  connect(ui_->planning_group_combo_box, &QComboBox::currentTextChanged, this,
93  connect(ui_->database_connect_button, SIGNAL(clicked()), this, SLOT(databaseConnectButtonClicked()));
94  connect(ui_->save_scene_button, SIGNAL(clicked()), this, SLOT(saveSceneButtonClicked()));
95  connect(ui_->save_query_button, SIGNAL(clicked()), this, SLOT(saveQueryButtonClicked()));
96  connect(ui_->delete_scene_button, SIGNAL(clicked()), this, SLOT(deleteSceneButtonClicked()));
97  connect(ui_->delete_query_button, SIGNAL(clicked()), this, SLOT(deleteQueryButtonClicked()));
98  connect(ui_->planning_scene_tree, SIGNAL(itemSelectionChanged()), this, SLOT(planningSceneItemClicked()));
99  connect(ui_->load_scene_button, SIGNAL(clicked()), this, SLOT(loadSceneButtonClicked()));
100  connect(ui_->load_query_button, SIGNAL(clicked()), this, SLOT(loadQueryButtonClicked()));
101  connect(ui_->allow_looking, SIGNAL(toggled(bool)), this, SLOT(allowLookingToggled(bool)));
102  connect(ui_->allow_replanning, SIGNAL(toggled(bool)), this, SLOT(allowReplanningToggled(bool)));
103  connect(ui_->allow_external_program, SIGNAL(toggled(bool)), this, SLOT(allowExternalProgramCommunication(bool)));
104  connect(ui_->planning_pipeline_combo_box, SIGNAL(currentIndexChanged(int)), this,
105  SLOT(planningPipelineIndexChanged(int)));
106  connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this,
107  SLOT(planningAlgorithmIndexChanged(int)));
108  connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearScene()));
109  connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int)));
110  connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange()));
111  connect(ui_->scene_scale, SIGNAL(sliderReleased()), this, SLOT(sceneScaleEndChange()));
112  connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeSceneObjects()));
113  connect(ui_->object_x, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
114  connect(ui_->object_y, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
115  connect(ui_->object_z, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
116  connect(ui_->object_rx, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
117  connect(ui_->object_ry, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
118  connect(ui_->object_rz, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
119  connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishScene()));
120  connect(ui_->collision_objects_list, SIGNAL(currentRowChanged(int)), this, SLOT(currentCollisionObjectChanged()));
121  connect(ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
122  SLOT(collisionObjectChanged(QListWidgetItem*)));
123  connect(ui_->path_constraints_combo_box, SIGNAL(currentIndexChanged(int)), this,
124  SLOT(pathConstraintsIndexChanged(int)));
125  connect(ui_->clear_octomap_button, SIGNAL(clicked()), this, SLOT(onClearOctomapClicked()));
126  connect(ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this,
127  SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int)));
128  connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked()));
129 
130  connect(ui_->add_object_button, &QPushButton::clicked, this, &MotionPlanningFrame::addSceneObject);
131  connect(ui_->shapes_combo_box, &QComboBox::currentTextChanged, this, &MotionPlanningFrame::shapesComboBoxChanged);
132  connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked()));
133  connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked()));
134  connect(ui_->load_state_button, SIGNAL(clicked()), this, SLOT(loadStateButtonClicked()));
135  connect(ui_->save_start_state_button, SIGNAL(clicked()), this, SLOT(saveStartStateButtonClicked()));
136  connect(ui_->save_goal_state_button, SIGNAL(clicked()), this, SLOT(saveGoalStateButtonClicked()));
137  connect(ui_->set_as_start_state_button, SIGNAL(clicked()), this, SLOT(setAsStartStateButtonClicked()));
138  connect(ui_->set_as_goal_state_button, SIGNAL(clicked()), this, SLOT(setAsGoalStateButtonClicked()));
139  connect(ui_->remove_state_button, SIGNAL(clicked()), this, SLOT(removeStateButtonClicked()));
140  connect(ui_->clear_states_button, SIGNAL(clicked()), this, SLOT(clearStatesButtonClicked()));
141  connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SLOT(approximateIKChanged(int)));
142 
143  connect(ui_->detect_objects_button, SIGNAL(clicked()), this, SLOT(detectObjectsButtonClicked()));
144  connect(ui_->pick_button, SIGNAL(clicked()), this, SLOT(pickObjectButtonClicked()));
145  connect(ui_->place_button, SIGNAL(clicked()), this, SLOT(placeObjectButtonClicked()));
146  connect(ui_->detected_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedDetectedObjectChanged()));
147  connect(ui_->detected_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
148  SLOT(detectedObjectChanged(QListWidgetItem*)));
149  connect(ui_->support_surfaces_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedSupportSurfaceChanged()));
150 
151  connect(ui_->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabChanged(int)));
152 
153  /* Notice changes to be saved in config file */
154  connect(ui_->database_host, SIGNAL(textChanged(QString)), this, SIGNAL(configChanged()));
155  connect(ui_->database_port, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
156 
157  connect(joints_tab_, SIGNAL(configChanged()), this, SIGNAL(configChanged()));
158 
159  connect(ui_->planning_time, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
160  connect(ui_->planning_attempts, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
161  connect(ui_->velocity_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
162  connect(ui_->acceleration_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
163 
164  connect(ui_->allow_replanning, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
165  connect(ui_->allow_looking, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
166  connect(ui_->allow_external_program, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
167  connect(ui_->use_cartesian_path, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
168  connect(ui_->collision_aware_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
169  connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
170 
171  connect(ui_->wcenter_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
172  connect(ui_->wcenter_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
173  connect(ui_->wcenter_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
174  connect(ui_->wsize_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
175  connect(ui_->wsize_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
176  connect(ui_->wsize_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
177 
178  QShortcut* copy_object_shortcut = new QShortcut(QKeySequence(Qt::CTRL | Qt::Key_C), ui_->collision_objects_list);
179  connect(copy_object_shortcut, SIGNAL(activated()), this, SLOT(copySelectedCollisionObjects()));
180 
181  ui_->reset_db_button->hide();
182  ui_->background_job_progress->hide();
183  ui_->background_job_progress->setMaximum(0);
184 
185  ui_->tabWidget->setCurrentIndex(1);
186 
187  known_collision_objects_version_ = 0;
188 
189  initFromMoveGroupNS();
190 
191  object_recognition_client_ =
192  std::make_unique<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction>>(
194 
195  if (object_recognition_client_)
196  {
197  try
198  {
199  waitForAction(object_recognition_client_, ros::Duration(3.0), OBJECT_RECOGNITION_ACTION);
200  }
201  catch (std::exception& ex)
202  {
203  // ROS_ERROR("Object recognition action: %s", ex.what());
204  object_recognition_client_.reset();
205  }
206  }
207 
208  try
209  {
210  const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
211  if (ps)
212  {
213  semantic_world_ = std::make_shared<moveit::semantic_world::SemanticWorld>(ps);
214  }
215  else
216  semantic_world_.reset();
217  if (semantic_world_)
218  {
219  semantic_world_->addTableCallback([this] { updateTables(); });
220  }
221  }
222  catch (std::exception& ex)
223  {
224  ROS_ERROR("%s", ex.what());
225  }
226 }
227 
229 {
230  delete ui_;
231 }
232 
234 {
235  planning_display_->useApproximateIK(state == Qt::Checked);
236 }
237 
239 {
240  // This is needed to prevent UI event (resuming the options) triggered
241  // before getRobotInteraction() is loaded and ready
242  if (first_time_)
243  return;
244 
245  planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(enable);
247  if (enable)
248  {
249  ros::NodeHandle nh;
250  plan_subscriber_ = nh.subscribe("/rviz/moveit/plan", 1, &MotionPlanningFrame::remotePlanCallback, this);
251  execute_subscriber_ = nh.subscribe("/rviz/moveit/execute", 1, &MotionPlanningFrame::remoteExecuteCallback, this);
252  stop_subscriber_ = nh.subscribe("/rviz/moveit/stop", 1, &MotionPlanningFrame::remoteStopCallback, this);
254  nh.subscribe("/rviz/moveit/update_start_state", 1, &MotionPlanningFrame::remoteUpdateStartStateCallback, this);
256  nh.subscribe("/rviz/moveit/update_goal_state", 1, &MotionPlanningFrame::remoteUpdateGoalStateCallback, this);
258  "/rviz/moveit/update_custom_start_state", 1, &MotionPlanningFrame::remoteUpdateCustomStartStateCallback, this);
260  "/rviz/moveit/update_custom_goal_state", 1, &MotionPlanningFrame::remoteUpdateCustomGoalStateCallback, this);
261  }
262  else
263  { // disable
271  }
272 }
273 
275 {
276  const QSignalBlocker planning_group_blocker(ui_->planning_group_combo_box);
277  ui_->planning_group_combo_box->clear();
278 
279  const moveit::core::RobotModelConstPtr& kmodel = planning_display_->getRobotModel();
280  for (const std::string& group_name : kmodel->getJointModelGroupNames())
281  ui_->planning_group_combo_box->addItem(QString::fromStdString(group_name));
282 }
283 
285 {
286  const QSignalBlocker start_state_blocker(ui_->start_state_combo_box);
287  const QSignalBlocker goal_state_blocker(ui_->goal_state_combo_box);
288  ui_->start_state_combo_box->clear();
289  ui_->goal_state_combo_box->clear();
290 
292  return;
293 
294  const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
296  if (group.empty())
297  return;
298  const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
299  if (jmg)
300  {
301  ui_->start_state_combo_box->addItem(QString("<random valid>"));
302  ui_->start_state_combo_box->addItem(QString("<random>"));
303  ui_->start_state_combo_box->addItem(QString("<current>"));
304  ui_->start_state_combo_box->addItem(QString("<same as goal>"));
305  ui_->start_state_combo_box->addItem(QString("<previous>"));
306 
307  ui_->goal_state_combo_box->addItem(QString("<random valid>"));
308  ui_->goal_state_combo_box->addItem(QString("<random>"));
309  ui_->goal_state_combo_box->addItem(QString("<current>"));
310  ui_->goal_state_combo_box->addItem(QString("<same as start>"));
311  ui_->goal_state_combo_box->addItem(QString("<previous>"));
312 
313  const std::vector<std::string>& known_states = jmg->getDefaultStateNames();
314  if (!known_states.empty())
315  {
316  ui_->start_state_combo_box->insertSeparator(ui_->start_state_combo_box->count());
317  ui_->goal_state_combo_box->insertSeparator(ui_->goal_state_combo_box->count());
318  for (const std::string& known_state : known_states)
319  {
320  ui_->start_state_combo_box->addItem(QString::fromStdString(known_state));
321  ui_->goal_state_combo_box->addItem(QString::fromStdString(known_state));
322  }
323  }
324 
325  ui_->start_state_combo_box->setCurrentIndex(2); // default to 'current'
326  ui_->goal_state_combo_box->setCurrentIndex(2); // default to 'current'
327  }
328 }
329 
331 {
333  return;
334 
336  planning_display_->addMainLoopJob([this]() { populateConstraintsList(std::vector<std::string>()); });
337 
338  const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
340  planning_display_->addMainLoopJob([&view = *ui_->planner_param_treeview, group] { view.setGroupName(group); });
341  planning_display_->addMainLoopJob(
342  [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); });
343 
344  if (!group.empty() && robot_model)
345  {
346  if (move_group_ && move_group_->getName() == group)
347  return;
348  ROS_INFO("Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(),
349  planning_display_->getMoveGroupNS().c_str());
351  opt.robot_model_ = robot_model;
352  opt.robot_description_.clear();
353  opt.node_handle_ = ros::NodeHandle(planning_display_->getMoveGroupNS());
354  try
355  {
356 #ifdef RVIZ_TF1
357  std::shared_ptr<tf2_ros::Buffer> tf_buffer = moveit::planning_interface::getSharedTF();
358 #else
359  std::shared_ptr<tf2_ros::Buffer> tf_buffer = context_->getFrameManager()->getTF2BufferPtr();
360 #endif
361  move_group_ =
362  std::make_shared<moveit::planning_interface::MoveGroupInterface>(opt, tf_buffer, ros::WallDuration(30, 0));
363 
364  if (planning_scene_storage_)
365  move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value());
366  }
367  catch (std::exception& ex)
368  {
369  ROS_ERROR("%s", ex.what());
370  }
371  planning_display_->addMainLoopJob([&view = *ui_->planner_param_treeview, this] { view.setMoveGroup(move_group_); });
372  if (move_group_)
373  {
374  move_group_->allowLooking(ui_->allow_looking->isChecked());
375  move_group_->allowReplanning(ui_->allow_replanning->isChecked());
376  bool has_unique_endeffector = !move_group_->getEndEffectorLink().empty();
377  planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(has_unique_endeffector); });
378  std::vector<moveit_msgs::PlannerInterfaceDescription> desc;
379  if (move_group_->getInterfaceDescriptions(desc))
380  planning_display_->addMainLoopJob([this, desc] { populatePlannersList(desc); });
381  planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList");
382 
383  if (first_time_)
384  {
385  first_time_ = false;
386  const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
387  if (ps)
388  {
389  planning_display_->setQueryStartState(ps->getCurrentState());
390  planning_display_->setQueryGoalState(ps->getCurrentState());
391  }
392  // This ensures saved UI settings applied after planning_display_ is ready
393  planning_display_->useApproximateIK(ui_->approximate_ik->isChecked());
394  if (ui_->allow_external_program->isChecked())
395  planning_display_->addMainLoopJob([this] { allowExternalProgramCommunication(true); });
396  }
397  }
398  }
399 }
400 
401 void MotionPlanningFrame::clearRobotModel()
402 {
403  ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr());
404  joints_tab_->clearRobotModel();
405  move_group_.reset();
406 }
407 
408 void MotionPlanningFrame::changePlanningGroup()
409 {
410  planning_display_->addBackgroundJob([this] { changePlanningGroupHelper(); }, "Frame::changePlanningGroup");
411  joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(),
412  planning_display_->getQueryStartStateHandler(),
413  planning_display_->getQueryGoalStateHandler());
414 }
415 
416 void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
417 {
419  planning_display_->addMainLoopJob([this] { populateCollisionObjectsList(); });
420 }
421 
422 void MotionPlanningFrame::addSceneObject()
423 {
424  static const double MIN_VAL = 1e-6;
425 
426  // get size values
427  double x_length = ui_->shape_size_x_spin_box->isEnabled() ? ui_->shape_size_x_spin_box->value() : MIN_VAL;
428  double y_length = ui_->shape_size_y_spin_box->isEnabled() ? ui_->shape_size_y_spin_box->value() : MIN_VAL;
429  double z_length = ui_->shape_size_z_spin_box->isEnabled() ? ui_->shape_size_z_spin_box->value() : MIN_VAL;
430  if (x_length < MIN_VAL || y_length < MIN_VAL || z_length < MIN_VAL)
431  {
432  QMessageBox::warning(this, QString("Dimension is too small"), QString("Size values need to be >= %1").arg(MIN_VAL));
433  return;
434  }
435 
436  // by default, name object by shape type
437  std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString();
438  shapes::ShapeConstPtr shape;
439  switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item
440  {
441  case shapes::BOX:
442  shape = std::make_shared<shapes::Box>(x_length, y_length, z_length);
443  break;
444  case shapes::SPHERE:
445  shape = std::make_shared<shapes::Sphere>(0.5 * x_length);
446  break;
447  case shapes::CONE:
448  shape = std::make_shared<shapes::Cone>(0.5 * x_length, z_length);
449  break;
450  case shapes::CYLINDER:
451  shape = std::make_shared<shapes::Cylinder>(0.5 * x_length, z_length);
452  break;
453  case shapes::PLANE:
454  shape = std::make_shared<shapes::Plane>(0., 0., 1., 0.);
455  break;
456  case shapes::MESH:
457  {
458  QUrl url;
459  if (ui_->shapes_combo_box->currentText().contains("file")) // open from file
460  url = QFileDialog::getOpenFileUrl(this, tr("Import Object Mesh"), QString(),
461  "CAD files (*.stl *.obj *.dae);;All files (*.*)");
462  else // open from URL
463  url = QInputDialog::getText(this, tr("Import Object Mesh"), tr("URL for file to import from:"),
464  QLineEdit::Normal, QString("http://"));
465  if (!url.isEmpty())
466  shape = loadMeshResource(url.toString().toStdString());
467  if (!shape)
468  return;
469  // name mesh objects by their file name
470  selected_shape = url.fileName().toStdString();
471  break;
472  }
473  default:
474  QMessageBox::warning(this, QString("Unsupported shape"),
475  QString("The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText()));
476  }
477 
478  std::string shape_name;
479  if (auto ps = planning_display_->getPlanningSceneRW())
480  {
481  // find available (initial) name of object
482  int idx = 0;
483  do
484  shape_name = selected_shape + "_" + std::to_string(++idx);
485  while (ps->getWorld()->hasObject(shape_name));
486 
487  // Actually add object to the plugin's PlanningScene
488  ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity());
489  }
490  setLocalSceneEdited();
491  planning_display_->queueRenderSceneGeometry();
492 
493  // Finally add object name to GUI list
494  auto item = addCollisionObjectToList(shape_name, ui_->collision_objects_list->count(), false);
495 
496  // Select it and make it current so that its IM is displayed
497  ui_->collision_objects_list->clearSelection();
498  item->setSelected(true);
499  ui_->collision_objects_list->setCurrentItem(item);
500 }
501 
502 shapes::ShapePtr MotionPlanningFrame::loadMeshResource(const std::string& url)
503 {
505  if (mesh)
506  {
507  // If the object is very large, ask the user if the scale should be reduced.
508  bool object_is_very_large = false;
509  for (unsigned int i = 0; i < mesh->vertex_count; i++)
510  {
511  if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) ||
512  (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) ||
513  (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD))
514  {
515  object_is_very_large = true;
516  break;
517  }
518  }
519  if (object_is_very_large)
520  {
521  QMessageBox msg_box;
522  msg_box.setText(
523  "The object is very large (greater than 10 m). The file may be in millimeters instead of meters.");
524  msg_box.setInformativeText("Attempt to fix the size by shrinking the object?");
525  msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
526  msg_box.setDefaultButton(QMessageBox::Yes);
527  if (msg_box.exec() == QMessageBox::Yes)
528  {
529  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
530  {
531  unsigned int i3 = i * 3;
532  mesh->vertices[i3] *= 0.001;
533  mesh->vertices[i3 + 1] *= 0.001;
534  mesh->vertices[i3 + 2] *= 0.001;
535  }
536  }
537  }
538 
539  return shapes::ShapePtr(mesh);
540  }
541  else
542  {
543  QMessageBox::warning(this, QString("Import error"), QString("Unable to import object"));
544  return shapes::ShapePtr();
545  }
546 }
547 
548 void MotionPlanningFrame::enable()
549 {
550  ui_->planning_algorithm_combo_box->clear();
551  ui_->library_label->setText("NO PLANNING LIBRARY LOADED");
552  ui_->library_label->setStyleSheet("QLabel { color : red; font: bold }");
553  ui_->object_status->setText("");
554 
555  const std::string new_ns = ros::names::resolve(planning_display_->getMoveGroupNS());
556  if (nh_.getNamespace() != new_ns)
557  {
558  ROS_INFO("MoveGroup namespace changed: %s -> %s. Reloading params.", nh_.getNamespace().c_str(), new_ns.c_str());
559  initFromMoveGroupNS();
560  }
561 
562  // activate the frame
563  if (parentWidget())
564  parentWidget()->show();
565 }
566 
567 // (re)initialize after MotionPlanningDisplay::changedMoveGroupNS()
568 // Should be called from constructor and enable() only
569 void MotionPlanningFrame::initFromMoveGroupNS()
570 {
571  nh_ = ros::NodeHandle(planning_display_->getMoveGroupNS()); // <namespace>/<MoveGroupNS>
572 
573  // Create namespace-dependent services, topics, and subscribers
574  clear_octomap_service_client_ = nh_.serviceClient<std_srvs::Empty>(move_group::CLEAR_OCTOMAP_SERVICE_NAME);
575 
576  object_recognition_subscriber_ =
577  nh_.subscribe("recognized_object_array", 1, &MotionPlanningFrame::listenDetectedObjects, this);
578 
579  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
580  planning_scene_world_publisher_ = nh_.advertise<moveit_msgs::PlanningSceneWorld>("planning_scene_world", 1);
581 
582  // Set initial velocity and acceleration scaling factors from ROS parameters
583  double factor;
584  nh_.param<double>("robot_description_planning/default_velocity_scaling_factor", factor, 0.1);
585  ui_->velocity_scaling_factor->setValue(factor);
586  nh_.param<double>("robot_description_planning/default_acceleration_scaling_factor", factor, 0.1);
587  ui_->acceleration_scaling_factor->setValue(factor);
588 
589  // Fetch parameters from private move_group sub space
590  ros::NodeHandle nh_mg("move_group"); // <namespace>/<MoveGroupNS>/move_group
591  std::string param_name;
592  std::string host_param;
593  int port;
594  if (nh_mg.searchParam("warehouse_host", param_name) && nh_mg.getParam(param_name, host_param))
595  ui_->database_host->setText(QString::fromStdString(host_param));
596  if (nh_mg.searchParam("warehouse_port", param_name) && nh_mg.getParam(param_name, port))
597  ui_->database_port->setValue(port);
598 
599  // Get default planning pipeline id
600  nh_mg.param<std::string>("default_planning_pipeline", default_planning_pipeline_, "");
601 }
602 
603 void MotionPlanningFrame::disable()
604 {
605  move_group_.reset();
606  scene_marker_.reset();
607  if (parentWidget())
608  parentWidget()->hide();
609 }
610 
611 void MotionPlanningFrame::tabChanged(int index)
612 {
613  if (scene_marker_ && ui_->tabWidget->tabText(index).toStdString() != TAB_OBJECTS)
614  scene_marker_.reset();
615  else if (ui_->tabWidget->tabText(index).toStdString() == TAB_OBJECTS)
616  currentCollisionObjectChanged();
617 }
618 
619 void MotionPlanningFrame::updateSceneMarkers(float wall_dt, float /*ros_dt*/)
620 {
621  if (scene_marker_)
622  scene_marker_->update(wall_dt);
623 }
624 
625 void MotionPlanningFrame::updateExternalCommunication()
626 {
627  if (ui_->allow_external_program->isChecked())
628  {
629  planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(true);
630  }
631 }
632 
633 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::MotionPlanningFrame::planningGroupTextChanged
void planningGroupTextChanged(const QString &planning_group)
Definition: motion_planning_frame_planning.cpp:331
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateCustomGoalStateCallback
void remoteUpdateCustomGoalStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:635
moveit_rviz_plugin::MotionPlanningFrame::remotePlanCallback
void remotePlanCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:574
moveit_rviz_plugin::MotionPlanningFrame::fillPlanningGroupOptions
void fillPlanningGroupOptions()
Definition: motion_planning_frame.cpp:306
moveit_rviz_plugin::MotionPlanningDisplay::getRobotInteraction
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
Definition: motion_planning_display.h:114
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateCustomStartStateCallback
void remoteUpdateCustomStartStateCallback(const moveit_msgs::RobotStateConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:617
moveit_rviz_plugin::MotionPlanningFrame::changePlanningGroupHelper
void changePlanningGroupHelper()
Definition: motion_planning_frame.cpp:362
shapes::PLANE
PLANE
motion_planning_display.h
moveit_rviz_plugin::MotionPlanningFrame::MotionPlanningFrame
MotionPlanningFrame(const MotionPlanningFrame &)=delete
moveit_rviz_plugin::MotionPlanningFrame::shapesComboBoxChanged
void shapesComboBoxChanged(const QString &text)
Definition: motion_planning_frame_objects.cpp:76
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateGoalStateCallback
void remoteUpdateGoalStateCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:603
frame_manager.h
shape_operations.h
buffer.h
ros::Subscriber::shutdown
void shutdown()
moveit_rviz_plugin::MotionPlanningFrame::addSceneObject
void addSceneObject()
Definition: motion_planning_frame.cpp:454
moveit_rviz_plugin::MotionPlanningFrame::enable
void enable()
Definition: motion_planning_frame.cpp:580
shapes::SPHERE
SPHERE
moveit_rviz_plugin::PlanningSceneDisplay::getPlanningSceneMonitor
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
Definition: planning_scene_display.cpp:300
shapes::Mesh
moveit_rviz_plugin::PlanningSceneDisplay::addMainLoopJob
void addMainLoopJob(const boost::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
Definition: planning_scene_display.cpp:265
moveit_rviz_plugin::MotionPlanningFrame::allowExternalProgramCommunication
void allowExternalProgramCommunication(bool enable)
Definition: motion_planning_frame.cpp:270
moveit::planning_interface::MoveGroupInterface::Options
shapes::MESH
MESH
ros::names::resolve
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
moveit::planning_interface::getSharedTF
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
moveit_rviz_plugin::MotionPlanningFrame::~MotionPlanningFrame
~MotionPlanningFrame() override
Definition: motion_planning_frame.cpp:260
planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType
SceneUpdateType
shapes::CONE
CONE
move_group::CLEAR_OCTOMAP_SERVICE_NAME
static const std::string CLEAR_OCTOMAP_SERVICE_NAME
moveit_rviz_plugin::OBJECT_RECOGNITION_ACTION
const std::string OBJECT_RECOGNITION_ACTION
Definition: motion_planning_frame.h:86
moveit_rviz_plugin::MotionPlanningFrame::update_goal_state_subscriber_
ros::Subscriber update_goal_state_subscriber_
Definition: motion_planning_frame.h:307
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
moveit_rviz_plugin::MotionPlanningFrame::ui_
Ui::MotionPlanningUI * ui_
Definition: motion_planning_frame.h:127
Ui
Definition: motion_planning_frame.h:69
capability_names.h
moveit_rviz_plugin::MotionPlanningDisplay::toggleSelectPlanningGroupSubscription
void toggleSelectPlanningGroupSubscription(bool enable)
Definition: motion_planning_display.cpp:306
shapes::Mesh::vertex_count
unsigned int vertex_count
moveit_rviz_plugin::MotionPlanningDisplay::queryStartStateChanged
void queryStartStateChanged()
rviz::Property::connect
std::enable_if<!QtPrivate::FunctionPointer< Func >::IsPointerToMemberFunction, QMetaObject::Connection >::type connect(const QObject *context, Func &&slot, Qt::ConnectionType type=Qt::AutoConnection)
ROS_ERROR
#define ROS_ERROR(...)
common_objects.h
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
moveit::core::JointModelGroup::getDefaultStateNames
const std::vector< std::string > & getDefaultStateNames() const
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
shapes::Mesh::vertices
double * vertices
rviz::DisplayContext
moveit_rviz_plugin::MotionPlanningFrame::remoteStopCallback
void remoteStopCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:584
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::MotionPlanningFrame::update_start_state_subscriber_
ros::Subscriber update_start_state_subscriber_
Definition: motion_planning_frame.h:306
moveit_rviz_plugin::TAB_OBJECTS
static const std::string TAB_OBJECTS
Definition: motion_planning_frame.h:91
moveit_rviz_plugin::MotionPlanningFrame::stop_subscriber_
ros::Subscriber stop_subscriber_
Definition: motion_planning_frame.h:305
moveit_rviz_plugin::MotionPlanningFrame::fillStateSelectionOptions
void fillStateSelectionOptions()
Definition: motion_planning_frame.cpp:316
moveit_rviz_plugin::MotionPlanningFrame::approximateIKChanged
void approximateIKChanged(int state)
Definition: motion_planning_frame.cpp:265
tf_buffer
tf2_ros::Buffer * tf_buffer
planning_scene_monitor::LockedPlanningSceneRO
moveit_rviz_plugin::MotionPlanningFrame::remoteUpdateStartStateCallback
void remoteUpdateStartStateCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:589
moveit_rviz_plugin::MotionPlanningFrame::populateConstraintsList
void populateConstraintsList()
Definition: motion_planning_frame_planning.cpp:487
moveit_rviz_plugin::MotionPlanningFrame::update_custom_start_state_subscriber_
ros::Subscriber update_custom_start_state_subscriber_
Definition: motion_planning_frame.h:308
shapes::CYLINDER
CYLINDER
moveit_rviz_plugin::MotionPlanningDisplay::useApproximateIK
void useApproximateIK(bool flag)
Definition: motion_planning_display.cpp:986
moveit_rviz_plugin::MotionPlanningFrame::update_custom_goal_state_subscriber_
ros::Subscriber update_custom_goal_state_subscriber_
Definition: motion_planning_frame.h:309
moveit::core::JointModelGroup
moveit_rviz_plugin::PlanningSceneDisplay::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: planning_scene_display.cpp:310
moveit_rviz_plugin::MotionPlanningDisplay::getCurrentPlanningGroup
std::string getCurrentPlanningGroup() const
Definition: motion_planning_display.cpp:1083
moveit_rviz_plugin::MotionPlanningDisplay::queryGoalStateChanged
void queryGoalStateChanged()
planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY
UPDATE_GEOMETRY
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
motion_planning_frame_joints_widget.h
moveit_rviz_plugin::MotionPlanningFrame::planning_display_
MotionPlanningDisplay * planning_display_
Definition: motion_planning_frame.h:125
moveit_rviz_plugin::MotionPlanningFrame::remoteExecuteCallback
void remoteExecuteCallback(const std_msgs::EmptyConstPtr &msg)
Definition: motion_planning_frame_planning.cpp:579
ros::WallDuration
ROS_INFO
#define ROS_INFO(...)
moveit_rviz_plugin::MotionPlanningFrame::execute_subscriber_
ros::Subscriber execute_subscriber_
Definition: motion_planning_frame.h:304
motion_planning_frame.h
moveit_rviz_plugin::MotionPlanningFrame::first_time_
bool first_time_
Definition: motion_planning_frame.h:333
ros::Duration
shapes::BOX
BOX
ros::NodeHandle
moveit_rviz_plugin::MotionPlanningFrame::plan_subscriber_
ros::Subscriber plan_subscriber_
Definition: motion_planning_frame.h:303
moveit_rviz_plugin::LARGE_MESH_THRESHOLD
static const double LARGE_MESH_THRESHOLD
Definition: motion_planning_frame.h:96
display_context.h
group
group


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:25