setup_assistant_widget.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman */
00036 
00037 // SA
00038 #include "setup_screen_widget.h"  // a base class for screens in the setup assistant
00039 #include "setup_assistant_widget.h"
00040 // Qt
00041 #include <QStackedLayout>
00042 #include <QListWidget>
00043 #include <QListWidgetItem>
00044 #include <QDebug>
00045 #include <QFont>
00046 #include <QLabel>
00047 #include <QPushButton>
00048 #include <QCloseEvent>
00049 #include <QMessageBox>
00050 #include <QString>
00051 #include <pluginlib/class_loader.h>  // for loading all avail kinematic planners
00052 // Rviz
00053 #include <rviz/render_panel.h>
00054 #include <rviz/visualization_manager.h>
00055 #include <rviz/view_manager.h>
00056 #include <rviz/default_plugin/view_controllers/orbit_view_controller.h>
00057 #include <moveit/robot_state_rviz_plugin/robot_state_display.h>
00058 
00059 namespace moveit_setup_assistant
00060 {
00061 // ******************************************************************************************
00062 // Outer User Interface for MoveIt Configuration Assistant
00063 // ******************************************************************************************
00064 SetupAssistantWidget::SetupAssistantWidget(QWidget* parent, boost::program_options::variables_map args)
00065   : QWidget(parent)
00066 {
00067   rviz_manager_ = NULL;
00068   rviz_render_panel_ = NULL;
00069 
00070   // Create object to hold all moveit configuration data
00071   config_data_.reset(new MoveItConfigData());
00072 
00073   // Set debug mode flag if necessary
00074   if (args.count("debug"))
00075     config_data_->debug_ = true;
00076 
00077   // Basic widget container -----------------------------------------
00078   QHBoxLayout* layout = new QHBoxLayout();
00079   layout->setAlignment(Qt::AlignTop);
00080 
00081   // Create main content stack for various screens
00082   main_content_ = new QStackedLayout();
00083   current_index_ = 0;
00084 
00085   // Wrap main_content_ with a widget
00086   middle_frame_ = new QWidget(this);
00087   middle_frame_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
00088   middle_frame_->setLayout(main_content_);
00089 
00090   // Screens --------------------------------------------------------
00091 
00092   // Start Screen
00093   ssw_ = new StartScreenWidget(this, config_data_);
00094   ssw_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
00095   connect(ssw_, SIGNAL(readyToProgress()), this, SLOT(progressPastStartScreen()));
00096   connect(ssw_, SIGNAL(loadRviz()), this, SLOT(loadRviz()));
00097   main_content_->addWidget(ssw_);
00098 
00099   // Pass command arg values to start screen and show appropriate part of screen
00100   if (args.count("urdf_path"))
00101   {
00102     ssw_->urdf_file_->setPath(args["urdf_path"].as<std::string>());
00103     ssw_->select_mode_->btn_new_->click();
00104   }
00105   if (args.count("config_pkg"))
00106   {
00107     ssw_->stack_path_->setPath(args["config_pkg"].as<std::string>());
00108     ssw_->select_mode_->btn_exist_->click();
00109   }
00110   else
00111   {
00112     // Open the directory where the MSA was started from.
00113     // cf. http://stackoverflow.com/a/7413516/577001
00114     QString pwdir("");
00115     char* pwd;
00116     pwd = getenv("PWD");
00117     pwdir.append(pwd);
00118     ssw_->stack_path_->setPath(pwdir);
00119   }
00120 
00121   // Add Navigation Buttons (but do not load widgets yet except start screen)
00122   nav_name_list_ << "Start";
00123   nav_name_list_ << "Self-Collisions";
00124   nav_name_list_ << "Virtual Joints";
00125   nav_name_list_ << "Planning Groups";
00126   nav_name_list_ << "Robot Poses";
00127   nav_name_list_ << "End Effectors";
00128   nav_name_list_ << "Passive Joints";
00129   nav_name_list_ << "Author Information";
00130   nav_name_list_ << "Configuration Files";
00131 
00132   // Navigation Left Pane --------------------------------------------------
00133   navs_view_ = new NavigationWidget(this);
00134   navs_view_->setNavs(nav_name_list_);
00135   navs_view_->setDisabled(true);
00136   navs_view_->setSelected(0);  // start screen
00137 
00138   // Rviz View Right Pane ---------------------------------------------------
00139   rviz_container_ = new QWidget(this);
00140   rviz_container_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00141   rviz_container_->hide();  // do not show until after the start screen
00142 
00143   // Split screen -----------------------------------------------------
00144   splitter_ = new QSplitter(Qt::Horizontal, this);
00145   splitter_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
00146   splitter_->addWidget(navs_view_);
00147   splitter_->addWidget(middle_frame_);
00148   splitter_->addWidget(rviz_container_);
00149   splitter_->setHandleWidth(6);
00150   // splitter_->setCollapsible( 0, false ); // don't let navigation collapse
00151   layout->addWidget(splitter_);
00152 
00153   // Add event for switching between screens -------------------------
00154   connect(navs_view_, SIGNAL(clicked(const QModelIndex&)), this, SLOT(navigationClicked(const QModelIndex&)));
00155 
00156   // Final Layout Setup ---------------------------------------------
00157   this->setLayout(layout);
00158 
00159   // Title
00160   this->setWindowTitle("MoveIt Setup Assistant");  // title of window
00161 
00162   // Show screen before message
00163   QApplication::processEvents();
00164 }
00165 
00166 // ******************************************************************************************
00167 // Decontructor
00168 // ******************************************************************************************
00169 SetupAssistantWidget::~SetupAssistantWidget()
00170 {
00171   if (rviz_manager_ != NULL)
00172     rviz_manager_->removeAllDisplays();
00173   if (rviz_render_panel_ != NULL)
00174     delete rviz_render_panel_;
00175   if (rviz_manager_ != NULL)
00176     delete rviz_manager_;
00177 }
00178 
00179 void SetupAssistantWidget::virtualJointReferenceFrameChanged()
00180 {
00181   if (rviz_manager_ && robot_state_display_)
00182   {
00183     rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame()));
00184     robot_state_display_->reset();
00185   }
00186 }
00187 
00188 // ******************************************************************************************
00189 // Change screens of Setup Assistant
00190 // ******************************************************************************************
00191 void SetupAssistantWidget::navigationClicked(const QModelIndex& index)
00192 {
00193   // Convert QModelIndex to int
00194   moveToScreen(index.row());
00195 }
00196 
00197 // ******************************************************************************************
00198 // Change screens
00199 // ******************************************************************************************
00200 void SetupAssistantWidget::moveToScreen(const int index)
00201 {
00202   boost::mutex::scoped_lock slock(change_screen_lock_);
00203 
00204   if (current_index_ != index)
00205   {
00206     current_index_ = index;
00207 
00208     // Unhighlight anything on robot
00209     unhighlightAll();
00210 
00211     // Change screens
00212     main_content_->setCurrentIndex(index);
00213 
00214     // Send the focus given command to the screen widget
00215     SetupScreenWidget* ssw = qobject_cast<SetupScreenWidget*>(main_content_->widget(index));
00216     ssw->focusGiven();
00217 
00218     // Change navigation selected option
00219     navs_view_->setSelected(index);  // Select first item in list
00220   }
00221 }
00222 
00223 // ******************************************************************************************
00224 // Loads other windows, enables navigation
00225 // ******************************************************************************************
00226 void SetupAssistantWidget::progressPastStartScreen()
00227 {
00228   // Load all widgets ------------------------------------------------
00229 
00230   // Self-Collisions
00231   dcw_ = new DefaultCollisionsWidget(this, config_data_);
00232   main_content_->addWidget(dcw_);
00233   connect(dcw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00234           SLOT(highlightLink(const std::string&, const QColor&)));
00235   connect(dcw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00236   connect(dcw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00237 
00238   // Virtual Joints
00239   vjw_ = new VirtualJointsWidget(this, config_data_);
00240   main_content_->addWidget(vjw_);
00241   connect(vjw_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
00242   connect(vjw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00243           SLOT(highlightLink(const std::string&, const QColor&)));
00244   connect(vjw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00245   connect(vjw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00246   connect(vjw_, SIGNAL(referenceFrameChanged()), this, SLOT(virtualJointReferenceFrameChanged()));
00247 
00248   // Planning Groups
00249   pgw_ = new PlanningGroupsWidget(this, config_data_);
00250   main_content_->addWidget(pgw_);
00251   connect(pgw_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
00252   connect(pgw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00253           SLOT(highlightLink(const std::string&, const QColor&)));
00254   connect(pgw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00255   connect(pgw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00256 
00257   // Robot Poses
00258   rpw_ = new RobotPosesWidget(this, config_data_);
00259   main_content_->addWidget(rpw_);
00260   connect(rpw_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
00261   connect(rpw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00262           SLOT(highlightLink(const std::string&, const QColor&)));
00263   connect(rpw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00264   connect(rpw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00265 
00266   // End Effectors
00267   efw_ = new EndEffectorsWidget(this, config_data_);
00268   main_content_->addWidget(efw_);
00269   connect(efw_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
00270   connect(efw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00271           SLOT(highlightLink(const std::string&, const QColor&)));
00272   connect(efw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00273   connect(efw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00274 
00275   // Virtual Joints
00276   pjw_ = new PassiveJointsWidget(this, config_data_);
00277   main_content_->addWidget(pjw_);
00278   connect(pjw_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
00279   connect(pjw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00280           SLOT(highlightLink(const std::string&, const QColor&)));
00281   connect(pjw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00282   connect(pjw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00283 
00284   // Author Information
00285   aiw_ = new AuthorInformationWidget(this, config_data_);
00286   main_content_->addWidget(aiw_);
00287   connect(aiw_, SIGNAL(isModal(bool)), this, SLOT(setModalMode(bool)));
00288   connect(aiw_, SIGNAL(highlightLink(const std::string&, const QColor&)), this,
00289           SLOT(highlightLink(const std::string&, const QColor&)));
00290   connect(aiw_, SIGNAL(highlightGroup(const std::string&)), this, SLOT(highlightGroup(const std::string&)));
00291   connect(aiw_, SIGNAL(unhighlightAll()), this, SLOT(unhighlightAll()));
00292 
00293   // Configuration Files
00294   cfw_ = new ConfigurationFilesWidget(this, config_data_);
00295   main_content_->addWidget(cfw_);
00296 
00297   // Enable all nav buttons -------------------------------------------
00298   for (int i = 0; i < nav_name_list_.count(); ++i)
00299   {
00300     navs_view_->setEnabled(i, true);
00301   }
00302 
00303   // Enable navigation
00304   navs_view_->setDisabled(false);
00305 
00306   // Replace logo with Rviz screen
00307   rviz_container_->show();
00308 
00309   // Move to next screen in debug mode
00310   if (config_data_->debug_)
00311   {
00312     moveToScreen(3);
00313   }
00314 }
00315 
00316 // ******************************************************************************************
00317 // Ping ROS on internval
00318 // ******************************************************************************************
00319 void SetupAssistantWidget::updateTimer()
00320 {
00321   ros::spinOnce();  // keep ROS node alive
00322 }
00323 
00324 // ******************************************************************************************
00325 // Load Rviz once we have a robot description ready
00326 // ******************************************************************************************
00327 void SetupAssistantWidget::loadRviz()
00328 {
00329   // Create rviz frame
00330   rviz_render_panel_ = new rviz::RenderPanel();
00331   rviz_render_panel_->setMinimumWidth(200);
00332   rviz_render_panel_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
00333 
00334   rviz_manager_ = new rviz::VisualizationManager(rviz_render_panel_);
00335   rviz_render_panel_->initialize(rviz_manager_->getSceneManager(), rviz_manager_);
00336   rviz_manager_->initialize();
00337   rviz_manager_->startUpdate();
00338 
00339   // Set the fixed and target frame
00340   rviz_manager_->setFixedFrame(QString::fromStdString(config_data_->getRobotModel()->getModelFrame()));
00341 
00342   // Create the MoveIt Rviz Plugin and attach to display
00343   robot_state_display_ = new moveit_rviz_plugin::RobotStateDisplay();
00344   robot_state_display_->setName("Robot State");
00345 
00346   rviz_manager_->addDisplay(robot_state_display_, true);
00347 
00348   // Set the topic on which the moveit_msgs::PlanningScene messages are recieved
00349   robot_state_display_->subProp("Robot State Topic")->setValue(QString::fromStdString(MOVEIT_ROBOT_STATE));
00350 
00351   // Set robot description
00352   robot_state_display_->subProp("Robot Description")->setValue(QString::fromStdString(ROBOT_DESCRIPTION));
00353 
00354   // Zoom into robot
00355   rviz::ViewController* view = rviz_manager_->getViewManager()->getCurrent();
00356   view->subProp("Distance")->setValue(4.0f);
00357 
00358   // Add Rviz to Planning Groups Widget
00359   QHBoxLayout* rviz_layout = new QHBoxLayout();
00360   rviz_layout->addWidget(rviz_render_panel_);
00361   rviz_container_->setLayout(rviz_layout);
00362 
00363   rviz_container_->show();
00364 }
00365 
00366 // ******************************************************************************************
00367 // Highlight a robot link
00368 // ******************************************************************************************
00369 void SetupAssistantWidget::highlightLink(const std::string& link_name, const QColor& color)
00370 {
00371   const robot_model::LinkModel* lm = config_data_->getRobotModel()->getLinkModel(link_name);
00372   if (!lm->getShapes().empty())  // skip links with no geometry
00373     robot_state_display_->setLinkColor(link_name, color);
00374 }
00375 
00376 // ******************************************************************************************
00377 // Highlight a robot group
00378 // ******************************************************************************************
00379 void SetupAssistantWidget::highlightGroup(const std::string& group_name)
00380 {
00381   // Highlight the selected planning group by looping through the links
00382   if (!config_data_->getRobotModel()->hasJointModelGroup(group_name))
00383     return;
00384 
00385   const robot_model::JointModelGroup* joint_model_group = config_data_->getRobotModel()->getJointModelGroup(group_name);
00386   if (joint_model_group)
00387   {
00388     const std::vector<const robot_model::LinkModel*>& link_models = joint_model_group->getLinkModels();
00389     // Iterate through the links
00390     for (std::vector<const robot_model::LinkModel*>::const_iterator link_it = link_models.begin();
00391          link_it < link_models.end(); ++link_it)
00392       highlightLink((*link_it)->getName(), QColor(255, 0, 0));
00393   }
00394 }
00395 
00396 // ******************************************************************************************
00397 // Unhighlight all robot links
00398 // ******************************************************************************************
00399 void SetupAssistantWidget::unhighlightAll()
00400 {
00401   // Get the names of the all links robot
00402   const std::vector<std::string>& links = config_data_->getRobotModel()->getLinkModelNamesWithCollisionGeometry();
00403 
00404   // Quit if no links found
00405   if (links.empty())
00406   {
00407     return;
00408   }
00409 
00410   // check if rviz is ready
00411   if (!rviz_manager_ || !robot_state_display_)
00412   {
00413     return;
00414   }
00415 
00416   // Iterate through the links
00417   for (std::vector<std::string>::const_iterator link_it = links.begin(); link_it < links.end(); ++link_it)
00418   {
00419     if ((*link_it).empty())
00420       continue;
00421 
00422     robot_state_display_->unsetLinkColor(*link_it);
00423   }
00424 }
00425 
00426 // ******************************************************************************************
00427 // Qt close event function for reminding user to save
00428 // ******************************************************************************************
00429 void SetupAssistantWidget::closeEvent(QCloseEvent* event)
00430 {
00431   // Only prompt to close if not in debug mode
00432   if (!config_data_->debug_)
00433   {
00434     if (QMessageBox::question(this, "Exit Setup Assistant",
00435                               QString("Are you sure you want to exit the MoveIt Setup Assistant?"),
00436                               QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
00437     {
00438       event->ignore();
00439       return;
00440     }
00441   }
00442 
00443   // Shutdown app
00444   event->accept();
00445 }
00446 
00447 // ******************************************************************************************
00448 // Qt Error Handling - TODO
00449 // ******************************************************************************************
00450 bool SetupAssistantWidget::notify(QObject* reciever, QEvent* event)
00451 {
00452   QMessageBox::critical(this, "Error", "An error occurred and was caught by Qt notify event handler.", QMessageBox::Ok);
00453 
00454   return false;
00455 }
00456 
00457 // ******************************************************************************************
00458 // Change the widget modal state based on subwidgets state
00459 // ******************************************************************************************
00460 void SetupAssistantWidget::setModalMode(bool isModal)
00461 {
00462   navs_view_->setDisabled(isModal);
00463 
00464   for (int i = 0; i < nav_name_list_.count(); ++i)
00465   {
00466     navs_view_->setEnabled(i, !isModal);
00467   }
00468 }
00469 
00470 }  // namespace


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Mon Jul 24 2017 02:22:29