46 #include <geometric_shapes/shape_operations.h>
48 #include <rviz_common/display_context.hpp>
49 #include <rviz_common/frame_manager_iface.hpp>
50 #include <tf2_ros/buffer.h>
52 #include <std_srvs/srv/empty.hpp>
54 #include <QMessageBox>
55 #include <QInputDialog>
56 #include <QFileDialog>
60 #include "ui_motion_planning_rviz_plugin_frame.h"
68 , planning_display_(pdisplay)
70 , ui_(new
Ui::MotionPlanningUI())
74 auto ros_node_abstraction =
context_->getRosNodeAbstraction().lock();
75 if (!ros_node_abstraction)
77 RCLCPP_INFO(logger_,
"Unable to lock weak_ptr from DisplayContext in MotionPlanningFrame constructor");
80 node_ = ros_node_abstraction->get_raw_node();
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);
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);
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()));
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)));
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()));
174 connect(
ui_->tabWidget, SIGNAL(currentChanged(
int)),
this, SLOT(tabChanged(
int)));
177 connect(
ui_->database_host, SIGNAL(textChanged(QString)),
this, SIGNAL(
configChanged()));
178 connect(
ui_->database_port, SIGNAL(valueChanged(
int)),
this, SIGNAL(
configChanged()));
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()));
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()));
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()));
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()));
202 ui_->reset_db_button->hide();
203 ui_->background_job_progress->hide();
204 ui_->background_job_progress->setMaximum(0);
206 ui_->tabWidget->setCurrentIndex(1);
208 known_collision_objects_version_ = 0;
212 object_recognition_client_ = rclcpp_action::create_client<object_recognition_msgs::action::ObjectRecognition>(
215 if (object_recognition_client_)
217 if (!object_recognition_client_->wait_for_action_server(std::chrono::seconds(3)))
220 object_recognition_client_.reset();
250 void MotionPlanningFrame::approximateIKChanged(
int state)
255 void MotionPlanningFrame::setItemSelectionInList(
const std::string& item_name,
bool selection, QListWidget* list)
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);
262 void MotionPlanningFrame::allowExternalProgramCommunication(
bool enable)
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);
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);
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();
312 void MotionPlanningFrame::fillPlanningGroupOptions()
314 const QSignalBlocker planning_group_blocker(
ui_->planning_group_combo_box);
315 ui_->planning_group_combo_box->clear();
318 for (
const std::string& group_name : kmodel->getJointModelGroupNames())
319 ui_->planning_group_combo_box->addItem(QString::fromStdString(group_name));
322 void MotionPlanningFrame::fillStateSelectionOptions()
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();
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>"));
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>"));
352 if (!known_states.empty())
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)
358 ui_->start_state_combo_box->addItem(QString::fromStdString(known_state));
359 ui_->goal_state_combo_box->addItem(QString::fromStdString(known_state));
363 ui_->start_state_combo_box->setCurrentIndex(2);
364 ui_->goal_state_combo_box->setCurrentIndex(2);
368 void MotionPlanningFrame::changePlanningGroupHelper()
379 planning_display_->addMainLoopJob(
380 [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); });
382 if (!group.empty() && robot_model)
384 RCLCPP_INFO(logger_,
"group %s", group.c_str());
385 if (move_group_ && move_group_->getName() == group)
387 RCLCPP_INFO(logger_,
"Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(),
388 planning_display_->getMoveGroupNS().c_str());
391 opt.robot_model = robot_model;
392 opt.robot_description.clear();
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());
407 catch (std::exception& ex)
409 RCLCPP_ERROR(logger_,
"%s", ex.what());
411 planning_display_->addMainLoopJob([&
view = *ui_->planner_param_treeview,
this] { view.setMoveGroup(move_group_); });
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");
429 planning_display_->setQueryStartState(ps->getCurrentState());
430 planning_display_->setQueryGoalState(ps->getCurrentState());
433 planning_display_->useApproximateIK(ui_->approximate_ik->isChecked());
434 if (ui_->allow_external_program->isChecked())
435 planning_display_->addMainLoopJob([
this] { allowExternalProgramCommunication(
true); });
441 void MotionPlanningFrame::clearRobotModel()
443 ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr());
444 joints_tab_->clearRobotModel();
448 void MotionPlanningFrame::changePlanningGroup()
450 planning_display_->addBackgroundJob([
this] { changePlanningGroupHelper(); },
"Frame::changePlanningGroup");
451 joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(),
452 planning_display_->getQueryStartStateHandler(),
453 planning_display_->getQueryGoalStateHandler());
459 planning_display_->addMainLoopJob([
this] { populateCollisionObjectsList(); });
462 void MotionPlanningFrame::addSceneObject()
464 static const double MIN_VAL = 1e-6;
466 if (!planning_display_->getPlanningSceneMonitor())
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)
477 QMessageBox::warning(
this, QString(
"Dimension is too small"), QString(
"Size values need to be >= %1").arg(MIN_VAL));
482 std::string selected_shape = ui_->shapes_combo_box->currentText().toStdString();
483 shapes::ShapeConstPtr shape;
484 switch (ui_->shapes_combo_box->currentData().toInt())
487 shape = std::make_shared<shapes::Box>(x_length, y_length, z_length);
490 shape = std::make_shared<shapes::Sphere>(0.5 * x_length);
493 shape = std::make_shared<shapes::Cone>(0.5 * x_length, z_length);
495 case shapes::CYLINDER:
496 shape = std::make_shared<shapes::Cylinder>(0.5 * x_length, z_length);
501 if (ui_->shapes_combo_box->currentText().contains(
"file"))
503 url = QFileDialog::getOpenFileUrl(
this, tr(
"Import Object Mesh"), QString(),
504 "CAD files (*.stl *.obj *.dae);;All files (*.*)");
508 url = QInputDialog::getText(
this, tr(
"Import Object Mesh"), tr(
"URL for file to import from:"),
509 QLineEdit::Normal, QString(
"http://"));
512 shape = loadMeshResource(url.toString().toStdString());
516 selected_shape = url.fileName().toStdString();
520 QMessageBox::warning(
this, QString(
"Unsupported shape"),
521 QString(
"The '%1' is not supported.").arg(ui_->shapes_combo_box->currentText()));
526 std::string shape_name = selected_shape +
"_" + std::to_string(idx);
527 while (planning_display_->getPlanningSceneRO()->getWorld()->hasObject(shape_name))
530 shape_name = selected_shape +
"_" + std::to_string(idx);
536 ps->getWorldNonConst()->addToObject(shape_name, shape, Eigen::Isometry3d::Identity());
538 setLocalSceneEdited();
540 planning_display_->addMainLoopJob([
this] { populateCollisionObjectsList(); });
543 planning_display_->addMainLoopJob([
this, shape_name, list_widget = ui_->collision_objects_list] {
544 setItemSelectionInList(shape_name,
true, list_widget);
547 planning_display_->queueRenderSceneGeometry();
550 shapes::ShapePtr MotionPlanningFrame::loadMeshResource(
const std::string& url)
552 shapes::Mesh* mesh = shapes::createMeshFromResource(url);
556 bool object_is_very_large =
false;
557 for (
unsigned int i = 0; i < mesh->vertex_count; ++i)
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))
563 object_is_very_large =
true;
567 if (object_is_very_large)
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)
577 for (
unsigned int i = 0; i < mesh->vertex_count; ++i)
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;
587 return shapes::ShapePtr(mesh);
591 QMessageBox::warning(
this, QString(
"Import error"), QString(
"Unable to import object"));
592 return shapes::ShapePtr();
596 void MotionPlanningFrame::enable()
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(
"");
603 const std::string new_ns = planning_display_->getMoveGroupNS();
604 if (node_->get_namespace() != new_ns)
606 RCLCPP_INFO(logger_,
"MoveGroup namespace changed: %s -> %s. Reloading params.", node_->get_namespace(),
608 initFromMoveGroupNS();
613 parentWidget()->show();
618 void MotionPlanningFrame::initFromMoveGroupNS()
621 clear_octomap_service_client_ = node_->create_client<std_srvs::srv::Empty>(move_group::CLEAR_OCTOMAP_SERVICE_NAME);
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);
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);
634 if (!node_->has_parameter(planning_display_->getMoveGroupNS() +
"default_planning_pipeline"))
635 node_->declare_parameter<std::string>(planning_display_->getMoveGroupNS() +
"default_planning_pipeline",
"");
638 node_->get_parameter(planning_display_->getMoveGroupNS() +
"default_planning_pipeline", default_planning_pipeline_);
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);
648 std::string host_param;
649 if (node_->get_parameter(
"warehouse_host", host_param))
651 ui_->database_host->setText(QString::fromStdString(host_param));
655 if (node_->get_parameter(
"warehouse_port", port))
657 ui_->database_port->setValue(port);
661 void MotionPlanningFrame::disable()
664 scene_marker_.reset();
666 parentWidget()->hide();
669 void MotionPlanningFrame::tabChanged(
int index)
671 if (scene_marker_ && ui_->tabWidget->tabText(index).toStdString() != TAB_OBJECTS)
673 scene_marker_.reset();
675 else if (ui_->tabWidget->tabText(index).toStdString() == TAB_OBJECTS)
677 selectedCollisionObjectChanged();
681 void MotionPlanningFrame::updateSceneMarkers(
double ,
double )
684 scene_marker_->update();
687 void MotionPlanningFrame::updateExternalCommunication()
689 if (ui_->allow_external_program->isChecked())
691 planning_display_->getRobotInteraction()->toggleMoveInteractiveMarkerTopic(
true);
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'.
std::string getCurrentPlanningGroup() const
void useApproximateIK(bool flag)
void toggleSelectPlanningGroupSubscription(bool enable)
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
void initFromMoveGroupNS()
rviz_common::DisplayContext * context_
Ui::MotionPlanningUI * ui_
~MotionPlanningFrame() override
MotionPlanningFrameJointsWidget * joints_tab_
MotionPlanningDisplay * planning_display_
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,...
rclcpp::Logger getLogger()
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
const std::string OBJECT_RECOGNITION_ACTION
Main namespace for MoveIt.
Specification of options to use when constructing the MoveGroupInterface class.