moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 #include <moveit/utils/logger.hpp>
45 
46 #include <geometric_shapes/shape_operations.h>
47 
48 #include <rviz_common/display_context.hpp>
49 #include <rviz_common/frame_manager_iface.hpp>
50 #include <tf2_ros/buffer.h>
51 
52 #include <std_srvs/srv/empty.hpp>
53 
54 #include <QMessageBox>
55 #include <QInputDialog>
56 #include <QFileDialog>
57 #include <QComboBox>
58 #include <QShortcut>
59 
60 #include "ui_motion_planning_rviz_plugin_frame.h"
61 
62 namespace moveit_rviz_plugin
63 {
64 
65 MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_common::DisplayContext* context,
66  QWidget* parent)
67  : QWidget(parent)
68  , planning_display_(pdisplay)
69  , context_(context)
70  , ui_(new Ui::MotionPlanningUI())
71  , logger_(moveit::getLogger("moveit.ros.motion_planning_frame"))
72  , first_time_(true)
73 {
74  auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
75  if (!ros_node_abstraction)
76  {
77  RCLCPP_INFO(logger_, "Unable to lock weak_ptr from DisplayContext in MotionPlanningFrame constructor");
78  return;
79  }
80  node_ = ros_node_abstraction->get_raw_node();
81 
82  // Prepare database parameters
83  if (!node_->has_parameter("warehouse_host"))
84  node_->declare_parameter<std::string>("warehouse_host", "127.0.0.1");
85  if (!node_->has_parameter("warehouse_plugin"))
86  node_->declare_parameter<std::string>("warehouse_plugin", "warehouse_ros_mongo::MongoDatabaseConnection");
87  if (!node_->has_parameter("warehouse_port"))
88  node_->declare_parameter<int>("warehouse_port", 33829);
89 
90  // set up the GUI
91  ui_->setupUi(this);
92  ui_->shapes_combo_box->addItem("Box", shapes::BOX);
93  ui_->shapes_combo_box->addItem("Sphere", shapes::SPHERE);
94  ui_->shapes_combo_box->addItem("Cylinder", shapes::CYLINDER);
95  ui_->shapes_combo_box->addItem("Cone", shapes::CONE);
96  ui_->shapes_combo_box->addItem("Mesh from file", shapes::MESH);
97  ui_->shapes_combo_box->addItem("Mesh from URL", shapes::MESH);
98  setLocalSceneEdited(false);
99 
100  // add more tabs
102  ui_->tabWidget->insertTab(2, joints_tab_, "Joints");
103  connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged()));
104  connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged()));
105 
106  // connect buttons to actions; each action usually registers the function pointer for the actual computation,
107  // to keep the GUI more responsive (using the background job processing)
108  connect(ui_->plan_button, SIGNAL(clicked()), this, SLOT(planButtonClicked()));
109  connect(ui_->execute_button, SIGNAL(clicked()), this, SLOT(executeButtonClicked()));
110  connect(ui_->plan_and_execute_button, SIGNAL(clicked()), this, SLOT(planAndExecuteButtonClicked()));
111  connect(ui_->stop_button, SIGNAL(clicked()), this, SLOT(stopButtonClicked()));
112  connect(ui_->start_state_combo_box, SIGNAL(activated(QString)), this, SLOT(startStateTextChanged(QString)));
113  connect(ui_->goal_state_combo_box, SIGNAL(activated(QString)), this, SLOT(goalStateTextChanged(QString)));
114  connect(ui_->planning_group_combo_box, SIGNAL(currentIndexChanged(QString)), this,
115  SLOT(planningGroupTextChanged(QString)));
116  connect(ui_->database_connect_button, SIGNAL(clicked()), this, SLOT(databaseConnectButtonClicked()));
117  connect(ui_->save_scene_button, SIGNAL(clicked()), this, SLOT(saveSceneButtonClicked()));
118  connect(ui_->save_query_button, SIGNAL(clicked()), this, SLOT(saveQueryButtonClicked()));
119  connect(ui_->delete_scene_button, SIGNAL(clicked()), this, SLOT(deleteSceneButtonClicked()));
120  connect(ui_->delete_query_button, SIGNAL(clicked()), this, SLOT(deleteQueryButtonClicked()));
121  connect(ui_->planning_scene_tree, SIGNAL(itemSelectionChanged()), this, SLOT(planningSceneItemClicked()));
122  connect(ui_->load_scene_button, SIGNAL(clicked()), this, SLOT(loadSceneButtonClicked()));
123  connect(ui_->load_query_button, SIGNAL(clicked()), this, SLOT(loadQueryButtonClicked()));
124  connect(ui_->allow_looking, SIGNAL(toggled(bool)), this, SLOT(allowLookingToggled(bool)));
125  connect(ui_->allow_replanning, SIGNAL(toggled(bool)), this, SLOT(allowReplanningToggled(bool)));
126  connect(ui_->allow_external_program, SIGNAL(toggled(bool)), this, SLOT(allowExternalProgramCommunication(bool)));
127  connect(ui_->planning_pipeline_combo_box, SIGNAL(currentIndexChanged(int)), this,
128  SLOT(planningPipelineIndexChanged(int)));
129  connect(ui_->planning_algorithm_combo_box, SIGNAL(currentIndexChanged(int)), this,
130  SLOT(planningAlgorithmIndexChanged(int)));
131  connect(ui_->clear_scene_button, SIGNAL(clicked()), this, SLOT(clearScene()));
132  connect(ui_->scene_scale, SIGNAL(valueChanged(int)), this, SLOT(sceneScaleChanged(int)));
133  connect(ui_->scene_scale, SIGNAL(sliderPressed()), this, SLOT(sceneScaleStartChange()));
134  connect(ui_->scene_scale, SIGNAL(sliderReleased()), this, SLOT(sceneScaleEndChange()));
135  connect(ui_->remove_object_button, SIGNAL(clicked()), this, SLOT(removeSceneObject()));
136  connect(ui_->object_x, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
137  connect(ui_->object_y, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
138  connect(ui_->object_z, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
139  connect(ui_->object_rx, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
140  connect(ui_->object_ry, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
141  connect(ui_->object_rz, SIGNAL(valueChanged(double)), this, SLOT(objectPoseValueChanged(double)));
142  connect(ui_->publish_current_scene_button, SIGNAL(clicked()), this, SLOT(publishScene()));
143  connect(ui_->collision_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedCollisionObjectChanged()));
144  connect(ui_->collision_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
145  SLOT(collisionObjectChanged(QListWidgetItem*)));
146  connect(ui_->path_constraints_combo_box, SIGNAL(currentIndexChanged(int)), this,
147  SLOT(pathConstraintsIndexChanged(int)));
148  connect(ui_->clear_octomap_button, SIGNAL(clicked()), this, SLOT(onClearOctomapClicked()));
149  connect(ui_->planning_scene_tree, SIGNAL(itemChanged(QTreeWidgetItem*, int)), this,
150  SLOT(warehouseItemNameChanged(QTreeWidgetItem*, int)));
151  connect(ui_->reset_db_button, SIGNAL(clicked()), this, SLOT(resetDbButtonClicked()));
152 
153  connect(ui_->add_object_button, &QPushButton::clicked, this, &MotionPlanningFrame::addSceneObject);
154  connect(ui_->shapes_combo_box, &QComboBox::currentTextChanged, this, &MotionPlanningFrame::shapesComboBoxChanged);
155  connect(ui_->export_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(exportGeometryAsTextButtonClicked()));
156  connect(ui_->import_scene_geometry_text_button, SIGNAL(clicked()), this, SLOT(importGeometryFromTextButtonClicked()));
157  connect(ui_->load_state_button, SIGNAL(clicked()), this, SLOT(loadStateButtonClicked()));
158  connect(ui_->save_start_state_button, SIGNAL(clicked()), this, SLOT(saveStartStateButtonClicked()));
159  connect(ui_->save_goal_state_button, SIGNAL(clicked()), this, SLOT(saveGoalStateButtonClicked()));
160  connect(ui_->set_as_start_state_button, SIGNAL(clicked()), this, SLOT(setAsStartStateButtonClicked()));
161  connect(ui_->set_as_goal_state_button, SIGNAL(clicked()), this, SLOT(setAsGoalStateButtonClicked()));
162  connect(ui_->remove_state_button, SIGNAL(clicked()), this, SLOT(removeStateButtonClicked()));
163  connect(ui_->clear_states_button, SIGNAL(clicked()), this, SLOT(clearStatesButtonClicked()));
164  connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SLOT(approximateIKChanged(int)));
165 
166  connect(ui_->detect_objects_button, SIGNAL(clicked()), this, SLOT(detectObjectsButtonClicked()));
167  connect(ui_->pick_button, SIGNAL(clicked()), this, SLOT(pickObjectButtonClicked()));
168  connect(ui_->place_button, SIGNAL(clicked()), this, SLOT(placeObjectButtonClicked()));
169  connect(ui_->detected_objects_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedDetectedObjectChanged()));
170  connect(ui_->detected_objects_list, SIGNAL(itemChanged(QListWidgetItem*)), this,
171  SLOT(detectedObjectChanged(QListWidgetItem*)));
172  connect(ui_->support_surfaces_list, SIGNAL(itemSelectionChanged()), this, SLOT(selectedSupportSurfaceChanged()));
173 
174  connect(ui_->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabChanged(int)));
175 
176  /* Notice changes to be safed in config file */
177  connect(ui_->database_host, SIGNAL(textChanged(QString)), this, SIGNAL(configChanged()));
178  connect(ui_->database_port, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
179 
180  connect(ui_->planning_time, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
181  connect(ui_->planning_attempts, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
182  connect(ui_->velocity_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
183  connect(ui_->acceleration_scaling_factor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
184 
185  connect(ui_->allow_replanning, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
186  connect(ui_->allow_looking, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
187  connect(ui_->allow_external_program, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
188  connect(ui_->use_cartesian_path, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
189  connect(ui_->collision_aware_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
190  connect(ui_->approximate_ik, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
191 
192  connect(ui_->wcenter_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
193  connect(ui_->wcenter_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
194  connect(ui_->wcenter_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
195  connect(ui_->wsize_x, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
196  connect(ui_->wsize_y, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
197  connect(ui_->wsize_z, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
198 
199  QShortcut* copy_object_shortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_C), ui_->collision_objects_list);
200  connect(copy_object_shortcut, SIGNAL(activated()), this, SLOT(copySelectedCollisionObject()));
201 
202  ui_->reset_db_button->hide();
203  ui_->background_job_progress->hide();
204  ui_->background_job_progress->setMaximum(0);
205 
206  ui_->tabWidget->setCurrentIndex(1);
207 
208  known_collision_objects_version_ = 0;
209 
211 
212  object_recognition_client_ = rclcpp_action::create_client<object_recognition_msgs::action::ObjectRecognition>(
214 
215  if (object_recognition_client_)
216  {
217  if (!object_recognition_client_->wait_for_action_server(std::chrono::seconds(3)))
218  {
219  RCLCPP_ERROR(logger_, "Action server: %s not available", OBJECT_RECOGNITION_ACTION.c_str());
220  object_recognition_client_.reset();
221  }
222  }
223  // TODO (ddengster): Enable when moveit_ros_perception is ported
224 
225  // try
226  // {
227  // const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
228  // if (ps)
229  // {
230  // semantic_world_.reset(new moveit::semantic_world::SemanticWorld(ps));
231  // }
232  // else
233  // semantic_world_.reset();
234  // if (semantic_world_)
235  // {
236  // semantic_world_->addTableCallback([this] { updateTables(); });
237  // }
238  // }
239  // catch (std::exception& ex)
240  // {
241  // RCLCPP_ERROR(logger_, "Failed to get semantic world: %s", ex.what());
242  // }
243 }
244 
246 {
247  delete ui_;
248 }
249 
250 void MotionPlanningFrame::approximateIKChanged(int state)
251 {
252  planning_display_->useApproximateIK(state == Qt::Checked);
253 }
254 
255 void MotionPlanningFrame::setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list)
256 {
257  QList<QListWidgetItem*> found_items = list->findItems(QString(item_name.c_str()), Qt::MatchExactly);
258  for (QListWidgetItem* found_item : found_items)
259  found_item->setSelected(selection);
260 }
261 
262 void MotionPlanningFrame::allowExternalProgramCommunication(bool enable)
263 {
264  // This is needed to prevent UI event (resuming the options) triggered
265  // before getRobotInteraction() is loaded and ready
266  if (first_time_)
267  return;
268 
269  planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(enable);
271  if (enable)
272  {
273  using std::placeholders::_1;
274  plan_subscriber_ = node_->create_subscription<std_msgs::msg::Empty>(
275  "/rviz/moveit/plan", rclcpp::SystemDefaultsQoS(),
276  [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remotePlanCallback(msg); });
277  execute_subscriber_ = node_->create_subscription<std_msgs::msg::Empty>(
278  "/rviz/moveit/execute", rclcpp::SystemDefaultsQoS(),
279  [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteExecuteCallback(msg); });
280  stop_subscriber_ = node_->create_subscription<std_msgs::msg::Empty>(
281  "/rviz/moveit/stop", rclcpp::SystemDefaultsQoS(),
282  [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteStopCallback(msg); });
283  update_start_state_subscriber_ = node_->create_subscription<std_msgs::msg::Empty>(
284  "/rviz/moveit/update_start_state", rclcpp::SystemDefaultsQoS(),
285  [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateStartStateCallback(msg); });
286  update_goal_state_subscriber_ = node_->create_subscription<std_msgs::msg::Empty>(
287  "/rviz/moveit/update_goal_state", rclcpp::SystemDefaultsQoS(),
288  [this](const std_msgs::msg::Empty::ConstSharedPtr& msg) { return remoteUpdateGoalStateCallback(msg); });
289  update_custom_start_state_subscriber_ = node_->create_subscription<moveit_msgs::msg::RobotState>(
290  "/rviz/moveit/update_custom_start_state", rclcpp::SystemDefaultsQoS(),
291  [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) {
292  return remoteUpdateCustomStartStateCallback(msg);
293  });
294  update_custom_goal_state_subscriber_ = node_->create_subscription<moveit_msgs::msg::RobotState>(
295  "/rviz/moveit/update_custom_goal_state", rclcpp::SystemDefaultsQoS(),
296  [this](const moveit_msgs::msg::RobotState::ConstSharedPtr& msg) {
297  return remoteUpdateCustomGoalStateCallback(msg);
298  });
299  }
300  else
301  { // disable
302  plan_subscriber_.reset();
303  execute_subscriber_.reset();
304  stop_subscriber_.reset();
305  update_start_state_subscriber_.reset();
306  update_goal_state_subscriber_.reset();
307  update_custom_start_state_subscriber_.reset();
308  update_custom_goal_state_subscriber_.reset();
309  }
310 }
311 
312 void MotionPlanningFrame::fillPlanningGroupOptions()
313 {
314  const QSignalBlocker planning_group_blocker(ui_->planning_group_combo_box);
315  ui_->planning_group_combo_box->clear();
316 
317  const moveit::core::RobotModelConstPtr& kmodel = planning_display_->getRobotModel();
318  for (const std::string& group_name : kmodel->getJointModelGroupNames())
319  ui_->planning_group_combo_box->addItem(QString::fromStdString(group_name));
320 }
321 
322 void MotionPlanningFrame::fillStateSelectionOptions()
323 {
324  const QSignalBlocker start_state_blocker(ui_->start_state_combo_box);
325  const QSignalBlocker goal_state_blocker(ui_->goal_state_combo_box);
326  ui_->start_state_combo_box->clear();
327  ui_->goal_state_combo_box->clear();
328 
330  return;
331 
332  const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
333  std::string group = planning_display_->getCurrentPlanningGroup();
334  if (group.empty())
335  return;
336  const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
337  if (jmg)
338  {
339  ui_->start_state_combo_box->addItem(QString("<random valid>"));
340  ui_->start_state_combo_box->addItem(QString("<random>"));
341  ui_->start_state_combo_box->addItem(QString("<current>"));
342  ui_->start_state_combo_box->addItem(QString("<same as goal>"));
343  ui_->start_state_combo_box->addItem(QString("<previous>"));
344 
345  ui_->goal_state_combo_box->addItem(QString("<random valid>"));
346  ui_->goal_state_combo_box->addItem(QString("<random>"));
347  ui_->goal_state_combo_box->addItem(QString("<current>"));
348  ui_->goal_state_combo_box->addItem(QString("<same as start>"));
349  ui_->goal_state_combo_box->addItem(QString("<previous>"));
350 
351  const std::vector<std::string>& known_states = jmg->getDefaultStateNames();
352  if (!known_states.empty())
353  {
354  ui_->start_state_combo_box->insertSeparator(ui_->start_state_combo_box->count());
355  ui_->goal_state_combo_box->insertSeparator(ui_->goal_state_combo_box->count());
356  for (const std::string& known_state : known_states)
357  {
358  ui_->start_state_combo_box->addItem(QString::fromStdString(known_state));
359  ui_->goal_state_combo_box->addItem(QString::fromStdString(known_state));
360  }
361  }
362 
363  ui_->start_state_combo_box->setCurrentIndex(2); // default to 'current'
364  ui_->goal_state_combo_box->setCurrentIndex(2); // default to 'current'
365  }
366 }
367 
368 void MotionPlanningFrame::changePlanningGroupHelper()
369 {
371  return;
372 
373  planning_display_->addMainLoopJob([this] { fillStateSelectionOptions(); });
374  planning_display_->addMainLoopJob([this]() { populateConstraintsList(std::vector<std::string>()); });
375 
376  const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
377  std::string group = planning_display_->getCurrentPlanningGroup();
378  planning_display_->addMainLoopJob([&view = *ui_->planner_param_treeview, group] { view.setGroupName(group); });
379  planning_display_->addMainLoopJob(
380  [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); });
381 
382  if (!group.empty() && robot_model)
383  {
384  RCLCPP_INFO(logger_, "group %s", group.c_str());
385  if (move_group_ && move_group_->getName() == group)
386  return;
387  RCLCPP_INFO(logger_, "Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(),
388  planning_display_->getMoveGroupNS().c_str());
390  group, moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION, planning_display_->getMoveGroupNS());
391  opt.robot_model = robot_model;
392  opt.robot_description.clear();
393  try
394  {
395 #ifdef RVIZ_TF1
396  std::shared_ptr<tf2_ros::Buffer> tf_buffer = moveit::planning_interface::getSharedTF();
397 #else
398  //@note: tf2 no longer accessible?
399  // /std::shared_ptr<tf2_ros::Buffer> tf_buffer = context_->getFrameManager()->getTF2BufferPtr();
400  std::shared_ptr<tf2_ros::Buffer> tf_buffer = moveit::planning_interface::getSharedTF();
401 #endif
402  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
403  node_, opt, tf_buffer, rclcpp::Duration::from_seconds(30));
404  if (planning_scene_storage_)
405  move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value());
406  }
407  catch (std::exception& ex)
408  {
409  RCLCPP_ERROR(logger_, "%s", ex.what());
410  }
411  planning_display_->addMainLoopJob([&view = *ui_->planner_param_treeview, this] { view.setMoveGroup(move_group_); });
412  if (move_group_)
413  {
414  move_group_->allowLooking(ui_->allow_looking->isChecked());
415  move_group_->allowReplanning(ui_->allow_replanning->isChecked());
416  bool has_unique_endeffector = !move_group_->getEndEffectorLink().empty();
417  planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(has_unique_endeffector); });
418  std::vector<moveit_msgs::msg::PlannerInterfaceDescription> desc;
419  if (move_group_->getInterfaceDescriptions(desc))
420  planning_display_->addMainLoopJob([this, desc] { populatePlannersList(desc); });
421  planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList");
422 
423  if (first_time_)
424  {
425  first_time_ = false;
426  const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
427  if (ps)
428  {
429  planning_display_->setQueryStartState(ps->getCurrentState());
430  planning_display_->setQueryGoalState(ps->getCurrentState());
431  }
432  // This ensures saved UI settings applied after planning_display_ is ready
433  planning_display_->useApproximateIK(ui_->approximate_ik->isChecked());
434  if (ui_->allow_external_program->isChecked())
435  planning_display_->addMainLoopJob([this] { allowExternalProgramCommunication(true); });
436  }
437  }
438  }
439 }
440 
441 void MotionPlanningFrame::clearRobotModel()
442 {
443  ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr());
444  joints_tab_->clearRobotModel();
445  move_group_.reset();
446 }
447 
448 void MotionPlanningFrame::changePlanningGroup()
449 {
450  planning_display_->addBackgroundJob([this] { changePlanningGroupHelper(); }, "Frame::changePlanningGroup");
451  joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(),
452  planning_display_->getQueryStartStateHandler(),
453  planning_display_->getQueryGoalStateHandler());
454 }
455 
456 void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
457 {
459  planning_display_->addMainLoopJob([this] { populateCollisionObjectsList(); });
460 }
461 
462 void MotionPlanningFrame::addSceneObject()
463 {
464  static const double MIN_VAL = 1e-6;
465 
466  if (!planning_display_->getPlanningSceneMonitor())
467  {
468  return;
469  }
470 
471  // get size values
472  double x_length = ui_->shape_size_x_spin_box->isEnabled() ? ui_->shape_size_x_spin_box->value() : MIN_VAL;
473  double y_length = ui_->shape_size_y_spin_box->isEnabled() ? ui_->shape_size_y_spin_box->value() : MIN_VAL;
474  double z_length = ui_->shape_size_z_spin_box->isEnabled() ? ui_->shape_size_z_spin_box->value() : MIN_VAL;
475  if (x_length < MIN_VAL || y_length < MIN_VAL || z_length < MIN_VAL)
476  {
477  QMessageBox::warning(this, QString("Dimension is too small"), QString("Size values need to be >= %1").arg(MIN_VAL));
478  return;
479  }
480 
481  // by default, name object by shape type
482  std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString();
483  shapes::ShapeConstPtr shape;
484  switch (ui_->shapes_combo_box->currentData().toInt()) // fetch shape ID from current combobox item
485  {
486  case shapes::BOX:
487  shape = std::make_shared<shapes::Box>(x_length, y_length, z_length);
488  break;
489  case shapes::SPHERE:
490  shape = std::make_shared<shapes::Sphere>(0.5 * x_length);
491  break;
492  case shapes::CONE:
493  shape = std::make_shared<shapes::Cone>(0.5 * x_length, z_length);
494  break;
495  case shapes::CYLINDER:
496  shape = std::make_shared<shapes::Cylinder>(0.5 * x_length, z_length);
497  break;
498  case shapes::MESH:
499  {
500  QUrl url;
501  if (ui_->shapes_combo_box->currentText().contains("file"))
502  { // open from file
503  url = QFileDialog::getOpenFileUrl(this, tr("Import Object Mesh"), QString(),
504  "CAD files (*.stl *.obj *.dae);;All files (*.*)");
505  }
506  else
507  { // open from URL
508  url = QInputDialog::getText(this, tr("Import Object Mesh"), tr("URL for file to import from:"),
509  QLineEdit::Normal, QString("http://"));
510  }
511  if (!url.isEmpty())
512  shape = loadMeshResource(url.toString().toStdString());
513  if (!shape)
514  return;
515  // name mesh objects by their file name
516  selected_shape = url.fileName().toStdString();
517  break;
518  }
519  default:
520  QMessageBox::warning(this, QString("Unsupported shape"),
521  QString("The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText()));
522  }
523 
524  // find available (initial) name of object
525  int idx = 0;
526  std::string shape_name = selected_shape + "_" + std::to_string(idx);
527  while (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(shape_name))
528  {
529  idx++;
530  shape_name = selected_shape + "_" + std::to_string(idx);
531  }
532 
533  // Actually add object to the plugin's PlanningScene
534  {
535  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
536  ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity());
537  }
538  setLocalSceneEdited();
539 
540  planning_display_->addMainLoopJob([this] { populateCollisionObjectsList(); });
541 
542  // Automatically select the inserted object so that its IM is displayed
543  planning_display_->addMainLoopJob([this, shape_name, list_widget = ui_->collision_objects_list] {
544  setItemSelectionInList(shape_name, true, list_widget);
545  });
546 
547  planning_display_->queueRenderSceneGeometry();
548 }
549 
550 shapes::ShapePtr MotionPlanningFrame::loadMeshResource(const std::string& url)
551 {
552  shapes::Mesh* mesh = shapes::createMeshFromResource(url);
553  if (mesh)
554  {
555  // If the object is very large, ask the user if the scale should be reduced.
556  bool object_is_very_large = false;
557  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
558  {
559  if ((abs(mesh->vertices[i * 3 + 0]) > LARGE_MESH_THRESHOLD) ||
560  (abs(mesh->vertices[i * 3 + 1]) > LARGE_MESH_THRESHOLD) ||
561  (abs(mesh->vertices[i * 3 + 2]) > LARGE_MESH_THRESHOLD))
562  {
563  object_is_very_large = true;
564  break;
565  }
566  }
567  if (object_is_very_large)
568  {
569  QMessageBox msg_box;
570  msg_box.setText(
571  "The object is very large (greater than 10 m). The file may be in millimeters instead of meters.");
572  msg_box.setInformativeText("Attempt to fix the size by shrinking the object?");
573  msg_box.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
574  msg_box.setDefaultButton(QMessageBox::Yes);
575  if (msg_box.exec() == QMessageBox::Yes)
576  {
577  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
578  {
579  unsigned int i3 = i * 3;
580  mesh->vertices[i3] *= 0.001;
581  mesh->vertices[i3 + 1] *= 0.001;
582  mesh->vertices[i3 + 2] *= 0.001;
583  }
584  }
585  }
586 
587  return shapes::ShapePtr(mesh);
588  }
589  else
590  {
591  QMessageBox::warning(this, QString("Import error"), QString("Unable to import object"));
592  return shapes::ShapePtr();
593  }
594 }
595 
596 void MotionPlanningFrame::enable()
597 {
598  ui_->planning_algorithm_combo_box->clear();
599  ui_->library_label->setText("NO PLANNING LIBRARY LOADED");
600  ui_->library_label->setStyleSheet("QLabel { color : red; font: bold }");
601  ui_->object_status->setText("");
602 
603  const std::string new_ns = planning_display_->getMoveGroupNS();
604  if (node_->get_namespace() != new_ns)
605  {
606  RCLCPP_INFO(logger_, "MoveGroup namespace changed: %s -> %s. Reloading params.", node_->get_namespace(),
607  new_ns.c_str());
608  initFromMoveGroupNS();
609  }
610 
611  // activate the frame
612  if (parentWidget())
613  parentWidget()->show();
614 }
615 
616 // (re)initialize after MotionPlanningDisplay::changedMoveGroupNS()
617 // Should be called from constructor and enable() only
618 void MotionPlanningFrame::initFromMoveGroupNS()
619 {
620  // Create namespace-dependent services, topics, and subscribers
621  clear_octomap_service_client_ = node_->create_client<std_srvs::srv::Empty>(move_group::CLEAR_OCTOMAP_SERVICE_NAME);
622 
623  object_recognition_subscriber_ = node_->create_subscription<object_recognition_msgs::msg::RecognizedObjectArray>(
624  "recognized_object_array", rclcpp::SystemDefaultsQoS(),
625  [this](const object_recognition_msgs::msg::RecognizedObjectArray::ConstSharedPtr& msg) {
626  return listenDetectedObjects(msg);
627  });
628 
629  planning_scene_publisher_ = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 1);
630  planning_scene_world_publisher_ =
631  node_->create_publisher<moveit_msgs::msg::PlanningSceneWorld>("planning_scene_world", 1);
632 
633  // Declare parameter for default planning pipeline
634  if (!node_->has_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline"))
635  node_->declare_parameter<std::string>(planning_display_->getMoveGroupNS() + "default_planning_pipeline", "");
636 
637  // Query default planning pipeline id
638  node_->get_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline", default_planning_pipeline_);
639 
640  // Set initial velocity and acceleration scaling factors from ROS parameters
641  double factor;
642  node_->get_parameter_or("robot_description_planning.default_velocity_scaling_factor", factor, 0.1);
643  ui_->velocity_scaling_factor->setValue(factor);
644  node_->get_parameter_or("robot_description_planning.default_acceleration_scaling_factor", factor, 0.1);
645  ui_->acceleration_scaling_factor->setValue(factor);
646 
647  // Fetch parameters from private move_group sub space
648  std::string host_param;
649  if (node_->get_parameter("warehouse_host", host_param))
650  {
651  ui_->database_host->setText(QString::fromStdString(host_param));
652  }
653 
654  int port;
655  if (node_->get_parameter("warehouse_port", port))
656  {
657  ui_->database_port->setValue(port);
658  }
659 }
660 
661 void MotionPlanningFrame::disable()
662 {
663  move_group_.reset();
664  scene_marker_.reset();
665  if (parentWidget())
666  parentWidget()->hide();
667 }
668 
669 void MotionPlanningFrame::tabChanged(int index)
670 {
671  if (scene_marker_ && ui_->tabWidget->tabText(index).toStdString() != TAB_OBJECTS)
672  {
673  scene_marker_.reset();
674  }
675  else if (ui_->tabWidget->tabText(index).toStdString() == TAB_OBJECTS)
676  {
677  selectedCollisionObjectChanged();
678  }
679 }
680 
681 void MotionPlanningFrame::updateSceneMarkers(double /*wall_dt*/, double /*ros_dt*/)
682 {
683  if (scene_marker_)
684  scene_marker_->update();
685 }
686 
687 void MotionPlanningFrame::updateExternalCommunication()
688 {
689  if (ui_->allow_external_program->isChecked())
690  {
691  planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(true);
692  }
693 }
694 
695 } // namespace moveit_rviz_plugin
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
MotionPlanningFrameJointsWidget * joints_tab_
MotionPlanningFrame(const MotionPlanningFrame &)=delete
void addMainLoopJob(const std::function< void()> &job)
queue the execution of this function for the next time the main update() loop gets called
const moveit::core::RobotModelConstPtr & getRobotModel() const
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor()
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
@ UPDATE_GEOMETRY
The geometry of the scene was updated. This includes receiving new octomaps, collision objects,...
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
const std::string OBJECT_RECOGNITION_ACTION
Main namespace for MoveIt.
Definition: exceptions.h:43
Specification of options to use when constructing the MoveGroupInterface class.