setup_assistant_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 // SA
38 #include "setup_screen_widget.h" // a base class for screens in the setup assistant
39 #include "setup_assistant_widget.h"
40 // Qt
41 #include <QStackedLayout>
42 #include <QListWidget>
43 #include <QListWidgetItem>
44 #include <QDebug>
45 #include <QFont>
46 #include <QLabel>
47 #include <QPushButton>
48 #include <QCloseEvent>
49 #include <QMessageBox>
50 #include <QString>
51 #include <pluginlib/class_loader.hpp> // for loading all avail kinematic planners
52 // Rviz
53 #include <rviz/render_panel.h>
55 #include <rviz/view_manager.h>
58 
59 namespace moveit_setup_assistant
60 {
61 // ******************************************************************************************
62 // Outer User Interface for MoveIt! Configuration Assistant
63 // ******************************************************************************************
64 SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, boost::program_options::variables_map args)
65  : QWidget(parent)
66 {
69 
70  // Create object to hold all moveit configuration data
71  config_data_.reset(new MoveItConfigData());
72 
73  // Set debug mode flag if necessary
74  if (args.count("debug"))
75  config_data_->debug_ = true;
76 
77  // Setting the window icon
78  std::string moveit_ros_visualization_package_path = ros::package::getPath("moveit_ros_visualization");
79  moveit_ros_visualization_package_path += "/icons/classes/MotionPlanning.png";
80  this->setWindowIcon(QIcon(moveit_ros_visualization_package_path.c_str()));
81 
82  // Basic widget container -----------------------------------------
83  QHBoxLayout* layout = new QHBoxLayout();
84  layout->setAlignment(Qt::AlignTop);
85 
86  // Create main content stack for various screens
87  main_content_ = new QStackedLayout();
88  current_index_ = 0;
89 
90  // Wrap main_content_ with a widget
91  middle_frame_ = new QWidget(this);
92  middle_frame_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
93  middle_frame_->setLayout(main_content_);
94 
95  // Screens --------------------------------------------------------
96 
97  // Start Screen
99  start_screen_widget_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
100  connect(start_screen_widget_, SIGNAL(readyToProgress()), this, SLOT(progressPastStartScreen()));
101  connect(start_screen_widget_, SIGNAL(loadRviz()), this, SLOT(loadRviz()));
103 
104  // Pass command arg values to start screen and show appropriate part of screen
105  if (args.count("urdf_path"))
106  {
107  start_screen_widget_->urdf_file_->setPath(args["urdf_path"].as<std::string>());
109  }
110  if (args.count("config_pkg"))
111  {
112  start_screen_widget_->stack_path_->setPath(args["config_pkg"].as<std::string>());
114  }
115  else
116  {
117  // Open the directory where the MSA was started from.
118  // cf. http://stackoverflow.com/a/7413516/577001
119  QString pwdir("");
120  char* pwd;
121  pwd = getenv("PWD");
122  pwdir.append(pwd);
124  }
125 
126  // Add Navigation Buttons (but do not load widgets yet except start screen)
127  nav_name_list_ << "Start";
128  nav_name_list_ << "Self-Collisions";
129  nav_name_list_ << "Virtual Joints";
130  nav_name_list_ << "Planning Groups";
131  nav_name_list_ << "Robot Poses";
132  nav_name_list_ << "End Effectors";
133  nav_name_list_ << "Passive Joints";
134  nav_name_list_ << "ROS Control";
135  nav_name_list_ << "Simulation";
136  nav_name_list_ << "3D Perception";
137  nav_name_list_ << "Author Information";
138  nav_name_list_ << "Configuration Files";
139 
140  // Navigation Left Pane --------------------------------------------------
141  navs_view_ = new NavigationWidget(this);
143  navs_view_->setDisabled(true);
144  navs_view_->setSelected(0); // start screen
145 
146  // Rviz View Right Pane ---------------------------------------------------
147  rviz_container_ = new QWidget(this);
148  rviz_container_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
149  rviz_container_->hide(); // do not show until after the start screen
150 
151  // Split screen -----------------------------------------------------
152  splitter_ = new QSplitter(Qt::Horizontal, this);
153  splitter_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
154  splitter_->addWidget(navs_view_);
155  splitter_->addWidget(middle_frame_);
156  splitter_->addWidget(rviz_container_);
157  splitter_->setHandleWidth(6);
158  // splitter_->setCollapsible( 0, false ); // don't let navigation collapse
159  layout->addWidget(splitter_);
160 
161  // Add event for switching between screens -------------------------
162  connect(navs_view_, SIGNAL(clicked(const QModelIndex&)), this, SLOT(navigationClicked(const QModelIndex&)));
163 
164  // Final Layout Setup ---------------------------------------------
165  this->setLayout(layout);
166 
167  // Title
168  this->setWindowTitle("MoveIt! Setup Assistant"); // title of window
169 
170  // Show screen before message
171  QApplication::processEvents();
172 }
173 
174 // ******************************************************************************************
175 // Decontructor
176 // ******************************************************************************************
178 {
179  if (rviz_manager_ != NULL)
181  if (rviz_render_panel_ != NULL)
182  delete rviz_render_panel_;
183  if (rviz_manager_ != NULL)
184  delete rviz_manager_;
185 }
186 
188 {
190  {
191  rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame()));
193  }
194 }
195 
196 // ******************************************************************************************
197 // Change screens of Setup Assistant
198 // ******************************************************************************************
199 void SetupAssistantWidget::navigationClicked(const QModelIndex& index)
200 {
201  // Convert QModelIndex to int
202  moveToScreen(index.row());
203 }
204 
205 // ******************************************************************************************
206 // Change screens
207 // ******************************************************************************************
209 {
210  boost::mutex::scoped_lock slock(change_screen_lock_);
211 
212  if (current_index_ != index)
213  {
214  // Send the focus lost command to the screen widget
215  SetupScreenWidget* ssw = qobject_cast<SetupScreenWidget*>(main_content_->widget(current_index_));
216  if (!ssw->focusLost())
217  {
219  return; // switching not accepted
220  }
221 
222  current_index_ = index;
223 
224  // Unhighlight anything on robot
225  unhighlightAll();
226 
227  // Change screens
228  main_content_->setCurrentIndex(index);
229 
230  // Send the focus given command to the screen widget
231  ssw = qobject_cast<SetupScreenWidget*>(main_content_->widget(index));
232  ssw->focusGiven();
233 
234  // Change navigation selected option
235  navs_view_->setSelected(index); // Select first item in list
236  }
237 }
238 
239 // ******************************************************************************************
240 // Loads other windows, enables navigation
241 // ******************************************************************************************
243 {
244  // Load all widgets ------------------------------------------------
245 
246  // Self-Collisions
249  connect(default_collisions_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
250  SLOT(highlightLink(const std::string&, const QColor&)));
251  connect(default_collisions_widget_, SIGNAL(highlightGroup(const std::string&)), this,
252  SLOT(highlightGroup(const std::string&)));
253  connect(default_collisions_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
254 
255  // Virtual Joints
258  connect(virtual_joints_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
259  connect(virtual_joints_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
260  SLOT(highlightLink(const std::string&, const QColor&)));
261  connect(virtual_joints_widget_, SIGNAL(highlightGroup(const std::string&)), this,
262  SLOT(highlightGroup(const std::string&)));
263  connect(virtual_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
264  connect(virtual_joints_widget_, SIGNAL(referenceFrameChanged()), this, SLOT(virtualJointReferenceFrameChanged()));
265 
266  // Planning Groups
269  connect(planning_groups_widget, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
270  connect(planning_groups_widget, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
271  SLOT(highlightLink(const std::string&, const QColor&)));
272  connect(planning_groups_widget, SIGNAL(highlightGroup(const std::string&)), this,
273  SLOT(highlightGroup(const std::string&)));
274  connect(planning_groups_widget, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
275 
276  // Robot Poses
279  connect(robot_poses_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
280  connect(robot_poses_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
281  SLOT(highlightLink(const std::string&, const QColor&)));
282  connect(robot_poses_widget_, SIGNAL(highlightGroup(const std::string&)), this,
283  SLOT(highlightGroup(const std::string&)));
284  connect(robot_poses_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
285 
286  // End Effectors
289  connect(end_effectors_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
290  connect(end_effectors_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
291  SLOT(highlightLink(const std::string&, const QColor&)));
292  connect(end_effectors_widget_, SIGNAL(highlightGroup(const std::string&)), this,
293  SLOT(highlightGroup(const std::string&)));
294  connect(end_effectors_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
295 
296  // Virtual Joints
299  connect(passive_joints_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
300  connect(passive_joints_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
301  SLOT(highlightLink(const std::string&, const QColor&)));
302  connect(passive_joints_widget_, SIGNAL(highlightGroup(const std::string&)), this,
303  SLOT(highlightGroup(const std::string&)));
304  connect(passive_joints_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
305 
306  // ROS Controllers
309  connect(controllers_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
310  connect(controllers_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
311  SLOT(highlightLink(const std::string&, const QColor&)));
312  connect(controllers_widget_, SIGNAL(highlightGroup(const std::string&)), this,
313  SLOT(highlightGroup(const std::string&)));
314  connect(controllers_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
315 
316  // Simulation Screen
318  main_content_->addWidget(simulation_widget_);
319  connect(simulation_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
320  connect(simulation_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
321  SLOT(highlightLink(const std::string&, const QColor&)));
322  connect(simulation_widget_, SIGNAL(highlightGroup(const std::string&)), this,
323  SLOT(highlightGroup(const std::string&)));
324  connect(simulation_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
325 
326  // Perception
328  main_content_->addWidget(perception_widget_);
329  connect(perception_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
330  connect(perception_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
331  SLOT(highlightLink(const std::string&, const QColor&)));
332  connect(perception_widget_, SIGNAL(highlightGroup(const std::string&)), this,
333  SLOT(highlightGroup(const std::string&)));
334  connect(perception_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
335 
336  // Author Information
339  connect(author_information_widget_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
340  connect(author_information_widget_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
341  SLOT(highlightLink(const std::string&, const QColor&)));
342  connect(author_information_widget_, SIGNAL(highlightGroup(const std::string&)), this,
343  SLOT(highlightGroup(const std::string&)));
344  connect(author_information_widget_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
345 
346  // Configuration Files
349 
350  // Enable all nav buttons -------------------------------------------
351  for (int i = 0; i < nav_name_list_.count(); ++i)
352  {
353  navs_view_->setEnabled(i, true);
354  }
355 
356  // Enable navigation
357  navs_view_->setDisabled(false);
358 
359  // Replace logo with Rviz screen
360  rviz_container_->show();
361 
362  // Move to next screen in debug mode
363  if (config_data_->debug_)
364  {
365  moveToScreen(3);
366  }
367 }
368 
369 // ******************************************************************************************
370 // Ping ROS on internval
371 // ******************************************************************************************
373 {
374  ros::spinOnce(); // keep ROS node alive
375 }
376 
377 // ******************************************************************************************
378 // Load Rviz once we have a robot description ready
379 // ******************************************************************************************
381 {
382  // Create rviz frame
384  rviz_render_panel_->setMinimumWidth(200);
385  rviz_render_panel_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
386 
391 
392  // Set the fixed and target frame
393  rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame()));
394 
395  // Create the MoveIt! Rviz Plugin and attach to display
397  robot_state_display_->setName("Robot State");
398 
400 
401  // Set the topic on which the moveit_msgs::PlanningScene messages are received
402  robot_state_display_->subProp("Robot State Topic")->setValue(QString::fromStdString(MOVEIT_ROBOT_STATE));
403 
404  // Set robot description
405  robot_state_display_->subProp("Robot Description")->setValue(QString::fromStdString(ROBOT_DESCRIPTION));
406 
407  // Zoom into robot
409  view->subProp("Distance")->setValue(4.0f);
410 
411  // Add Rviz to Planning Groups Widget
412  QHBoxLayout* rviz_layout = new QHBoxLayout();
413  rviz_layout->addWidget(rviz_render_panel_);
414  rviz_container_->setLayout(rviz_layout);
415 
416  rviz_container_->show();
417 }
418 
419 // ******************************************************************************************
420 // Highlight a robot link
421 // ******************************************************************************************
422 void SetupAssistantWidget::highlightLink(const std::string& link_name, const QColor& color)
423 {
424  const robot_model::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name);
425  if (!lm->getShapes().empty()) // skip links with no geometry
426  robot_state_display_->setLinkColor(link_name, color);
427 }
428 
429 // ******************************************************************************************
430 // Highlight a robot group
431 // ******************************************************************************************
432 void SetupAssistantWidget::highlightGroup(const std::string& group_name)
433 {
434  // Highlight the selected planning group by looping through the links
435  if (!config_data_->getRobotModel()->hasJointModelGroup(group_name))
436  return;
437 
438  const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name);
439  if (joint_model_group)
440  {
441  const std::vector<const robot_model::LinkModel*>& link_models = joint_model_group->getLinkModels();
442  // Iterate through the links
443  for (std::vector<const robot_model::LinkModel*>::const_iterator link_it = link_models.begin();
444  link_it < link_models.end(); ++link_it)
445  highlightLink((*link_it)->getName(), QColor(255, 0, 0));
446  }
447 }
448 
449 // ******************************************************************************************
450 // Unhighlight all robot links
451 // ******************************************************************************************
453 {
454  // Get the names of the all links robot
455  const std::vector<std::string>& links = config_data_->getRobotModel()->getLinkModelNamesWithCollisionGeometry();
456 
457  // Quit if no links found
458  if (links.empty())
459  {
460  return;
461  }
462 
463  // check if rviz is ready
465  {
466  return;
467  }
468 
469  // Iterate through the links
470  for (std::vector<std::string>::const_iterator link_it = links.begin(); link_it < links.end(); ++link_it)
471  {
472  if ((*link_it).empty())
473  continue;
474 
476  }
477 }
478 
479 // ******************************************************************************************
480 // Qt close event function for reminding user to save
481 // ******************************************************************************************
482 void SetupAssistantWidget::closeEvent(QCloseEvent* event)
483 {
484  // Only prompt to close if not in debug mode
485  if (!config_data_->debug_)
486  {
487  if (QMessageBox::question(this, "Exit Setup Assistant",
488  QString("Are you sure you want to exit the MoveIt! Setup Assistant?"),
489  QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
490  {
491  event->ignore();
492  return;
493  }
494  }
495 
496  // Shutdown app
497  event->accept();
498 }
499 
500 // ******************************************************************************************
501 // Qt Error Handling - TODO
502 // ******************************************************************************************
503 bool SetupAssistantWidget::notify(QObject* reciever, QEvent* event)
504 {
505  QMessageBox::critical(this, "Error", "An error occurred and was caught by Qt notify event handler.", QMessageBox::Ok);
506 
507  return false;
508 }
509 
510 // ******************************************************************************************
511 // Change the widget modal state based on subwidgets state
512 // ******************************************************************************************
514 {
515  navs_view_->setDisabled(isModal);
516 
517  for (int i = 0; i < nav_name_list_.count(); ++i)
518  {
519  navs_view_->setEnabled(i, !isModal);
520  }
521 }
522 
523 } // namespace
#define NULL
void unsetLinkColor(const std::string &link_name)
ViewController * getCurrent() const
virtual ViewManager * getViewManager() const
void initialize(Ogre::SceneManager *scene_manager, DisplayContext *manager)
void highlightGroup(const std::string &group_name)
virtual bool setValue(const QVariant &new_value)
f
SetupAssistantWidget(QWidget *parent, boost::program_options::variables_map args)
Start screen user interface for MoveIt! Configuration Assistant.
void setFixedFrame(const QString &frame)
Ogre::SceneManager * getSceneManager() const
virtual bool focusLost()
function called when widget lost focus, allows to accept/reject changes and to reject switching (retu...
void highlightLink(const std::string &link_name, const QColor &color)
void setEnabled(const int &index, bool enabled)
virtual Property * subProp(const QString &sub_name)
void setPath(const QString &path)
Set the path with QString.
void setNavs(const QList< QString > &navs)
virtual void focusGiven()
function called when widget is activated, allows to update/initialize GUI
User interface for editing the default collision matrix list in an SRDF.
void addDisplay(Display *display, bool enabled)
moveit_ros_control::ROSControllersWidget * controllers_widget_
static const std::string MOVEIT_ROBOT_STATE
ROSLIB_DECL std::string getPath(const std::string &package_name)
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
void setName(const QString &name)
moveit_rviz_plugin::RobotStateDisplay * robot_state_display_
This class is shared with all widgets and contains the common configuration data needed for generatin...
void setLinkColor(const std::string &link_name, const QColor &color)
static const std::string ROBOT_DESCRIPTION
ROSCPP_DECL void spinOnce()
virtual bool notify(QObject *rec, QEvent *ev)


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sun Oct 18 2020 13:19:28