51 #include <std_srvs/Empty.h>
53 #include <QMessageBox>
54 #include <QInputDialog>
55 #include <QFileDialog>
59 #include "ui_motion_planning_rviz_plugin_frame.h"
64 : QWidget(parent), planning_display_(pdisplay), context_(context), ui_(new
Ui::MotionPlanningUI()), first_time_(true)
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);
78 joints_tab_ =
new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget);
79 ui_->tabWidget->insertTab(2, joints_tab_,
"Joints");
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()));
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)));
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()));
151 connect(ui_->tabWidget, SIGNAL(currentChanged(
int)),
this, SLOT(tabChanged(
int)));
154 connect(ui_->database_host, SIGNAL(textChanged(QString)),
this, SIGNAL(configChanged()));
155 connect(ui_->database_port, SIGNAL(valueChanged(
int)),
this, SIGNAL(configChanged()));
157 connect(joints_tab_, SIGNAL(configChanged()),
this, SIGNAL(configChanged()));
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()));
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()));
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()));
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()));
181 ui_->reset_db_button->hide();
182 ui_->background_job_progress->hide();
183 ui_->background_job_progress->setMaximum(0);
185 ui_->tabWidget->setCurrentIndex(1);
187 known_collision_objects_version_ = 0;
189 initFromMoveGroupNS();
191 object_recognition_client_ =
192 std::make_unique<actionlib::SimpleActionClient<object_recognition_msgs::ObjectRecognitionAction>>(
195 if (object_recognition_client_)
201 catch (std::exception& ex)
204 object_recognition_client_.reset();
213 semantic_world_ = std::make_shared<moveit::semantic_world::SemanticWorld>(ps);
216 semantic_world_.reset();
219 semantic_world_->addTableCallback([
this] { updateTables(); });
222 catch (std::exception& ex)
276 const QSignalBlocker planning_group_blocker(
ui_->planning_group_combo_box);
277 ui_->planning_group_combo_box->clear();
280 for (
const std::string& group_name : kmodel->getJointModelGroupNames())
281 ui_->planning_group_combo_box->addItem(QString::fromStdString(group_name));
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();
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>"));
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>"));
314 if (!known_states.empty())
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)
320 ui_->start_state_combo_box->addItem(QString::fromStdString(known_state));
321 ui_->goal_state_combo_box->addItem(QString::fromStdString(known_state));
325 ui_->start_state_combo_box->setCurrentIndex(2);
326 ui_->goal_state_combo_box->setCurrentIndex(2);
341 planning_display_->addMainLoopJob(
342 [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); });
344 if (!
group.empty() && robot_model)
346 if (move_group_ && move_group_->getName() ==
group)
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());
359 std::shared_ptr<tf2_ros::Buffer>
tf_buffer = context_->getFrameManager()->getTF2BufferPtr();
362 std::make_shared<moveit::planning_interface::MoveGroupInterface>(opt, tf_buffer,
ros::WallDuration(30, 0));
364 if (planning_scene_storage_)
365 move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value());
367 catch (std::exception& ex)
371 planning_display_->addMainLoopJob([&view = *ui_->planner_param_treeview,
this] { view.setMoveGroup(move_group_); });
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");
389 planning_display_->setQueryStartState(ps->getCurrentState());
390 planning_display_->setQueryGoalState(ps->getCurrentState());
393 planning_display_->useApproximateIK(ui_->approximate_ik->isChecked());
394 if (ui_->allow_external_program->isChecked())
395 planning_display_->addMainLoopJob([
this] { allowExternalProgramCommunication(
true); });
401 void MotionPlanningFrame::clearRobotModel()
403 ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr());
404 joints_tab_->clearRobotModel();
408 void MotionPlanningFrame::changePlanningGroup()
410 planning_display_->addBackgroundJob([
this] { changePlanningGroupHelper(); },
"Frame::changePlanningGroup");
411 joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(),
412 planning_display_->getQueryStartStateHandler(),
413 planning_display_->getQueryGoalStateHandler());
419 planning_display_->addMainLoopJob([
this] { populateCollisionObjectsList(); });
422 void MotionPlanningFrame::addSceneObject()
424 static const double MIN_VAL = 1e-6;
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)
432 QMessageBox::warning(
this, QString(
"Dimension is too small"), QString(
"Size values need to be >= %1").arg(MIN_VAL));
437 std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString();
439 switch (ui_->shapes_combo_box->currentData().toInt())
442 shape = std::make_shared<shapes::Box>(x_length, y_length, z_length);
445 shape = std::make_shared<shapes::Sphere>(0.5 * x_length);
448 shape = std::make_shared<shapes::Cone>(0.5 * x_length, z_length);
451 shape = std::make_shared<shapes::Cylinder>(0.5 * x_length, z_length);
454 shape = std::make_shared<shapes::Plane>(0., 0., 1., 0.);
459 if (ui_->shapes_combo_box->currentText().contains(
"file"))
460 url = QFileDialog::getOpenFileUrl(
this, tr(
"Import Object Mesh"), QString(),
461 "CAD files (*.stl *.obj *.dae);;All files (*.*)");
463 url = QInputDialog::getText(
this, tr(
"Import Object Mesh"), tr(
"URL for file to import from:"),
464 QLineEdit::Normal, QString(
"http://"));
466 shape = loadMeshResource(url.toString().toStdString());
470 selected_shape = url.fileName().toStdString();
474 QMessageBox::warning(
this, QString(
"Unsupported shape"),
475 QString(
"The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText()));
478 std::string shape_name;
479 if (
auto ps = planning_display_->getPlanningSceneRW())
484 shape_name = selected_shape +
"_" + std::to_string(++idx);
485 while (ps->getWorld()->hasObject(shape_name));
488 ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity());
490 setLocalSceneEdited();
491 planning_display_->queueRenderSceneGeometry();
494 auto item = addCollisionObjectToList(shape_name, ui_->collision_objects_list->count(),
false);
497 ui_->collision_objects_list->clearSelection();
498 item->setSelected(
true);
499 ui_->collision_objects_list->setCurrentItem(item);
502 shapes::ShapePtr MotionPlanningFrame::loadMeshResource(
const std::string& url)
508 bool object_is_very_large =
false;
515 object_is_very_large =
true;
519 if (object_is_very_large)
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)
531 unsigned int i3 = i * 3;
543 QMessageBox::warning(
this, QString(
"Import error"), QString(
"Unable to import object"));
548 void MotionPlanningFrame::enable()
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(
"");
556 if (nh_.getNamespace() != new_ns)
558 ROS_INFO(
"MoveGroup namespace changed: %s -> %s. Reloading params.", nh_.getNamespace().c_str(), new_ns.c_str());
559 initFromMoveGroupNS();
564 parentWidget()->show();
569 void MotionPlanningFrame::initFromMoveGroupNS()
576 object_recognition_subscriber_ =
577 nh_.subscribe(
"recognized_object_array", 1, &MotionPlanningFrame::listenDetectedObjects,
this);
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);
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);
591 std::string param_name;
592 std::string host_param;
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);
600 nh_mg.param<std::string>(
"default_planning_pipeline", default_planning_pipeline_,
"");
603 void MotionPlanningFrame::disable()
606 scene_marker_.reset();
608 parentWidget()->hide();
611 void MotionPlanningFrame::tabChanged(
int index)
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();
619 void MotionPlanningFrame::updateSceneMarkers(
float wall_dt,
float )
622 scene_marker_->update(wall_dt);
625 void MotionPlanningFrame::updateExternalCommunication()
627 if (ui_->allow_external_program->isChecked())
629 planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(
true);