moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
start_screen_widget.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: Dave Coleman */
36
37// Qt
38#include <QApplication>
39#include <QFileDialog>
40#include <QFont>
41#include <QHBoxLayout>
42#include <QLabel>
43#include <QLineEdit>
44#include <QMessageBox>
45#include <QProgressBar>
46#include <QPushButton>
47#include <QString>
48#include <QTextEdit>
49#include <QTimer>
50#include <QVBoxLayout>
51
52// SA
54// C
55#include <fstream> // for reading in urdf
56#include <streambuf>
57
58namespace moveit_setup
59{
60namespace core
61{
62// ******************************************************************************************
63// Start screen user interface for MoveIt Configuration Assistant
64// ******************************************************************************************
66{
67 // Basic widget container
68 QVBoxLayout* layout = new QVBoxLayout(this);
69
70 // Horizontal layout splitter
71 QHBoxLayout* hlayout = new QHBoxLayout();
72 // Left side of screen
73 QVBoxLayout* left_layout = new QVBoxLayout();
74 // Right side of screen
75 QVBoxLayout* right_layout = new QVBoxLayout();
76
77 // Right Image Area ----------------------------------------------
78 right_image_ = new QImage();
79 right_image_label_ = new QLabel(this);
80 auto image_path = getSharePath("moveit_setup_assistant") / "resources/MoveIt_Setup_Assistant2.png";
81
82 if (right_image_->load(image_path.c_str()))
83 {
84 right_image_label_->setPixmap(QPixmap::fromImage(*right_image_));
85 right_image_label_->setMinimumHeight(384); // size of right_image_label_
86 }
87 else
88 {
89 RCLCPP_ERROR_STREAM(setup_step_.getLogger(), "FAILED TO LOAD " << image_path);
90 }
91
92 right_layout->addWidget(right_image_label_);
93 right_layout->setAlignment(right_image_label_, Qt::AlignRight | Qt::AlignTop);
94
95 // Top Label Area ---------------------------------------------------
96 HeaderWidget* header = new HeaderWidget(
97 "MoveIt Setup Assistant",
98 "These tools will assist you in creating a Semantic Robot Description Format (SRDF) file, various yaml "
99 "configuration and many roslaunch files for utilizing all aspects of MoveIt functionality.",
100 this);
101 layout->addWidget(header);
102
103 // Select Mode Area -------------------------------------------------
104 select_mode_ = new SelectModeWidget(this);
105 connect(select_mode_->btn_new_, SIGNAL(clicked()), this, SLOT(showNewOptions()));
106 connect(select_mode_->btn_exist_, SIGNAL(clicked()), this, SLOT(showExistingOptions()));
107 left_layout->addWidget(select_mode_);
108
109 // Path Box Area ----------------------------------------------------
110
111 // Stack Path Dialog
112 stack_path_ = new LoadPathArgsWidget("Load MoveIt Configuration Package",
113 "Specify the package name or path of an existing MoveIt configuration package "
114 "to be edited for your robot. Example package name: <i>panda_moveit_config</i>",
115 "optional xacro arguments:", this, true); // directory
116 // user needs to select option before this is shown
117 stack_path_->hide();
118 stack_path_->setArgs("");
119 connect(stack_path_, SIGNAL(pathChanged(QString)), this, SLOT(onPackagePathChanged(QString)));
120 left_layout->addWidget(stack_path_);
121
122 // URDF File Dialog
124 "Load a URDF or COLLADA Robot Model",
125 "Specify the location of an existing Universal Robot Description Format or COLLADA file for your robot",
126 "optional xacro arguments:", this, false, true); // no directory, load only
127 // user needs to select option before this is shown
128 urdf_file_->hide();
129 urdf_file_->setArgs("");
130 connect(urdf_file_, SIGNAL(pathChanged(QString)), this, SLOT(onUrdfPathChanged(QString)));
131 left_layout->addWidget(urdf_file_);
132
133 // Load settings box ---------------------------------------------
134 QHBoxLayout* load_files_layout = new QHBoxLayout();
135
136 progress_bar_ = new QProgressBar(this);
137 progress_bar_->setMaximum(100);
138 progress_bar_->setMinimum(0);
139 progress_bar_->hide();
140 load_files_layout->addWidget(progress_bar_);
141
142 btn_load_ = new QPushButton("&Load Files", this);
143 btn_load_->setMinimumWidth(180);
144 btn_load_->setMinimumHeight(40);
145 btn_load_->hide();
146 load_files_layout->addWidget(btn_load_);
147 load_files_layout->setAlignment(btn_load_, Qt::AlignRight);
148 connect(btn_load_, SIGNAL(clicked()), this, SLOT(loadFilesClick()));
149
150 // Next step instructions
151 next_label_ = new QLabel(this);
152 QFont next_label_font(QFont().defaultFamily(), 11, QFont::Bold);
153 next_label_->setFont(next_label_font);
154 next_label_->setText("Success! Use the left navigation pane to continue.");
155 next_label_->hide(); // only show once the files have been loaded.
156
157 // Final Layout Setup ---------------------------------------------
158 // Alignment
159 layout->setAlignment(Qt::AlignTop);
160 hlayout->setAlignment(Qt::AlignTop);
161 left_layout->setAlignment(Qt::AlignTop);
162 right_layout->setAlignment(Qt::AlignTop);
163
164 // Stretch
165 left_layout->setSpacing(10);
166
167 // Attach Layouts
168 hlayout->addLayout(left_layout);
169 hlayout->addLayout(right_layout);
170 layout->addLayout(hlayout);
171
172 // Vertical Spacer
173 layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Minimum, QSizePolicy::Expanding));
174
175 // Attach bottom layout
176 layout->addWidget(next_label_);
177 layout->setAlignment(next_label_, Qt::AlignRight);
178 layout->addLayout(load_files_layout);
179
180 setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
181 setLayout(layout);
182
183 // Debug mode: auto load the configuration file by clicking button after a timeout
184 if (debug_)
185 {
186 // select_mode_->btn_exist_->click();
187
188 QTimer* update_timer = new QTimer(this);
189 update_timer->setSingleShot(true); // only run once
190 connect(update_timer, SIGNAL(timeout()), btn_load_, SLOT(click()));
191 update_timer->start(100);
192 }
193}
194
195// ******************************************************************************************
196// Destructor
197// ******************************************************************************************
199{
200 delete right_image_; // does not have a parent passed to it
201}
202
204{
205 std::filesystem::path pkg_path = setup_step_.getPackagePath();
206 if (!pkg_path.empty())
207 {
208 stack_path_->setPath(pkg_path);
209 select_mode_->btn_exist_->click();
210 return;
211 }
212
213 std::filesystem::path urdf_path = setup_step_.getURDFPath();
214 if (!urdf_path.empty())
215 {
216 urdf_file_->setPath(urdf_path);
217 select_mode_->btn_new_->click();
218 }
219}
220
221// ******************************************************************************************
222// Show options for creating a new configuration package
223// ******************************************************************************************
224void StartScreenWidget::showNewOptions()
225{
226 // Do GUI stuff
227 select_mode_->btn_exist_->setChecked(false);
228 select_mode_->btn_new_->setChecked(true);
230 urdf_file_->show();
231 stack_path_->hide();
232 btn_load_->show();
233
234 // Remember choice
235 create_new_package_ = true;
236}
237
238// ******************************************************************************************
239// Show options for editing an existing configuration package
240// ******************************************************************************************
241void StartScreenWidget::showExistingOptions()
242{
243 // Do GUI stuff
244 select_mode_->btn_exist_->setChecked(true);
245 select_mode_->btn_new_->setChecked(false);
247 urdf_file_->hide();
248 stack_path_->show();
249 btn_load_->show();
250
251 // Remember choice
252 create_new_package_ = false;
253}
254
255// ******************************************************************************************
256// Load files to parameter server - CLICK
257// ******************************************************************************************
258void StartScreenWidget::loadFilesClick()
259{
260 // Disable start screen GUI components from being changed
261 urdf_file_->setDisabled(true);
262 // srdf_file_->setDisabled(true);
263 stack_path_->setDisabled(true);
264 select_mode_->setDisabled(true);
265 btn_load_->setDisabled(true);
266 progress_bar_->show();
267
268 bool result;
269
270 // Decide if this is a new config package, or loading an old one
271 if (create_new_package_)
272 {
273 result = loadNewFiles();
274 }
275 else
276 {
277 result = loadExistingFiles();
278 }
279
280 // Check if there was a failure loading files
281 if (!result)
282 {
283 // Re-enable components
284 urdf_file_->setDisabled(false);
285 // srdf_file_->setDisabled(false);
286 stack_path_->setDisabled(false);
287 select_mode_->setDisabled(false);
288 btn_load_->setDisabled(false);
289 progress_bar_->hide();
290 }
291 else
292 {
293 // Hide the logo image so that other screens can resize the rviz thing properly
294 right_image_label_->hide();
295 }
296}
297
298void StartScreenWidget::onPackagePathChanged(const QString& /*path*/)
299{
300 if (!loadPackageSettings(false))
301 return;
302 // set xacro args from loaded settings
303 stack_path_->setArgs(QString::fromStdString(setup_step_.getXacroArgs()));
304}
305
306void StartScreenWidget::onUrdfPathChanged(const QString& path)
307{
308 try
309 {
310 setup_step_.loadURDFFile(path.toStdString(), urdf_file_->getArgs().toStdString());
311 urdf_file_->setArgsEnabled(setup_step_.isXacroFile());
312 }
313 catch (const std::runtime_error& e)
314 {
315 RCLCPP_ERROR(setup_step_.getLogger(), "Error Loading URDF Path. %s", e.what());
316 QMessageBox::warning(this, "Error Loading URDF Path", QString(e.what()));
317 }
318}
319
320bool StartScreenWidget::loadPackageSettings(bool show_warnings)
321{
322 // Get the package path
323 std::filesystem::path package_path_input = stack_path_->getPath();
324
325 try
326 {
327 setup_step_.loadExisting(package_path_input);
328 return true;
329 }
330 catch (const std::runtime_error& e)
331 {
332 if (show_warnings)
333 QMessageBox::warning(this, "Error Loading Files", e.what());
334 return false;
335 }
336}
337
338// ******************************************************************************************
339// Load existing package files
340// ******************************************************************************************
341bool StartScreenWidget::loadExistingFiles()
342{
343 try
344 {
345 // Progress Indicator
346 progress_bar_->setValue(10);
347 QApplication::processEvents();
348
349 if (!loadPackageSettings(true))
350 return false;
351 }
352 catch (const std::runtime_error& e)
353 {
354 RCLCPP_ERROR(setup_step_.getLogger(), "Error Loading SRDF. %s", e.what());
355 QMessageBox::warning(this, "Error Loading SRDF", QString(e.what()));
356 return false;
357 }
358
359 // Progress Indicator
360 progress_bar_->setValue(100);
361 QApplication::processEvents();
362
363 next_label_->show(); // only show once the files have been loaded
364 Q_EMIT dataUpdated();
365
366 RCLCPP_INFO(setup_step_.getLogger(), "Loading Setup Assistant Complete");
367
368 Q_EMIT advanceRequest();
369 return true; // success!
370}
371
372// ******************************************************************************************
373// Load chosen files for creating new package
374// ******************************************************************************************
375bool StartScreenWidget::loadNewFiles()
376{
377 // Get URDF file path
378 std::filesystem::path urdf_path = urdf_file_->getPath();
379
380 // Check that box is filled out
381 if (urdf_path.empty())
382 {
383 QMessageBox::warning(this, "Error Loading Files", "No robot model file specified");
384 return false;
385 }
386
387 // Check that this file exits
388 if (!std::filesystem::is_regular_file(urdf_path))
389 {
390 QMessageBox::warning(this, "Error Loading Files",
391 QString("Unable to locate the URDF file: ").append(urdf_path.c_str()));
392 return false;
393 }
394
395 // Progress Indicator
396 progress_bar_->setValue(20);
397 QApplication::processEvents();
398
399 // use xacro args from GUI
400 std::string xacro_args = urdf_file_->getArgs().toStdString();
401 try
402 {
403 setup_step_.loadURDFFile(urdf_path, xacro_args);
404 }
405 catch (const std::runtime_error& e)
406 {
407 QMessageBox::warning(this, "Error Loading URDF", QString(e.what()));
408 return false;
409 }
410
411 // Progress Indicator
412 progress_bar_->setValue(50);
413 QApplication::processEvents();
414
415 // DONE LOADING --------------------------------------------------------------------------
416
417 // Call a function that enables navigation
418 Q_EMIT dataUpdated();
419
420 // Progress Indicator
421 progress_bar_->setValue(70);
422 QApplication::processEvents();
423
424 // Progress Indicator
425 progress_bar_->setValue(100);
426 QApplication::processEvents();
427
428 next_label_->show(); // only show once the files have been loaded
429
430 RCLCPP_INFO(setup_step_.getLogger(), "Loading Setup Assistant Complete");
431 return true; // success!
432}
433
434// ******************************************************************************************
435// ******************************************************************************************
436// Class for selecting which mode
437// ******************************************************************************************
438// ******************************************************************************************
439
440// ******************************************************************************************
441// Create the widget
442// ******************************************************************************************
443SelectModeWidget::SelectModeWidget(QWidget* parent) : QFrame(parent)
444{
445 // Set frame graphics
446 setFrameShape(QFrame::StyledPanel);
447 setFrameShadow(QFrame::Raised);
448 setLineWidth(1);
449 setMidLineWidth(0);
450
451 // Basic widget container
452 QVBoxLayout* layout = new QVBoxLayout(this);
453
454 // Horizontal layout splitter
455 QHBoxLayout* hlayout = new QHBoxLayout();
456
457 // Widget Title
458 QLabel* widget_title = new QLabel(this);
459 widget_title->setText("Create new or edit existing?");
460 QFont widget_title_font(QFont().defaultFamily(), 12, QFont::Bold);
461 widget_title->setFont(widget_title_font);
462 layout->addWidget(widget_title);
463 layout->setAlignment(widget_title, Qt::AlignTop);
464
465 // Widget Instructions
466 widget_instructions_ = new QLabel(this);
467 widget_instructions_->setAlignment(Qt::AlignLeft | Qt::AlignTop);
468 widget_instructions_->setWordWrap(true);
469 widget_instructions_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
470 widget_instructions_->setText(
471 "All settings for MoveIt are stored in the MoveIt configuration package. Here you have the option to create a "
472 "new configuration package or load an existing one. Note: changes to a MoveIt configuration package outside this "
473 "Setup Assistant are likely to be overwritten by this tool.");
474
475 layout->addWidget(widget_instructions_);
476 layout->setAlignment(widget_instructions_, Qt::AlignTop);
477
478 // New Button
479 btn_new_ = new QPushButton(this);
480 btn_new_->setText("Create &New MoveIt\nConfiguration Package");
481 hlayout->addWidget(btn_new_);
482
483 // Exist Button
484 btn_exist_ = new QPushButton(this);
485 btn_exist_->setText("&Edit Existing MoveIt\nConfiguration Package");
486 btn_exist_->setCheckable(true);
487 hlayout->addWidget(btn_exist_);
488
489 // Add horizontal layer to vertical layer
490 layout->addLayout(hlayout);
491 setLayout(layout);
492 btn_new_->setCheckable(true);
493}
494
495} // namespace core
496} // namespace moveit_setup
497
498#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
Extend LoadPathWidget with additional line edit for arguments.
void setArgsEnabled(bool enabled=true)
void setArgs(const QString &args)
void setPath(const QString &path)
Set the path with QString.
std::string getPath() const
Returns the file path in std::string format.
The GUI code for one SetupStep.
void dataUpdated()
When the underlying data has been updated (which can cause other steps to become "Ready")
void advanceRequest()
When this signal is received, the GUI should attempt to advance to the next step.
const rclcpp::Logger & getLogger() const
Makes a namespaced logger for this step available to the widget.
Start screen user interface for MoveIt Configuration Assistant.
void focusGiven() override
function called when widget is activated, allows to update/initialize GUI
std::filesystem::path getURDFPath()
void loadURDFFile(const std::filesystem::path &urdf_file_path, const std::string &xacro_args)
std::filesystem::path getPackagePath()
void loadExisting(const std::filesystem::path &package_path)
std::filesystem::path getSharePath(const std::string &package_name)
Return a path for the given package's share folder.
Definition utilities.hpp:51
std::string append(const std::string &left, const std::string &right)