main_window.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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: Mario Prats, Ioan Sucan */
00036 
00037 #include <main_window.h>
00038 #include <ui_utils.h>
00039 
00040 #include <QShortcut>
00041 #include <QFileDialog>
00042 #include <QMessageBox>
00043 
00044 #include <assert.h>
00045 #include <string>
00046 #include <rviz/tool_manager.h>
00047 #include <rviz/frame_manager.h>
00048 #include <rviz/display_factory.h>
00049 #include <rviz/selection/selection_manager.h>
00050 
00051 namespace benchmark_tool
00052 {
00053 const char* MainWindow::ROBOT_DESCRIPTION_PARAM = "robot_description";
00054 const char* MainWindow::ROBOT_DESCRIPTION_SEMANTIC_PARAM = "robot_description_semantic";
00055 const unsigned int MainWindow::DEFAULT_WAREHOUSE_PORT = 33830;
00056 
00057 MainWindow::MainWindow(int argc, char** argv, QWidget* parent) : QMainWindow(parent), goal_pose_dragging_(false)
00058 {
00059   QMessageBox::warning(this, "Development", "This GUI is under development and has many issues. Use at your own risk.");
00060 
00061   setWindowTitle("Benchmark Tool");
00062 
00063   goal_offset_.setIdentity();
00064 
00065   ui_.setupUi(this);
00066   settings_.reset(new QSettings("WillowGarage", "Benchmark Tool"));
00067   QVariant previous_database_names = settings_->value("database_name", QStringList());
00068   ui_.db_combo->addItems(previous_database_names.toStringList());
00069 
00070   robot_loader_dialog_ = new QDialog(0, 0);
00071   load_robot_ui_.setupUi(robot_loader_dialog_);
00072 
00073   run_benchmark_dialog_ = new QDialog(0, 0);
00074   run_benchmark_ui_.setupUi(run_benchmark_dialog_);
00075 
00076   bbox_dialog_ = new QDialog(0, 0);
00077   bbox_dialog_ui_.setupUi(bbox_dialog_);
00078 
00079   // Rviz render panel
00080   render_panel_ = new rviz::RenderPanel();
00081   ui_.render_widget->addWidget(render_panel_);
00082   ui_.splitter->setStretchFactor(1, 4);
00083 
00084   visualization_manager_ = new rviz::VisualizationManager(render_panel_);
00085   render_panel_->initialize(visualization_manager_->getSceneManager(), visualization_manager_);
00086 
00087   visualization_manager_->initialize();
00088   visualization_manager_->startUpdate();
00089 
00090   // Grid display
00091   visualization_manager_->createDisplay("rviz/Grid", "Grid", true);
00092 
00093   // Planning scene display
00094   scene_display_ = new moveit_rviz_plugin::PlanningSceneDisplay();
00095   scene_display_->setName("Planning Scene");
00096   scene_display_->subProp("Robot Description")->setValue(ROBOT_DESCRIPTION_PARAM);
00097   scene_display_->subProp("Scene Geometry")->subProp("Scene Alpha")->setValue(1.0);
00098   visualization_manager_->addDisplay(scene_display_, true);
00099 
00100   // Interactive Marker display
00101   int_marker_display_ = visualization_manager_->getDisplayFactory()->make("rviz/InteractiveMarkers");
00102   int_marker_display_->initialize(visualization_manager_);
00103   int_marker_display_->setEnabled(true);
00104   int_marker_display_->subProp("Update Topic")
00105       ->setValue(QString::fromStdString(robot_interaction::RobotInteraction::INTERACTIVE_MARKER_TOPIC + "/update"));
00106 
00107   if (scene_display_)
00108   {
00109     if (waitForPlanningSceneMonitor(scene_display_))
00110     {
00111       configure();
00112     }
00113     else
00114     {
00115       ROS_INFO("Couldn't find a valid robot description in the parameter server. Please load a robot on the GUI.");
00116     }
00117 
00118     if (ui_.planning_group_combo->count() > 0)
00119     {
00120       planningGroupChanged(ui_.planning_group_combo->currentText());
00121     }
00122 
00123     rviz::Tool* interact_tool = visualization_manager_->getToolManager()->addTool("rviz/Interact");
00124     if (interact_tool)
00125     {
00126       visualization_manager_->getToolManager()->setCurrentTool(interact_tool);
00127       interact_tool->activate();
00128     }
00129     visualization_manager_->getSelectionManager()->enableInteraction(true);
00130 
00131     // Setup ui: colors and icons
00132     ui_.db_connect_button->setStyleSheet("QPushButton { color : green }");
00133 
00134     ui_.goal_poses_open_button->setIcon(
00135         QIcon::fromTheme("document-open", QApplication::style()->standardIcon(QStyle::SP_DirOpenIcon)));
00136     ui_.goal_poses_add_button->setIcon(
00137         QIcon::fromTheme("list-add", QApplication::style()->standardIcon(QStyle::SP_FileDialogNewFolder)));
00138     ui_.goal_poses_remove_button->setIcon(
00139         QIcon::fromTheme("list-remove", QApplication::style()->standardIcon(QStyle::SP_DialogDiscardButton)));
00140     ui_.goal_poses_save_button->setIcon(
00141         QIcon::fromTheme("document-save", QApplication::style()->standardIcon(QStyle::SP_DriveFDIcon)));
00142     ui_.goal_switch_visibility_button->setIcon(QApplication::style()->standardIcon(QStyle::SP_DialogDiscardButton));
00143 
00144     ui_.start_states_open_button->setIcon(
00145         QIcon::fromTheme("document-open", QApplication::style()->standardIcon(QStyle::SP_DirOpenIcon)));
00146     ui_.start_states_add_button->setIcon(
00147         QIcon::fromTheme("list-add", QApplication::style()->standardIcon(QStyle::SP_FileDialogNewFolder)));
00148     ui_.start_states_remove_button->setIcon(
00149         QIcon::fromTheme("list-remove", QApplication::style()->standardIcon(QStyle::SP_DialogDiscardButton)));
00150     ui_.start_states_save_button->setIcon(
00151         QIcon::fromTheme("document-save", QApplication::style()->standardIcon(QStyle::SP_DriveFDIcon)));
00152 
00153     ui_.trajectory_open_button->setIcon(
00154         QIcon::fromTheme("document-open", QApplication::style()->standardIcon(QStyle::SP_DirOpenIcon)));
00155     ui_.trajectory_add_button->setIcon(
00156         QIcon::fromTheme("list-add", QApplication::style()->standardIcon(QStyle::SP_FileDialogNewFolder)));
00157     ui_.trajectory_remove_button->setIcon(
00158         QIcon::fromTheme("list-remove", QApplication::style()->standardIcon(QStyle::SP_DialogDiscardButton)));
00159     ui_.trajectory_save_button->setIcon(
00160         QIcon::fromTheme("document-save", QApplication::style()->standardIcon(QStyle::SP_DriveFDIcon)));
00161 
00162     // Connect signals and slots
00163     connect(ui_.actionExit, SIGNAL(triggered(bool)), this, SLOT(exitActionTriggered(bool)));
00164     connect(ui_.actionOpen, SIGNAL(triggered(bool)), this, SLOT(openActionTriggered(bool)));
00165     connect(ui_.planning_group_combo, SIGNAL(currentIndexChanged(const QString&)), this,
00166             SLOT(planningGroupChanged(const QString&)));
00167     connect(ui_.db_connect_button, SIGNAL(clicked()), this, SLOT(dbConnectButtonClicked()));
00168     connect(ui_.load_scene_button, SIGNAL(clicked()), this, SLOT(loadSceneButtonClicked()));
00169     connect(ui_.planning_scene_list, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this,
00170             SLOT(loadSceneButtonClicked(QListWidgetItem*)));
00171     connect(ui_.robot_interaction_button, SIGNAL(clicked()), this, SLOT(robotInteractionButtonClicked()));
00172 
00173     run_benchmark_ui_.benchmark_select_folder_button->setIcon(
00174         QIcon::fromTheme("document-open", QApplication::style()->standardIcon(QStyle::SP_DirOpenIcon)));
00175     connect(run_benchmark_ui_.run_benchmark_button, SIGNAL(clicked()), this, SLOT(runBenchmarkButtonClicked()));
00176     connect(run_benchmark_ui_.save_config_button, SIGNAL(clicked()), this, SLOT(saveBenchmarkConfigButtonClicked()));
00177     connect(run_benchmark_ui_.cancel_button, SIGNAL(clicked()), this, SLOT(cancelBenchmarkButtonClicked()));
00178     connect(run_benchmark_ui_.benchmark_select_folder_button, SIGNAL(clicked()), this,
00179             SLOT(benchmarkFolderButtonClicked()));
00180     connect(run_benchmark_ui_.benchmark_include_planners_checkbox, SIGNAL(clicked(bool)),
00181             run_benchmark_ui_.planning_interfaces_text, SLOT(setEnabled(bool)));
00182     connect(run_benchmark_ui_.benchmark_include_planners_checkbox, SIGNAL(clicked(bool)),
00183             run_benchmark_ui_.planning_interfaces_label, SLOT(setEnabled(bool)));
00184     connect(run_benchmark_ui_.benchmark_include_planners_checkbox, SIGNAL(clicked(bool)),
00185             run_benchmark_ui_.planning_algorithms_text, SLOT(setEnabled(bool)));
00186     connect(run_benchmark_ui_.benchmark_include_planners_checkbox, SIGNAL(clicked(bool)),
00187             run_benchmark_ui_.planning_algorithms_label, SLOT(setEnabled(bool)));
00188 
00189     // Goal poses
00190     QMenu* add_button_menu = new QMenu(ui_.goal_poses_add_button);
00191     QAction* add_single_goal_action = new QAction("Single goal", add_button_menu);
00192     QAction* bbox_goals_action = new QAction("Goals in a bounding box", add_button_menu);
00193     add_button_menu->addAction(add_single_goal_action);
00194     add_button_menu->addAction(bbox_goals_action);
00195     ui_.goal_poses_add_button->setMenu(add_button_menu);
00196     connect(add_single_goal_action, SIGNAL(triggered()), this, SLOT(createGoalPoseButtonClicked()));
00197     connect(bbox_goals_action, SIGNAL(triggered()), this, SLOT(showBBoxGoalsDialog()));
00198     connect(bbox_dialog_ui_.ok_button, SIGNAL(clicked()), this, SLOT(createBBoxGoalsButtonClicked()));
00199     connect(bbox_dialog_ui_.cancel_button, SIGNAL(clicked()), bbox_dialog_, SLOT(hide()));
00200     connect(ui_.goal_poses_remove_button, SIGNAL(clicked()), this, SLOT(deleteGoalsOnDBButtonClicked()));
00201     connect(ui_.load_poses_filter_text, SIGNAL(returnPressed()), this, SLOT(loadGoalsFromDBButtonClicked()));
00202     connect(ui_.goal_poses_open_button, SIGNAL(clicked()), this, SLOT(loadGoalsFromDBButtonClicked()));
00203     connect(ui_.goal_poses_save_button, SIGNAL(clicked()), this, SLOT(saveGoalsOnDBButtonClicked()));
00204     connect(ui_.goal_switch_visibility_button, SIGNAL(clicked()), this, SLOT(switchGoalVisibilityButtonClicked()));
00205     connect(ui_.goal_poses_list, SIGNAL(itemSelectionChanged()), this, SLOT(goalPoseSelectionChanged()));
00206     connect(ui_.goal_poses_list, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this,
00207             SLOT(goalPoseDoubleClicked(QListWidgetItem*)));
00208     connect(ui_.show_x_checkbox, SIGNAL(stateChanged(int)), this, SLOT(visibleAxisChanged(int)));
00209     connect(ui_.show_y_checkbox, SIGNAL(stateChanged(int)), this, SLOT(visibleAxisChanged(int)));
00210     connect(ui_.show_z_checkbox, SIGNAL(stateChanged(int)), this, SLOT(visibleAxisChanged(int)));
00211     connect(ui_.goal_offset_roll, SIGNAL(editingFinished()), this, SLOT(goalOffsetChanged()));
00212     connect(ui_.goal_offset_pitch, SIGNAL(editingFinished()), this, SLOT(goalOffsetChanged()));
00213     connect(ui_.goal_offset_yaw, SIGNAL(editingFinished()), this, SLOT(goalOffsetChanged()));
00214 
00215     connect(ui_.check_goal_collisions_button, SIGNAL(clicked()), this, SLOT(checkGoalsInCollision()));
00216     connect(ui_.check_goal_reachability_button, SIGNAL(clicked()), this, SLOT(checkGoalsReachable()));
00217     connect(ui_.run_benchmark_button, SIGNAL(clicked()), this, SLOT(runBenchmark()));
00218     connect(ui_.load_results_button, SIGNAL(clicked()), this, SLOT(loadBenchmarkResults()));
00219 
00220     connect(ui_.start_states_add_button, SIGNAL(clicked()), this, SLOT(saveStartStateButtonClicked()));
00221     connect(ui_.start_states_remove_button, SIGNAL(clicked()), this, SLOT(deleteStatesOnDBButtonClicked()));
00222     connect(ui_.load_states_filter_text, SIGNAL(returnPressed()), this, SLOT(loadStatesFromDBButtonClicked()));
00223     connect(ui_.start_states_open_button, SIGNAL(clicked()), this, SLOT(loadStatesFromDBButtonClicked()));
00224     connect(ui_.start_states_save_button, SIGNAL(clicked()), this, SLOT(saveStatesOnDBButtonClicked()));
00225     connect(ui_.start_states_list, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this,
00226             SLOT(startStateItemDoubleClicked(QListWidgetItem*)));
00227 
00228     QShortcut* copy_goals_shortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_D), ui_.goal_poses_list);
00229     connect(copy_goals_shortcut, SIGNAL(activated()), this, SLOT(copySelectedGoalPoses()));
00230 
00231     // Trajectories
00232     connect(ui_.trajectory_add_button, SIGNAL(clicked()), this, SLOT(createTrajectoryButtonClicked()));
00233     connect(ui_.trajectory_remove_button, SIGNAL(clicked()), this, SLOT(removeTrajectoryButtonClicked()));
00234     connect(ui_.trajectory_open_button, SIGNAL(clicked()), this, SLOT(loadTrajectoriesFromDBButtonClicked()));
00235     connect(ui_.trajectory_save_button, SIGNAL(clicked()), this, SLOT(saveTrajectoriesOnDBButtonClicked()));
00236     connect(ui_.trajectory_list, SIGNAL(itemSelectionChanged()), this, SLOT(trajectorySelectionChanged()));
00237     connect(ui_.trajectory_nwaypoints_spin, SIGNAL(valueChanged(int)), this, SLOT(trajectoryNWaypointsChanged(int)));
00238     connect(ui_.trajectory_execute_button, SIGNAL(clicked()), this, SLOT(trajectoryExecuteButtonClicked()));
00239 
00240     // Start a QTimer for handling main loop jobs
00241     main_loop_jobs_timer_.reset(new QTimer(this));
00242     connect(main_loop_jobs_timer_.get(), SIGNAL(timeout()), this, SLOT(MainLoop()));
00243     main_loop_jobs_timer_->start(1000 / MAIN_LOOP_RATE);
00244   }
00245   else
00246   {
00247     ROS_ERROR("Cannot load robot. Is the robot_description parameter set?");
00248     exit(0);
00249   }
00250 }
00251 
00252 MainWindow::~MainWindow()
00253 {
00254 }
00255 
00256 bool MainWindow::waitForPlanningSceneMonitor(moveit_rviz_plugin::PlanningSceneDisplay* scene_display)
00257 {
00258   // Wait until the planning scene monitor is up
00259   ros::Time tstart = ros::Time::now();
00260   static const double psm_waiting_timeout = 10.0;
00261   while (!scene_display->getPlanningSceneMonitor() && (ros::Time::now() - tstart) < ros::Duration(psm_waiting_timeout))
00262   {
00263     ROS_INFO_ONCE("Waiting for the planning scene monitor...");
00264     scene_display->update(0.1, 0.1);
00265   }
00266 
00267   return static_cast<bool>(scene_display->getPlanningSceneMonitor());
00268 }
00269 
00270 void MainWindow::exitActionTriggered(bool)
00271 {
00272   QApplication::quit();
00273 }
00274 
00275 void MainWindow::openActionTriggered(bool)
00276 {
00277   QString urdf_path =
00278       QFileDialog::getOpenFileName(this, tr("Select a robot description file"), tr(""), tr("URDF files (*.urdf)"));
00279 
00280   if (!urdf_path.isEmpty())
00281   {
00282     QString srdf_path = QFileDialog::getOpenFileName(this, tr("Select a semantic robot description file"), tr(""),
00283                                                      tr("SRDF files (*.srdf)"));
00284 
00285     if (!srdf_path.isEmpty())
00286     {
00287       loadNewRobot(urdf_path.toStdString(), srdf_path.toStdString());
00288     }
00289   }
00290 }
00291 
00292 void MainWindow::loadNewRobot(const std::string& urdf_path, const std::string& srdf_path)
00293 {
00294   load_robot_ui_.status_label->setText("");
00295   load_robot_ui_.load_progress_bar->setValue(0);
00296 
00297   robot_loader_dialog_->show();
00298 
00299   QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
00300 
00301   // Load urdf
00302   boost::filesystem::path boost_urdf_path(urdf_path);
00303   load_robot_ui_.status_label->setText(QString::fromStdString("Loading urdf " + boost_urdf_path.string()));
00304   load_robot_ui_.load_progress_bar->setValue(10);
00305   if (boost::filesystem::exists(boost_urdf_path))
00306   {
00307     std::ifstream urdf_input_stream(boost_urdf_path.string().c_str());
00308     std::stringstream urdf_sstr;
00309     urdf_sstr << urdf_input_stream.rdbuf();
00310     ros::param::set(ROBOT_DESCRIPTION_PARAM, urdf_sstr.str());
00311   }
00312   else
00313   {
00314     ROS_ERROR("Cannot load URDF file");
00315   }
00316 
00317   // Load srdf
00318   boost::filesystem::path boost_srdf_path(srdf_path);
00319   load_robot_ui_.status_label->setText(QString::fromStdString("Loading srdf " + boost_srdf_path.string()));
00320   load_robot_ui_.load_progress_bar->setValue(20);
00321   if (boost::filesystem::exists(boost_srdf_path))
00322   {
00323     std::ifstream srdf_input_stream(boost_srdf_path.string().c_str());
00324     std::stringstream srdf_sstr;
00325     srdf_sstr << srdf_input_stream.rdbuf();
00326     ros::param::set(ROBOT_DESCRIPTION_SEMANTIC_PARAM, srdf_sstr.str());
00327   }
00328   else
00329   {
00330     ROS_ERROR("Cannot load SRDF file");
00331   }
00332 
00333   // Load kinematics.yaml, joint_limits.yaml and ompl_planning.yaml
00334   // TODO: Can we assume kinematics.yaml to be in the same folder as the srdf?
00335   boost::filesystem::path kinematics_file =
00336       boost::filesystem::operator/(boost_srdf_path.branch_path(), "kinematics.yaml");
00337   load_robot_ui_.status_label->setText(QString::fromStdString("Loading " + kinematics_file.string()));
00338   load_robot_ui_.load_progress_bar->setValue(30);
00339   if (boost::filesystem::exists(kinematics_file) && boost::filesystem::is_regular_file(kinematics_file))
00340   {
00341     if (system(("rosparam load " + kinematics_file.string()).c_str()) < 0)
00342     {
00343       ROS_ERROR("Couldn't load kinematics.yaml file");
00344     }
00345   }
00346 
00347   boost::filesystem::path joint_limits_file =
00348       boost::filesystem::operator/(boost_srdf_path.branch_path(), "joint_limits.yaml");
00349   load_robot_ui_.status_label->setText(QString::fromStdString("Loading " + joint_limits_file.string()));
00350   load_robot_ui_.load_progress_bar->setValue(40);
00351   if (boost::filesystem::exists(joint_limits_file) && boost::filesystem::is_regular_file(joint_limits_file))
00352   {
00353     if (system(("rosparam load " + joint_limits_file.string()).c_str()) < 0)
00354     {
00355       ROS_ERROR("Couldn't load joint_limits.yaml file");
00356     }
00357   }
00358 
00359   boost::filesystem::path ompl_planning_file =
00360       boost::filesystem::operator/(boost_srdf_path.branch_path(), "ompl_planning.yaml");
00361   load_robot_ui_.status_label->setText(QString::fromStdString("Loading " + ompl_planning_file.string()));
00362   load_robot_ui_.load_progress_bar->setValue(50);
00363   if (boost::filesystem::exists(ompl_planning_file) && boost::filesystem::is_regular_file(ompl_planning_file))
00364   {
00365     if (system(("rosparam load " + ompl_planning_file.string()).c_str()) < 0)
00366     {
00367       ROS_ERROR("Couldn't load ompl_planning.yaml file");
00368     }
00369   }
00370 
00371   load_robot_ui_.status_label->setText(QString("Resetting scene display... "));
00372   load_robot_ui_.load_progress_bar->setValue(60);
00373   std::string old_scene_name;
00374   if (scene_display_->getPlanningSceneRO())
00375     old_scene_name = scene_display_->getPlanningSceneRO()->getName();
00376   scene_display_->reset();
00377 
00378   if (waitForPlanningSceneMonitor(scene_display_))
00379   {
00380     if (configure())
00381     {
00382       // Reload the scene geometry if one scene was already loaded
00383       load_robot_ui_.status_label->setText(QString("Reloading scene... "));
00384       load_robot_ui_.load_progress_bar->setValue(90);
00385       QList<QListWidgetItem*> found_items =
00386           ui_.planning_scene_list->findItems(QString::fromStdString(old_scene_name), Qt::MatchExactly);
00387       if (found_items.size() > 0)
00388       {
00389         found_items[0]->setSelected(true);
00390         loadSceneButtonClicked();
00391       }
00392 
00393       // Reload the goals
00394       load_robot_ui_.status_label->setText(QString("Reloading goals... "));
00395       load_robot_ui_.load_progress_bar->setValue(95);
00396       if (ui_.goal_poses_list->count() > 0)
00397       {
00398         loadGoalsFromDBButtonClicked();
00399       }
00400       load_robot_ui_.status_label->setText(QString(""));
00401       load_robot_ui_.load_progress_bar->setValue(100);
00402     }
00403   }
00404   else
00405   {
00406     ROS_INFO("Couldn't find a valid robot description in the parameter server. Please load a robot on the GUI.");
00407   }
00408 
00409   robot_loader_dialog_->close();
00410   load_robot_ui_.load_progress_bar->setValue(0);
00411   QApplication::restoreOverrideCursor();
00412 }
00413 
00414 bool MainWindow::configure()
00415 {
00416   if (!scene_display_->getPlanningSceneMonitor() || !scene_display_->getPlanningSceneMonitor()->getRobotModel())
00417   {
00418     ROS_ERROR("Cannot load robot");
00419     ui_.robot_interaction_button->setEnabled(false);
00420     ui_.load_scene_button->setEnabled(false);
00421     ui_.load_results_button->setEnabled(false);
00422     ui_.check_goal_collisions_button->setEnabled(false);
00423     ui_.check_goal_reachability_button->setEnabled(false);
00424     ui_.db_connect_button->setEnabled(false);
00425     ui_.goal_poses_add_button->setEnabled(false);
00426     ui_.goal_poses_remove_button->setEnabled(false);
00427     ui_.load_poses_filter_text->setEnabled(false);
00428     ui_.goal_poses_open_button->setEnabled(false);
00429     ui_.goal_poses_save_button->setEnabled(false);
00430     ui_.goal_switch_visibility_button->setEnabled(false);
00431     ui_.show_x_checkbox->setEnabled(false);
00432     ui_.show_y_checkbox->setEnabled(false);
00433     ui_.show_z_checkbox->setEnabled(false);
00434     ui_.goal_offset_roll->setEnabled(false);
00435     ui_.goal_offset_pitch->setEnabled(false);
00436     ui_.goal_offset_yaw->setEnabled(false);
00437     ui_.start_states_add_button->setEnabled(false);
00438     ui_.start_states_remove_button->setEnabled(false);
00439     ui_.start_states_open_button->setEnabled(false);
00440     ui_.start_states_save_button->setEnabled(false);
00441     ui_.run_benchmark_button->setEnabled(false);
00442 
00443     ui_.trajectory_nwaypoints_spin->setEnabled(false);
00444     ui_.trajectories_filter_text->setEnabled(false);
00445     ui_.trajectory_add_button->setEnabled(false);
00446     ui_.trajectory_remove_button->setEnabled(false);
00447     ui_.trajectory_save_button->setEnabled(false);
00448     ui_.trajectory_open_button->setEnabled(false);
00449     ui_.trajectory_execute_button->setEnabled(false);
00450     return false;
00451   }
00452 
00453   ui_.robot_interaction_button->setEnabled(true);
00454   ui_.load_scene_button->setEnabled(true);
00455   ui_.load_results_button->setEnabled(true);
00456   ui_.check_goal_collisions_button->setEnabled(true);
00457   ui_.check_goal_reachability_button->setEnabled(true);
00458   ui_.db_connect_button->setEnabled(true);
00459   ui_.goal_poses_add_button->setEnabled(true);
00460   ui_.goal_poses_remove_button->setEnabled(true);
00461   ui_.load_poses_filter_text->setEnabled(true);
00462   ui_.goal_poses_open_button->setEnabled(true);
00463   ui_.goal_poses_save_button->setEnabled(true);
00464   ui_.goal_switch_visibility_button->setEnabled(true);
00465   ui_.show_x_checkbox->setEnabled(true);
00466   ui_.show_y_checkbox->setEnabled(true);
00467   ui_.show_z_checkbox->setEnabled(true);
00468   ui_.goal_offset_roll->setEnabled(true);
00469   ui_.goal_offset_pitch->setEnabled(true);
00470   ui_.goal_offset_yaw->setEnabled(true);
00471   ui_.start_states_add_button->setEnabled(true);
00472   ui_.start_states_remove_button->setEnabled(true);
00473   ui_.start_states_open_button->setEnabled(true);
00474   ui_.start_states_save_button->setEnabled(true);
00475 
00476   ui_.trajectory_nwaypoints_spin->setEnabled(true);
00477   ui_.trajectories_filter_text->setEnabled(true);
00478   ui_.trajectory_add_button->setEnabled(true);
00479   ui_.trajectory_remove_button->setEnabled(true);
00480   ui_.trajectory_save_button->setEnabled(true);
00481   ui_.trajectory_open_button->setEnabled(true);
00482   ui_.trajectory_execute_button->setEnabled(true);
00483 
00484   // Set the fixed frame to the model frame
00485   load_robot_ui_.status_label->setText(QString("Setting fixed frame..."));
00486   load_robot_ui_.load_progress_bar->setValue(65);
00487   visualization_manager_->setFixedFrame(
00488       QString(scene_display_->getPlanningSceneMonitor()->getRobotModel()->getModelFrame().c_str()));
00489   int_marker_display_->setFixedFrame(
00490       QString::fromStdString(scene_display_->getPlanningSceneMonitor()->getRobotModel()->getModelFrame()));
00491 
00492   // robot interaction
00493   load_robot_ui_.status_label->setText(QString("Resetting robot interaction..."));
00494   load_robot_ui_.load_progress_bar->setValue(75);
00495   robot_interaction_.reset(
00496       new robot_interaction::RobotInteraction(scene_display_->getPlanningSceneMonitor()->getRobotModel()));
00497 
00498   // Configure robot-dependent ui elements
00499   ui_.load_states_filter_text->setText(
00500       QString::fromStdString(scene_display_->getPlanningSceneMonitor()->getRobotModel()->getName() + ".*"));
00501 
00502   // Get the list of planning groups and fill in the combo box
00503   load_robot_ui_.status_label->setText(QString("Updating planning groups..."));
00504   load_robot_ui_.load_progress_bar->setValue(85);
00505   std::vector<std::string> group_names =
00506       scene_display_->getPlanningSceneMonitor()->getRobotModel()->getJointModelGroupNames();
00507   ui_.planning_group_combo->clear();
00508   for (std::size_t i = 0; i < group_names.size(); i++)
00509   {
00510     ui_.planning_group_combo->addItem(QString(group_names[i].c_str()));
00511   }
00512 
00513   if (group_names.size() > 0)
00514     planningGroupChanged(QString(group_names[0].c_str()));
00515 
00516   return true;
00517 }
00518 
00519 void MainWindow::planningGroupChanged(const QString& text)
00520 {
00521   if (robot_interaction_ && !text.isEmpty())
00522   {
00523     robot_interaction_->decideActiveComponents(text.toStdString());
00524     if (robot_interaction_->getActiveEndEffectors().size() == 0)
00525       ROS_WARN_STREAM("No end-effectors defined for robot "
00526                       << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getName() << " and group "
00527                       << text.toStdString());
00528     else
00529     {
00530       // Update the kinematic state associated to the goals
00531       for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00532       {
00533         it->second->setRobotState(scene_display_->getPlanningSceneRO()->getCurrentState());
00534         it->second->setEndEffector(robot_interaction_->getActiveEndEffectors()[0]);
00535 
00536         if (query_goal_state_)
00537         {
00538           robotInteractionButtonClicked();
00539           robotInteractionButtonClicked();
00540         }
00541       }
00542     }
00543   }
00544 }
00545 
00546 void MainWindow::robotInteractionButtonClicked()
00547 {
00548   if (query_goal_state_ && robot_interaction_)
00549   {
00550     robot_interaction_->clearInteractiveMarkers();
00551     query_goal_state_.reset();
00552   }
00553   else if (robot_interaction_ && scene_display_)
00554   {
00555     query_goal_state_.reset(new robot_interaction::RobotInteraction::InteractionHandler(
00556         "goal", scene_display_->getPlanningSceneRO()->getCurrentState(),
00557         scene_display_->getPlanningSceneMonitor()->getTFClient()));
00558     query_goal_state_->setUpdateCallback(boost::bind(&MainWindow::scheduleStateUpdate, this));
00559     query_goal_state_->setGroupStateValidityCallback(
00560         boost::bind(&MainWindow::isIKSolutionCollisionFree, this, _1, _2, _3));
00561     robot_interaction_->addInteractiveMarkers(query_goal_state_);
00562   }
00563   else
00564   {
00565     ROS_WARN("robot interaction not initialized");
00566   }
00567   robot_interaction_->publishInteractiveMarkers();
00568 }
00569 
00570 bool MainWindow::isIKSolutionCollisionFree(robot_state::RobotState* state, const robot_model::JointModelGroup* group,
00571                                            const double* ik_solution)
00572 {
00573   if (scene_display_)
00574   {
00575     state->setJointGroupPositions(group, ik_solution);
00576     state->update();
00577     return !scene_display_->getPlanningSceneRO()->isStateColliding(*state, group->getName());
00578   }
00579   else
00580     return true;
00581 }
00582 
00583 void MainWindow::scheduleStateUpdate()
00584 {
00585   JobProcessing::addBackgroundJob(boost::bind(&MainWindow::scheduleStateUpdateBackgroundJob, this));
00586 }
00587 
00588 void MainWindow::scheduleStateUpdateBackgroundJob()
00589 {
00590   scene_display_->getPlanningSceneRW()->setCurrentState(*query_goal_state_->getState());
00591   scene_display_->queueRenderSceneGeometry();
00592 }
00593 
00594 void MainWindow::dbConnectButtonClicked()
00595 {
00596   JobProcessing::addBackgroundJob(boost::bind(&MainWindow::dbConnectButtonClickedBackgroundJob, this));
00597 }
00598 
00599 void MainWindow::dbConnectButtonClickedBackgroundJob()
00600 {
00601   if (planning_scene_storage_)
00602   {
00603     planning_scene_storage_.reset();
00604     robot_state_storage_.reset();
00605     constraints_storage_.reset();
00606     trajectory_constraints_storage_.reset();
00607     ui_.planning_scene_list->clear();
00608 
00609     JobProcessing::addMainLoopJob(
00610         boost::bind(&setButtonState, ui_.db_connect_button, false, "Disconnected", "QPushButton { color : red }"));
00611   }
00612   else
00613   {
00614     int port = -1;
00615     QStringList host_port;
00616 
00617     host_port = ui_.db_combo->currentText().split(":");
00618     if (host_port.size() > 1)
00619     {
00620       try
00621       {
00622         port = boost::lexical_cast<int>(host_port[1].toStdString());
00623       }
00624       catch (...)
00625       {
00626       }
00627     }
00628     else
00629       port = DEFAULT_WAREHOUSE_PORT;
00630 
00631     if (port > 0 && !host_port[0].isEmpty())
00632     {
00633       database_host_ = host_port[0].toStdString();
00634       database_port_ = port;
00635 
00636       // Store into settings
00637       QVariant previous_database_names = settings_->value("database_name", QStringList());
00638       QStringList name_list = previous_database_names.toStringList();
00639       name_list.removeAll(ui_.db_combo->currentText());
00640       name_list.push_front(ui_.db_combo->currentText());
00641       static const short db_names_history_size = 5;
00642       if (name_list.size() > db_names_history_size)
00643         name_list.pop_back();
00644       settings_->setValue("database_name", name_list);
00645       ui_.db_combo->clear();
00646       ui_.db_combo->addItems(name_list);
00647 
00648       // Connect
00649       JobProcessing::addMainLoopJob(
00650           boost::bind(&setButtonState, ui_.db_connect_button, true, "Connecting...", "QPushButton { color : green }"));
00651       try
00652       {
00653         warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase();
00654         conn->setParams(database_host_, database_port_, 5.0);
00655         if (conn->connect())
00656         {
00657           planning_scene_storage_.reset(new moveit_warehouse::PlanningSceneStorage(conn));
00658           robot_state_storage_.reset(new moveit_warehouse::RobotStateStorage(conn));
00659           constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn));
00660           trajectory_constraints_storage_.reset(new moveit_warehouse::TrajectoryConstraintsStorage(conn));
00661           JobProcessing::addMainLoopJob(boost::bind(&setButtonState, ui_.db_connect_button, true, "Getting data...",
00662                                                     "QPushButton { color : yellow }"));
00663 
00664           // Get all the scenes
00665           populatePlanningSceneList();
00666 
00667           JobProcessing::addMainLoopJob(
00668               boost::bind(&setButtonState, ui_.db_connect_button, true, "Disconnect", "QPushButton { color : blue }"));
00669         }
00670         else
00671         {
00672           JobProcessing::addMainLoopJob(
00673               boost::bind(&setButtonState, ui_.db_connect_button, false, "Connect", "QPushButton { color : green }"));
00674           JobProcessing::addMainLoopJob(boost::bind(&showCriticalMessage, this, "Error", "Unable to connect to "
00675                                                                                          "Database"));
00676           return;
00677         }
00678       }
00679       catch (std::runtime_error& ex)
00680       {
00681         ROS_ERROR("%s", ex.what());
00682         JobProcessing::addMainLoopJob(
00683             boost::bind(&setButtonState, ui_.db_connect_button, false, "Connect", "QPushButton { color : green }"));
00684         JobProcessing::addMainLoopJob(boost::bind(&showCriticalMessage, this, "Error", ex.what()));
00685         return;
00686       }
00687     }
00688     else
00689     {
00690       ROS_ERROR("Warehouse server must be introduced as host:port (eg. server.domain.com:33830)");
00691       JobProcessing::addMainLoopJob(
00692           boost::bind(&setButtonState, ui_.db_connect_button, false, "Disconnected", "QPushButton { color : red }"));
00693       JobProcessing::addMainLoopJob(boost::bind(&showCriticalMessage, this, "Error", "Warehouse server must be "
00694                                                                                      "introduced as host:port (eg. "
00695                                                                                      "server.domain.com:33830)"));
00696     }
00697   }
00698 }
00699 
00700 void MainWindow::populatePlanningSceneList(void)
00701 {
00702   ui_.planning_scene_list->setUpdatesEnabled(false);
00703 
00704   ui_.planning_scene_list->clear();
00705   ui_.planning_scene_list->setSortingEnabled(true);
00706   ui_.planning_scene_list->sortItems(Qt::AscendingOrder);
00707 
00708   std::vector<std::string> names;
00709   planning_scene_storage_->getPlanningSceneNames(names);
00710   for (std::size_t i = 0; i < names.size(); ++i)
00711   {
00712     ui_.planning_scene_list->addItem(names[i].c_str());
00713   }
00714 
00715   ui_.planning_scene_list->setUpdatesEnabled(true);
00716 }
00717 
00718 void MainWindow::loadSceneButtonClicked(QListWidgetItem* item)
00719 {
00720   loadSceneButtonClicked();
00721 }
00722 
00723 void MainWindow::loadSceneButtonClicked(void)
00724 {
00725   if (ui_.planning_scene_list->currentItem() &&
00726       ui_.planning_scene_list->currentItem()->text().toStdString() != scene_display_->getPlanningSceneRO()->getName())
00727   {
00728     ui_.goal_poses_list->clear();
00729     ui_.trajectory_list->clear();
00730     goal_poses_.clear();
00731     trajectories_.clear();
00732     JobProcessing::addBackgroundJob(boost::bind(&MainWindow::loadSceneButtonClickedBackgroundJob, this));
00733   }
00734 }
00735 
00736 void MainWindow::loadSceneButtonClickedBackgroundJob(void)
00737 {
00738   if (planning_scene_storage_)
00739   {
00740     QList<QListWidgetItem*> sel = ui_.planning_scene_list->selectedItems();
00741     if (!sel.empty())
00742     {
00743       QListWidgetItem* s = sel.front();
00744       std::string scene = s->text().toStdString();
00745 
00746       setStatusFromBackground(STATUS_INFO, QString::fromStdString("Attempting to load scene" + scene + "..."));
00747 
00748       moveit_warehouse::PlanningSceneWithMetadata scene_m;
00749       moveit_msgs::PlanningScene scene_msg;
00750       bool got_ps = false;
00751       try
00752       {
00753         got_ps = planning_scene_storage_->getPlanningScene(scene_m, scene);
00754       }
00755       catch (std::runtime_error& ex)
00756       {
00757         ROS_ERROR("%s", ex.what());
00758       }
00759 
00760       if (got_ps)
00761       {
00762         ROS_DEBUG("Loaded scene '%s'", scene.c_str());
00763         setStatusFromBackground(STATUS_INFO, QString::fromStdString("Rendering Scene..."));
00764 
00765         // Update the planning scene
00766         planning_scene_monitor::LockedPlanningSceneRW ps = scene_display_->getPlanningSceneRW();
00767         if (ps)
00768         {
00769           scene_msg = *scene_m;
00770           ps->getWorldNonConst()->clearObjects();
00771           for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
00772             ps->setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
00773           ps->processPlanningSceneWorldMsg(scene_msg.world);
00774           ps->setName(scene_msg.name);
00775           scene_display_->queueRenderSceneGeometry();
00776         }
00777 
00778         // Configure ui elements
00779         ui_.load_poses_filter_text->setText(QString::fromStdString(scene + ".*"));
00780         ui_.trajectories_filter_text->setText(QString::fromStdString(scene + ".*"));
00781         setStatusFromBackground(STATUS_INFO, QString::fromStdString(""));
00782       }
00783       else
00784       {
00785         ROS_WARN("Failed to load scene '%s'. Has the message format changed since the scene was saved?", scene.c_str());
00786       }
00787     }
00788   }
00789 }
00790 
00791 void MainWindow::updateGoalPoseMarkers(float wall_dt, float ros_dt)
00792 {
00793   if (goal_pose_dragging_)
00794   {
00795     for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
00796       if (it->second->isVisible())
00797         it->second->imarker->update(wall_dt);
00798   }
00799 
00800   for (TrajectoryMap::iterator it = trajectories_.begin(); it != trajectories_.end(); ++it)
00801     if (it->second->control_marker->isVisible())
00802     {
00803       it->second->control_marker->imarker->update(wall_dt);
00804       if (it->second->start_marker)
00805         it->second->start_marker->imarker->update(wall_dt);
00806       if (it->second->end_marker)
00807         it->second->end_marker->imarker->update(wall_dt);
00808     }
00809 }
00810 
00811 void MainWindow::MainLoop()
00812 {
00813   int_marker_display_->update(1.0 / MAIN_LOOP_RATE, 1.0 / MAIN_LOOP_RATE);
00814   updateGoalPoseMarkers(1.0 / MAIN_LOOP_RATE, 1.0 / MAIN_LOOP_RATE);
00815 
00816   JobProcessing::executeMainLoopJobs();
00817 }
00818 
00819 void MainWindow::selectItemJob(QListWidgetItem* item, bool flag)
00820 {
00821   item->setSelected(flag);
00822 }
00823 
00824 void MainWindow::setItemSelectionInList(const std::string& item_name, bool selection, QListWidget* list)
00825 {
00826   QList<QListWidgetItem*> found_items = list->findItems(QString(item_name.c_str()), Qt::MatchExactly);
00827   for (std::size_t i = 0; i < found_items.size(); ++i)
00828     found_items[i]->setSelected(selection);
00829 }
00830 }


benchmarks_gui
Author(s): Mario Prats
autogenerated on Mon Jul 24 2017 02:22:21