planning_scene_warehouse_viewer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the <ORGANIZATION> nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Matthew Klingensmith, E. Gil Jones
00031 
00032 #include <move_arm_warehouse/planning_scene_warehouse_viewer.h>
00033 #include <qt4/QtGui/qapplication.h>
00034 #include <assert.h>
00035 #include <qt4/QtGui/qsplashscreen.h>
00036 #include <qt4/QtGui/qdialogbuttonbox.h>
00037 #include <qt4/QtGui/qdialog.h>
00038 #include <qt4/QtGui/qheaderview.h>
00039 #include <qt4/QtGui/qimage.h>
00040 #include <ros/package.h>
00041 
00042 using namespace collision_space;
00043 using namespace kinematics_msgs;
00044 using namespace arm_navigation_msgs;
00045 using namespace head_monitor_msgs;
00046 using namespace move_arm_warehouse;
00047 using namespace planning_environment;
00048 using namespace planning_models;
00049 using namespace std;
00050 using namespace trajectory_msgs;
00051 using namespace planning_scene_utils;
00052 using namespace ros::param;
00053 WarehouseViewer* psv = NULL;
00054 bool inited = false;
00055 
00056 WarehouseViewer::WarehouseViewer(QWidget* parent, planning_scene_utils::PlanningSceneParameters& params) :
00057   QMainWindow(parent), PlanningSceneEditor(params)
00058 {
00059   quit_threads_ = false;
00060   initQtWidgets();
00061   selected_trajectory_name_ = "";
00062   warehouse_data_loaded_once_ = false;
00063   table_load_thread_ = NULL;
00064   planning_scene_initialized_ = false;
00065 
00066   qRegisterMetaType < std::string > ( "std::string" );
00067 
00068   popupLoadPlanningScenes();
00069 
00070 }
00071 
00072 WarehouseViewer::~WarehouseViewer()
00073 {
00074 
00075 }
00076 
00077 QGroupBox *WarehouseViewer::createMotionPlanBox()
00078 {
00079   QGroupBox* motionPlanBox = new QGroupBox;
00080   motionPlanBox->setTitle("Motion Plan Requests");
00081 
00082   selected_request_label_ = new QLabel;
00083   selected_request_label_->setText("Selected Request: None");
00084   selected_request_label_->setToolTip("Selected motion plan request. New trajectories will be planned for this request.");
00085 
00086   motion_plan_tree_ = new QTreeWidget;
00087   motion_plan_tree_->setColumnCount(1);
00088   motion_plan_tree_->setToolTip("Motion plan tree. Open a motion plan request to access its controls. Click to select a request.");
00089 
00090   QWidget* mpButtons = new QWidget;
00091 
00092   QPushButton* deleteMPRButton = new QPushButton;
00093   deleteMPRButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00094   deleteMPRButton->setText("Delete Motion Plan Request");
00095   deleteMPRButton->setToolTip("Deletes the currently selected motion plan request.");
00096 
00097   QPushButton* newMPRButton = new QPushButton;
00098   newMPRButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00099   newMPRButton->setText("New Motion Plan Request");
00100   newMPRButton->setToolTip("Create new motion plan request.");
00101 
00102 
00103   QHBoxLayout* mpButtonsLayout = new QHBoxLayout;
00104   QVBoxLayout* motionBoxLayout = new QVBoxLayout;
00105   motionBoxLayout->addWidget(motion_plan_tree_);
00106   motionBoxLayout->addWidget(selected_request_label_);
00107   motionBoxLayout->addWidget(mpButtons);
00108   mpButtonsLayout->addWidget(newMPRButton);
00109   mpButtonsLayout->addWidget(deleteMPRButton);
00110   mpButtons->setLayout(mpButtonsLayout);
00111   motionPlanBox->setLayout(motionBoxLayout);
00112 
00113   connect(deleteMPRButton, SIGNAL(clicked()), this, SLOT(deleteSelectedMotionPlan()));
00114   connect(newMPRButton, SIGNAL(clicked()), this, SLOT(createNewMotionPlanPressed()));
00115   connect(motion_plan_tree_, SIGNAL(itemSelectionChanged()), this, SLOT(motionPlanTableSelection()));
00116 
00117   return motionPlanBox;
00118 }
00119 
00120 QGroupBox *WarehouseViewer::createTrajectoryBox()
00121 {
00122   QGroupBox* trajectoryBox = new QGroupBox;
00123   trajectoryBox->setTitle("Trajectories");
00124 
00125   trajectory_tree_ = new QTreeWidget;
00126   trajectory_tree_->setColumnCount(1);
00127 
00128 
00129   QVBoxLayout* trajectoryBoxLayout = new QVBoxLayout;
00130   trajectoryBoxLayout->addWidget(trajectory_tree_);
00131   trajectoryBoxLayout->addWidget(createTrajectoryControlsBox());
00132   trajectoryBoxLayout->addWidget(createTrajectoryInfoBox());
00133   trajectoryBox->setLayout(trajectoryBoxLayout);
00134 
00135   connect(trajectory_tree_, SIGNAL(itemSelectionChanged()), this, SLOT(trajectoryTableSelection()));
00136 
00137   return trajectoryBox;
00138 }
00139 
00140 QGroupBox *WarehouseViewer::createTrajectoryControlsBox()
00141 {
00142   QGroupBox* controlsBox = new QGroupBox;
00143   controlsBox->setTitle("Trajectory Controls");
00144 
00145   QWidget* buttonsWidget = new QWidget;
00146 
00147   QWidget* playbackWidget = new QWidget;
00148 
00149   trajectory_slider_ = new QSlider(Qt::Horizontal);
00150   trajectory_slider_->setMinimum(0);
00151   trajectory_slider_->setMaximum(0);
00152 
00153   play_button_ = new QPushButton;
00154   play_button_->setText("Play Trajectory");
00155   play_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00156   play_button_->setToolTip("Plays the currently selected trajectory in RVIZ. Makes the trajectory visible.");
00157 
00158   filter_button_ = new QPushButton;
00159   filter_button_->setText("Filter Trajectory");
00160   filter_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00161   filter_button_->setToolTip("Sends the currently selected trajectory to the trajectory filter, producing a new trajectory.");
00162   if(params_.trajectory_filter_1_service_name_ == "none")
00163   {
00164     filter_button_->setEnabled(false);
00165   }
00166 
00167   trajectory_point_edit_ = new QSpinBox;
00168   trajectory_point_edit_->setRange(0, 0);
00169   trajectory_point_edit_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00170   trajectory_point_edit_->setToolTip("Currently displayed trajectory point. Drag to change.");
00171 
00172   replan_button_ = new QPushButton;
00173   replan_button_->setText("Plan New Trajectory");
00174   replan_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00175   replan_button_->setToolTip("Plans a new trajectory between the start and goal states of the current motion plan request, producing a new trajectory.");
00176   if(  (active_planner_index_==1 && params_.planner_1_service_name_ == "none")
00177     || (active_planner_index_==2 && params_.planner_2_service_name_ == "none") )
00178   {
00179     replan_button_->setEnabled(false);
00180   }
00181 
00182   execute_button_ = new QPushButton;
00183   execute_button_->setText("Execute On Robot");
00184   execute_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00185   execute_button_->setEnabled(params_.use_robot_data_);
00186   execute_button_->setToolTip("Sends the currently selected trajectory to the robot's controllers. (Only works if using robot data). Produces a new trajectory.");
00187 
00188   QPushButton* deleteTrajectoryButton = new QPushButton;
00189   deleteTrajectoryButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00190   deleteTrajectoryButton->setText("Delete Trajectory");
00191   deleteTrajectoryButton->setToolTip("Deletes the currently selected trajectory.");
00192 
00193 
00194   QVBoxLayout* controlsBoxLayout = new QVBoxLayout;
00195   QHBoxLayout* buttonLayout = new QHBoxLayout;
00196   QHBoxLayout* playbackLayout = new QHBoxLayout;
00197 
00198   playbackLayout->addWidget(play_button_);
00199   playbackLayout->addWidget(trajectory_slider_);
00200   playbackLayout->addWidget(trajectory_point_edit_);
00201   buttonLayout->addWidget(replan_button_);
00202   buttonLayout->addWidget(filter_button_);
00203   buttonLayout->addWidget(execute_button_);
00204   buttonLayout->addWidget(deleteTrajectoryButton);
00205   controlsBoxLayout->addWidget(playbackWidget);
00206   controlsBoxLayout->addWidget(buttonsWidget);
00207 
00208   playbackWidget->setLayout(playbackLayout);
00209   buttonsWidget->setLayout(buttonLayout);
00210   controlsBox->setLayout(controlsBoxLayout);
00211 
00212   connect(play_button_, SIGNAL(clicked()), this, SLOT(playButtonPressed()));
00213   connect(filter_button_, SIGNAL(clicked()), this, SLOT(filterButtonPressed()));
00214   connect(trajectory_slider_, SIGNAL(valueChanged(int)), this, SLOT(trajectorySliderChanged(int)));
00215   connect(replan_button_, SIGNAL(clicked()), this, SLOT(replanButtonPressed()));
00216   connect(trajectory_point_edit_, SIGNAL(valueChanged(int)), this, SLOT(trajectoryEditChanged()));
00217   connect(execute_button_, SIGNAL(clicked()), this, SLOT(executeButtonPressed()));
00218   connect(deleteTrajectoryButton, SIGNAL(clicked()), this, SLOT(deleteSelectedTrajectory()));
00219 
00220   return controlsBox;
00221 }
00222 
00223 QGroupBox *WarehouseViewer::createTrajectoryInfoBox()
00224 {
00225   QGroupBox* trajectory_info_box = new QGroupBox;
00226   trajectory_info_box->setTitle("Trajectory Info");
00227 
00228   QLabel* selected_trajectory_title = new QLabel;
00229   selected_trajectory_title->setText("Selected Trajectory:           "); // Extra space used for buffer
00230   selected_trajectory_title->setToolTip("Selected trajectory.");
00231 
00232   QLabel* selected_trajectory_source_title = new QLabel;
00233   selected_trajectory_source_title->setText("Source: ");
00234   selected_trajectory_source_title->setToolTip("Source of the trajectory.");
00235 
00236   selected_trajectory_error_title_ = new QLabel;
00237   selected_trajectory_error_title_->setText("");
00238 
00239   selected_trajectory_stat_1_title_ = new QLabel;
00240   selected_trajectory_stat_2_title_ = new QLabel;
00241   selected_trajectory_stat_3_title_ = new QLabel;
00242   selected_trajectory_stat_4_title_ = new QLabel;
00243   selected_trajectory_stat_5_title_ = new QLabel;
00244 
00245   selected_trajectory_label_ = new QLabel(this);
00246   selected_trajectory_label_->setText("None");
00247 
00248   selected_trajectory_source_label_ = new QLabel(this);
00249   selected_trajectory_source_label_->setText("");
00250 
00251   selected_trajectory_error_label_ = new QLabel(this);
00252   selected_trajectory_error_label_->setText("");
00253 
00254   selected_trajectory_stat_0_title_ = new QLabel(this);
00255   selected_trajectory_stat_0_title_->setText("");
00256 
00257   selected_trajectory_stat_0_label_ = new QLabel(this);
00258   selected_trajectory_stat_0_label_->setText("");
00259 
00260   selected_trajectory_stat_1_label_ = new QLabel(this);
00261   selected_trajectory_stat_1_label_->setText("");
00262 
00263   selected_trajectory_stat_2_label_ = new QLabel(this);
00264   selected_trajectory_stat_2_label_->setText("");
00265 
00266   selected_trajectory_stat_3_label_ = new QLabel(this);
00267   selected_trajectory_stat_3_label_->setText("");
00268 
00269   selected_trajectory_stat_4_label_ = new QLabel(this);
00270   selected_trajectory_stat_4_label_->setText("");
00271 
00272   selected_trajectory_stat_5_label_ = new QLabel(this);
00273   selected_trajectory_stat_5_label_->setText("");
00274 
00275   // Generic Layout!
00276   trajectory_info_box_layout_ = new QFormLayout;
00277   trajectory_info_box_layout_->addRow(selected_trajectory_title,selected_trajectory_label_);
00278   trajectory_info_box_layout_->addRow(selected_trajectory_source_title,selected_trajectory_source_label_);
00279   trajectory_info_box_layout_->addRow(selected_trajectory_error_title_,selected_trajectory_error_label_);
00280   trajectory_info_box_layout_->addRow(selected_trajectory_stat_0_title_,selected_trajectory_stat_0_label_);
00281   trajectory_info_box_layout_->addRow(selected_trajectory_stat_3_title_,selected_trajectory_stat_3_label_);
00282   trajectory_info_box_layout_->addRow(selected_trajectory_stat_1_title_,selected_trajectory_stat_1_label_);
00283   trajectory_info_box_layout_->addRow(selected_trajectory_stat_2_title_,selected_trajectory_stat_2_label_);
00284   trajectory_info_box_layout_->addRow(selected_trajectory_stat_4_title_,selected_trajectory_stat_4_label_);
00285   trajectory_info_box_layout_->addRow(selected_trajectory_stat_5_title_,selected_trajectory_stat_5_label_);
00286   trajectory_info_box->setLayout(trajectory_info_box_layout_);
00287 
00288   return trajectory_info_box;
00289 }
00290 
00291 std::string WarehouseViewer::intToString(int val)
00292 {
00293   std::stringstream ss;
00294   ss << val;
00295   return ss.str();
00296 }
00297 
00298 std::string WarehouseViewer::floatToString(double val)
00299 {
00300   std::stringstream ss;
00301   ss << val;
00302   return ss.str();
00303 }
00304 
00305 void WarehouseViewer::setCommonTrajectoryInfo()
00306 {
00307   selected_trajectory_error_title_->setText("");
00308   selected_trajectory_error_title_->setToolTip("");
00309   selected_trajectory_error_label_->setText("");
00310   selected_trajectory_stat_0_title_->setText("");
00311   selected_trajectory_stat_0_title_->setMinimumWidth(100);
00312   selected_trajectory_stat_0_label_->setText("");
00313   selected_trajectory_stat_1_title_->setText("");
00314   selected_trajectory_stat_1_title_->setToolTip("");
00315   selected_trajectory_stat_1_label_->setText("");
00316   selected_trajectory_stat_2_title_->setText("");
00317   selected_trajectory_stat_2_title_->setToolTip("");
00318   selected_trajectory_stat_2_label_->setText("");
00319   selected_trajectory_stat_3_title_->setText("");
00320   selected_trajectory_stat_3_title_->setToolTip("");
00321   selected_trajectory_stat_3_label_->setText("");
00322   selected_trajectory_stat_4_title_->setText("");
00323   selected_trajectory_stat_4_title_->setToolTip("");
00324   selected_trajectory_stat_4_label_->setText("");
00325   selected_trajectory_stat_5_title_->setText("");
00326   selected_trajectory_stat_5_title_->setToolTip("");
00327   selected_trajectory_stat_5_label_->setText("");
00328 }
00329 void WarehouseViewer::setPlannedTrajectoryInfo(bool success, planning_scene_utils::TrajectoryData& trajectory)
00330 {
00331   TrajectoryStats trajectory_stats(trajectory.getTrajectory());
00332   MotionPlanRequestData& motion_plan_req = motion_plan_map_[ selected_motion_plan_name_ ];
00333 
00334   selected_trajectory_error_title_->setText("Error Code: ");
00335   selected_trajectory_error_title_->setToolTip("Error code returned by service request.");
00336   selected_trajectory_stat_0_title_->setText("Plannning Time:");
00337   selected_trajectory_stat_0_title_->setToolTip("Time to service the trajectory planning request.");
00338   selected_trajectory_stat_0_label_->setText(QString::fromStdString(floatToString(trajectory.getDuration().toSec())+" seconds"));
00339   selected_trajectory_stat_1_title_->setText("Angular Movement: ");
00340   selected_trajectory_stat_1_title_->setToolTip("Sum of angular movement of all joints.");
00341   selected_trajectory_stat_2_title_->setText("Cartesian Distance: ");
00342   selected_trajectory_stat_2_title_->setToolTip("Cartesian distance travelled by the end-effector.");
00343 
00344   selected_trajectory_error_label_->setText(QString::fromStdString(
00345         armNavigationErrorCodeToString(trajectory.trajectory_error_code_) +
00346         " (" + intToString(trajectory.trajectory_error_code_.val).c_str() + ")" ) );
00347   selected_trajectory_stat_1_label_->setText(QString::fromStdString(
00348         floatToString(trajectory_stats.getAngularDistance()) + " radians" ));
00349   selected_trajectory_stat_2_label_->setText(QString::fromStdString(
00350         floatToString(trajectory_stats.getCartesianDistance(motion_plan_req)) +" meters"));
00351 
00352 }
00353 void WarehouseViewer::setFilteredTrajectoryInfo(bool success, planning_scene_utils::TrajectoryData& trajectory)
00354 {
00355   TrajectoryStats trajectory_stats(trajectory.getTrajectory());
00356   MotionPlanRequestData& motion_plan_req = motion_plan_map_[ selected_motion_plan_name_ ];
00357 
00358   selected_trajectory_error_title_->setText("Error Code: ");
00359   selected_trajectory_error_title_->setToolTip("Error code returned by service request.");
00360   selected_trajectory_stat_0_title_->setText("Filtering Time:");
00361   selected_trajectory_stat_0_title_->setToolTip("Time to service the trajectory filter request.");
00362   selected_trajectory_stat_0_label_->setText(QString::fromStdString(floatToString(trajectory.getDuration().toSec())+" seconds"));
00363   selected_trajectory_stat_1_title_->setText("Angular Movement: ");
00364   selected_trajectory_stat_1_title_->setToolTip("Sum of angular movement of all joints.");
00365   selected_trajectory_stat_2_title_->setText("Cartesian Distance: ");
00366   selected_trajectory_stat_2_title_->setToolTip("Cartesian distance travelled by the end-effector.");
00367   selected_trajectory_stat_3_title_->setText("Trajectory Length: ");
00368   selected_trajectory_stat_3_title_->setToolTip("Expected time to execute the trajectory.");
00369 
00370   selected_trajectory_error_label_->setText(QString::fromStdString(
00371         armNavigationErrorCodeToString(trajectory.trajectory_error_code_) +
00372         " (" + intToString(trajectory.trajectory_error_code_.val) + ")" ) );
00373   selected_trajectory_stat_1_label_->setText(QString::fromStdString(
00374         floatToString(trajectory_stats.getAngularDistance()) + " radians" ));
00375   selected_trajectory_stat_2_label_->setText(QString::fromStdString(
00376         floatToString(trajectory_stats.getCartesianDistance(motion_plan_req)) +" meters"));
00377   selected_trajectory_stat_3_label_->setText(QString::fromStdString(
00378         floatToString(trajectory_stats.getExecutionDuration().toSec()) +" seconds"));
00379 }
00380 void WarehouseViewer::setExecutedTrajectoryInfo(bool success, planning_scene_utils::TrajectoryData& trajectory)
00381 {
00382   TrajectoryStats trajectory_stats(trajectory.getTrajectory());
00383   MotionPlanRequestData& motion_plan_req = motion_plan_map_[ selected_motion_plan_name_ ];
00384 
00385   selected_trajectory_error_title_->setText("Error Code: ");
00386   selected_trajectory_error_title_->setToolTip("Error code returned by service request.");
00387   selected_trajectory_stat_0_title_->setText("Execution Time:");
00388   selected_trajectory_stat_0_title_->setToolTip("Time to service the trajectory execution request.");
00389   selected_trajectory_stat_0_label_->setText(QString::fromStdString(floatToString(trajectory.getDuration().toSec())+" seconds"));
00390   selected_trajectory_stat_1_title_->setText("Angular Movement: ");
00391   selected_trajectory_stat_1_title_->setToolTip("Sum of angular movement of all joints.");
00392   selected_trajectory_stat_2_title_->setText("Cartesian Distance: ");
00393   selected_trajectory_stat_2_label_->setToolTip("Currently selected trajectory cartesian distance.");
00394   selected_trajectory_stat_3_title_->setText("Trajectory Length: ");
00395   selected_trajectory_stat_3_title_->setToolTip("Actual time to execute the trajectory.");
00396   selected_trajectory_stat_4_title_->setText("Max Controller Error: ");
00397   selected_trajectory_stat_4_title_->setToolTip("Max of the angular errors of the controller during execution of the trajectory.");
00398 
00399   selected_trajectory_error_label_->setText(QString::fromStdString(
00400         getResultErrorFromCode(trajectory.trajectory_error_code_.val) +
00401         " (" + intToString(trajectory.trajectory_error_code_.val) + ")" ) );
00402   selected_trajectory_stat_1_label_->setText(QString::fromStdString(
00403         floatToString(trajectory_stats.getAngularDistance()) + " radians" ));
00404   selected_trajectory_stat_2_label_->setText(QString::fromStdString(
00405         floatToString(trajectory_stats.getCartesianDistance(motion_plan_req)) +" meters"));
00406   selected_trajectory_stat_3_label_->setText(QString::fromStdString(
00407         floatToString(trajectory_stats.getExecutionDuration().toSec()) +" seconds"));
00408   selected_trajectory_stat_4_label_->setText(QString::fromStdString(
00409         floatToString(trajectory_stats.getMaxAngularError(trajectory.getTrajectoryError())) +" radians"));
00410 }
00411 
00412 void WarehouseViewer::setOvershootTrajectoryInfo(bool success, planning_scene_utils::TrajectoryData& trajectory)
00413 {
00414   TrajectoryStats trajectory_stats(trajectory.getTrajectory());
00415   MotionPlanRequestData& motion_plan_req = motion_plan_map_[ selected_motion_plan_name_ ];
00416 
00417   //selected_trajectory_stat_0_title_->setToolTip("Time to service the trajectory execution request.");
00418   selected_trajectory_stat_1_title_->setText("Angular Movement: ");
00419   selected_trajectory_stat_1_title_->setToolTip("Sum of angular movement of all joints.");
00420   selected_trajectory_stat_2_title_->setText("Cartesian Distance: ");
00421   selected_trajectory_stat_2_label_->setToolTip("Currently selected trajectory cartesian distance.");
00422   selected_trajectory_stat_3_title_->setText("Trajectory Length: ");
00423   selected_trajectory_stat_3_title_->setToolTip(".");
00424   selected_trajectory_stat_4_title_->setText("Max Controller Error: ");
00425   selected_trajectory_stat_4_title_->setToolTip("Max of the angular errors of the controller during execution of the trajectory.");
00426 
00427   //selected_trajectory_error_label_->setText(QString::fromStdString(
00428   //      getResultErrorFromCode(trajectory.trajectory_error_code_.val) +
00429   //      " (" + intToString(trajectory.trajectory_error_code_.val) + ")" ) );
00430   selected_trajectory_stat_1_label_->setText(QString::fromStdString(
00431         floatToString(trajectory_stats.getAngularDistance()) + " radians" ));
00432   selected_trajectory_stat_2_label_->setText(QString::fromStdString(
00433         floatToString(trajectory_stats.getCartesianDistance(motion_plan_req)) +" meters"));
00434   selected_trajectory_stat_3_label_->setText("");
00435   selected_trajectory_stat_3_label_->setText(QString::fromStdString(
00436         floatToString(trajectory_stats.getExecutionDuration().toSec()) +" seconds"));
00437   selected_trajectory_stat_4_label_->setText(QString::fromStdString(
00438         floatToString(trajectory_stats.getMaxAngularError(trajectory.getTrajectoryError())) +" radians"));
00439 }
00440 
00441 void WarehouseViewer::initQtWidgets()
00442 {
00443   menu_bar_ = new QMenuBar(this);
00444   setMenuBar(menu_bar_);
00445   QWidget* centralWidget = new QWidget(this);
00446   setCentralWidget(centralWidget);
00447   centralWidget->setMinimumWidth(500);
00448 
00449   QVBoxLayout* layout = new QVBoxLayout(this);
00450   layout->addWidget(createMotionPlanBox());
00451   layout->addWidget(createTrajectoryBox());
00452 
00453   file_menu_ = menu_bar_->addMenu("File");
00454   planning_scene_menu_ = menu_bar_->addMenu("Planning Scene");
00455 
00456   new_planning_scene_action_ = file_menu_->addAction("New Planning Scene ...");
00457   load_planning_scene_action_ = file_menu_->addAction("Load Planning Scene ...");
00458   save_planning_scene_action_ = file_menu_->addAction("Save Planning Scene ...");
00459   copy_planning_scene_action_ = file_menu_->addAction("Save Copy of Planning Scene ...");
00460   new_motion_plan_action_ = file_menu_->addAction("New Motion Plan Request ...");
00461   refresh_action_ = planning_scene_menu_->addAction("Refresh Planning Scene...");
00462   view_outcomes_action_ = planning_scene_menu_->addAction("View Outcomes ...");
00463   alter_link_padding_action_ = planning_scene_menu_->addAction("Alter Link Padding ...");
00464   alter_allowed_collision_action_ = planning_scene_menu_->addAction("Alter Allowed Collision Operations ...");
00465   edit_robot_state_action_ = planning_scene_menu_->addAction("Edit Robot State ...");
00466   quit_action_ = file_menu_->addAction("Quit");
00467 
00468   collision_object_menu_ = menu_bar_->addMenu("Collision Objects");
00469   new_object_action_ = collision_object_menu_->addAction("New Primitive Collision Object ...");
00470   new_mesh_action_ = collision_object_menu_->addAction("New Mesh Collision Object ...");
00471 
00472   planner_configuration_menu_ = menu_bar_->addMenu("Planner configuration");
00473   set_primary_planner_action_ = planner_configuration_menu_->addAction("Use primary planner");
00474   set_primary_planner_action_->setCheckable(true);
00475   set_primary_planner_action_->setChecked(true);
00476   set_secondary_planner_action_ = planner_configuration_menu_->addAction("Use secondary planner");
00477   set_secondary_planner_action_->setCheckable(true);
00478   set_secondary_planner_action_->setChecked(false);
00479   set_primary_filter_action_ = planner_configuration_menu_->addAction("Use primary filter");
00480   set_primary_filter_action_->setCheckable(true);
00481   set_primary_filter_action_->setChecked(true);
00482   set_secondary_filter_action_ = planner_configuration_menu_->addAction("Use secondary filter");
00483   set_secondary_filter_action_->setCheckable(true);
00484   set_secondary_filter_action_->setChecked(false);
00485 
00486 
00487   connect(new_planning_scene_action_, SIGNAL(triggered()), this, SLOT(createNewPlanningSceneSlot()));
00488   connect(new_motion_plan_action_, SIGNAL(triggered()), this, SLOT(createNewMotionPlanPressed()));
00489   connect(new_object_action_, SIGNAL(triggered()), this, SLOT(createNewObjectPressed()));
00490   connect(new_mesh_action_, SIGNAL(triggered()), this, SLOT(createNewMeshPressed()));
00491   connect(save_planning_scene_action_, SIGNAL(triggered()), this, SLOT(savePlanningSceneSlot()));
00492   connect(copy_planning_scene_action_, SIGNAL(triggered()), this, SLOT(copyPlanningSceneSlot()));
00493   connect(load_planning_scene_action_, SIGNAL(triggered()), this, SLOT(popupLoadPlanningScenes()));
00494   connect(quit_action_, SIGNAL(triggered()), this, SLOT(quit()));
00495   connect(this, SIGNAL(updateTables()), this, SLOT(updateStateTriggered()));
00496   connect(refresh_action_, SIGNAL(triggered()), this, SLOT(refreshSceneButtonPressed()));
00497   connect(view_outcomes_action_, SIGNAL(triggered()), this, SLOT(viewOutcomesPressed()));
00498   connect(alter_link_padding_action_, SIGNAL(triggered()), this, SLOT(alterLinkPaddingPressed()));
00499   connect(alter_allowed_collision_action_, SIGNAL(triggered()), this, SLOT(alterAllowedCollisionPressed()));
00500   connect(edit_robot_state_action_, SIGNAL(triggered()), this, SLOT(editRobotStatePressed()));
00501   connect(this, SIGNAL(plannerFailure(int)),this, SLOT(popupPlannerFailure(int)));
00502   connect(this, SIGNAL(filterFailure(int)),this, SLOT(popupFilterFailure(int)));
00503   connect(this, SIGNAL(attachObjectSignal(const std::string&)), this, SLOT(attachObject(const std::string&)));
00504   connect(this, SIGNAL(allScenesLoaded()), this, SLOT(refreshPlanningSceneDialog()));
00505   connect(set_primary_planner_action_, SIGNAL(triggered()), this, SLOT(primaryPlannerTriggered()));
00506   connect(set_secondary_planner_action_, SIGNAL(triggered()), this, SLOT(secondaryPlannerTriggered()));
00507   connect(set_primary_filter_action_, SIGNAL(triggered()), this, SLOT(primaryFilterTriggered()));
00508   connect(set_secondary_filter_action_, SIGNAL(triggered()), this, SLOT(secondaryFilterTriggered()));
00509   connect(this, SIGNAL(selectedTrajectoryPointChanged(unsigned int)), this, SLOT(onSelectedTrajectoryPointChanged(unsigned int)));
00510   load_planning_scene_dialog_ = new QDialog(this);
00511 
00512   setupPlanningSceneDialog();
00513   connect(this, SIGNAL(changeProgress(int)), load_scene_progress_, SLOT(setValue(int)));
00514   menu_bar_->setMinimumWidth(500);
00515   centralWidget->setLayout(layout);
00516 
00517   createNewObjectDialog();
00518   createNewMeshDialog();
00519   createRequestDialog();
00520 
00521   //setCurrentPlanningScene(createNewPlanningScene(), false, false);
00522 
00523 }
00524 
00525 void WarehouseViewer::createOutcomeDialog()
00526 {
00527   outcome_dialog_ = new QDialog(this);
00528   outcome_dialog_->setWindowTitle("Planning Scene Outcomes");
00529   QVBoxLayout* layout = new QVBoxLayout(outcome_dialog_);
00530 
00531   QGroupBox* stagesBox = new QGroupBox(outcome_dialog_);
00532   stagesBox->setTitle("Planning Stage Outcomes");
00533 
00534   QVBoxLayout* stagesLayout = new QVBoxLayout(stagesBox);
00535   stagesBox->setLayout(stagesLayout);
00536 
00537   stage_outcome_table_ = new QTableWidget(stagesBox);
00538   stagesLayout->addWidget(stage_outcome_table_);
00539 
00540   QStringList stageHeaders;
00541   stageHeaders.append("Pipeline Stage");
00542   stageHeaders.append("Outcome");
00543 
00544   stage_outcome_table_->setColumnCount(2);
00545   stage_outcome_table_->setRowCount((int)error_map_.size());
00546 
00547   stage_outcome_table_->setHorizontalHeaderLabels(stageHeaders);
00548   stage_outcome_table_->setColumnWidth(0, 250);
00549   stage_outcome_table_->setColumnWidth(1, 400);
00550   stage_outcome_table_->setMinimumWidth(650);
00551 
00552   int r = 0;
00553   for(map<string, ArmNavigationErrorCodes>::iterator it = error_map_.begin(); it != error_map_.end(); it++, r++)
00554   {
00555     QTableWidgetItem* stageItem = new QTableWidgetItem(QString::fromStdString(it->first));
00556     stageItem->setFlags(Qt::ItemIsEnabled);
00557     stage_outcome_table_->setItem(r,0, stageItem);
00558 
00559     QTableWidgetItem* outcomeItem = new QTableWidgetItem(QString::fromStdString(armNavigationErrorCodeToString(it->second)));
00560     outcomeItem->setFlags(Qt::ItemIsEnabled);
00561     stage_outcome_table_->setItem(r,1, outcomeItem);
00562 
00563     if(it->second.val != ArmNavigationErrorCodes::SUCCESS)
00564     {
00565       outcomeItem->setTextColor(QColor(150, 0, 0));
00566     }
00567 
00568   }
00569   QGroupBox* trajectoryBox = new QGroupBox(outcome_dialog_);
00570   trajectoryBox->setTitle("Trajectory Outcomes");
00571 
00572   QVBoxLayout* trajectoryLayout = new QVBoxLayout(trajectoryBox);
00573   trajectoryBox->setLayout(trajectoryLayout);
00574 
00575   trajectory_outcome_table_ = new QTableWidget(trajectoryBox);
00576   trajectoryLayout->addWidget(trajectory_outcome_table_);
00577 
00578   QStringList trajectoryHeaders;
00579   trajectoryHeaders.append("MPR ID");
00580   trajectoryHeaders.append("Trajectory ID");
00581   trajectoryHeaders.append("Source");
00582   trajectoryHeaders.append("Outcome");
00583 
00584   unsigned int count = 0;
00585   for(map<string, map<string, TrajectoryData> >::iterator it = trajectory_map_.begin();
00586       it != trajectory_map_.end();
00587       it++) {
00588     count += it->second.size();
00589   }
00590 
00591   trajectory_outcome_table_->setColumnCount(4);
00592   trajectory_outcome_table_->setRowCount(count);
00593   trajectory_outcome_table_->setColumnWidth(0, 150);
00594   trajectory_outcome_table_->setColumnWidth(1, 150);
00595   trajectory_outcome_table_->setColumnWidth(2, 200);
00596   trajectory_outcome_table_->setColumnWidth(3, 200);
00597   trajectory_outcome_table_->setMinimumWidth(700);
00598 
00599   r = 0;
00600   for(map<string, map<string, TrajectoryData> >::iterator it = trajectory_map_.begin();
00601       it != trajectory_map_.end();
00602       it++) {
00603     
00604     for(map<string, TrajectoryData>::iterator it2 = it->second.begin(); it2 != it->second.end(); it2++, r++)
00605     {
00606       QTableWidgetItem* motionPlanRequestItem = new QTableWidgetItem(QString::fromStdString(it->first));
00607       motionPlanRequestItem->setFlags(Qt::ItemIsEnabled);
00608       trajectory_outcome_table_->setItem(r, 0, motionPlanRequestItem);
00609 
00610       QTableWidgetItem* trajectoryItem = new QTableWidgetItem(QString::fromStdString(it2->first));
00611       trajectoryItem->setFlags(Qt::ItemIsEnabled);
00612       trajectory_outcome_table_->setItem(r, 1, trajectoryItem);
00613       
00614       QTableWidgetItem* sourceItem = new QTableWidgetItem(QString::fromStdString(it2->second.getSource()));
00615       sourceItem->setFlags(Qt::ItemIsEnabled);
00616       trajectory_outcome_table_->setItem(r, 2, sourceItem);
00617       
00618       QTableWidgetItem* outcomeItem = new QTableWidgetItem(QString::fromStdString(armNavigationErrorCodeToString(it2->second.trajectory_error_code_)));
00619       outcomeItem ->setFlags(Qt::ItemIsEnabled);
00620       trajectory_outcome_table_->setItem(r, 3, outcomeItem);
00621 
00622       if(it2->second.trajectory_error_code_.val != ArmNavigationErrorCodes::SUCCESS)
00623       {
00624         outcomeItem->setTextColor(QColor(150, 0, 0));
00625         std::stringstream ss;
00626         ss << "Bad point: " << it2->second.getBadPoint();
00627         outcomeItem->setToolTip(QString::fromStdString(ss.str()));
00628       }
00629       
00630     }
00631   }
00632   layout->addWidget(stagesBox);
00633   layout->addWidget(trajectoryBox);
00634   outcome_dialog_->setLayout(layout);
00635 
00636 }
00637 
00638 void WarehouseViewer::viewOutcomesPressed()
00639 {
00640   createOutcomeDialog();
00641   outcome_dialog_->exec();
00642   delete outcome_dialog_;
00643 }
00644 
00645 void WarehouseViewer::createAlterLinkPaddingDialog()
00646 {
00647   if(current_planning_scene_name_ == "") {
00648     return;
00649   }
00650 
00651   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00652 
00653   if(planning_scene.link_padding.size() == 0) {
00654     planning_environment::convertFromLinkPaddingMapToLinkPaddingVector(cm_->getDefaultLinkPaddingMap(), planning_scene.link_padding);
00655     planning_scene_map_[current_planning_scene_name_].setPlanningScene(planning_scene);
00656   }
00657 
00658   alter_link_padding_dialog_ = new QDialog(this);
00659   alter_link_padding_dialog_->setWindowTitle("Alter Link Padding");
00660   QVBoxLayout* layout = new QVBoxLayout(alter_link_padding_dialog_);
00661   
00662   alter_link_padding_table_ = new QTableWidget(alter_link_padding_dialog_);
00663   QStringList alter_headers;
00664   alter_headers.append("Link name");
00665   alter_headers.append("Padding (m)");
00666   
00667   alter_link_padding_table_->setColumnCount(2);
00668   alter_link_padding_table_->setRowCount((int)planning_scene.link_padding.size());
00669   
00670   alter_link_padding_table_->setHorizontalHeaderLabels(alter_headers);
00671   alter_link_padding_table_->setColumnWidth(0, 300);
00672   alter_link_padding_table_->setColumnWidth(1, 150);
00673   alter_link_padding_table_->setMinimumWidth(500);
00674 
00675   for(unsigned int i = 0; i < planning_scene.link_padding.size(); i++) {
00676     QTableWidgetItem* link_item = new QTableWidgetItem(QString::fromStdString(planning_scene.link_padding[i].link_name));
00677     link_item->setFlags(Qt::ItemIsEnabled);
00678     alter_link_padding_table_->setItem(i, 0, link_item);
00679 
00680     QDoubleSpinBox* padding_spin_box = new QDoubleSpinBox(alter_link_padding_table_);
00681     padding_spin_box->setMinimum(0.0);
00682     padding_spin_box->setDecimals(3);
00683     padding_spin_box->setSingleStep(.005);
00684     padding_spin_box->setValue(planning_scene.link_padding[i].padding);
00685     connect(padding_spin_box, SIGNAL(valueChanged(double)), this, SLOT(alteredLinkPaddingValueChanged(double)));
00686 
00687     alter_link_padding_table_->setCellWidget(i, 1, padding_spin_box);
00688   }
00689   layout->addWidget(alter_link_padding_table_);
00690   alter_link_padding_dialog_->setLayout(layout);
00691 }
00692 
00693 void WarehouseViewer::alterLinkPaddingPressed()
00694 {
00695   createAlterLinkPaddingDialog();
00696   alter_link_padding_dialog_->exec();
00697   delete alter_link_padding_dialog_;
00698 }
00699 
00700 void WarehouseViewer::alteredLinkPaddingValueChanged(double d)
00701 {
00702   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00703   planning_scene.link_padding.clear();
00704 
00705   for(int i = 0; i < alter_link_padding_table_->rowCount(); i++)
00706   {
00707     arm_navigation_msgs::LinkPadding lp;
00708     lp.link_name = alter_link_padding_table_->item(i,0)->text().toStdString();
00709     lp.padding = dynamic_cast<QDoubleSpinBox*>(alter_link_padding_table_->cellWidget(i,1))->value();
00710     planning_scene.link_padding.push_back(lp);
00711   }
00712   planning_scene_map_[current_planning_scene_name_].setPlanningScene(planning_scene);
00713   sendPlanningScene(planning_scene_map_[current_planning_scene_name_]);
00714 }
00715 
00716 void WarehouseViewer::alterAllowedCollisionPressed()
00717 {
00718   createAlterAllowedCollisionDialog();
00719   alter_allowed_collision_dialog_->exec();
00720   delete alter_allowed_collision_dialog_;
00721 }
00722 
00723 void WarehouseViewer::createAlterAllowedCollisionDialog()
00724 {
00725   if(current_planning_scene_name_ == "") {
00726     return;
00727   }
00728 
00729   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00730 
00731   if(planning_scene.allowed_collision_matrix.link_names.size() == 0) {
00732     planning_environment::convertFromACMToACMMsg(cm_->getDefaultAllowedCollisionMatrix(), planning_scene.allowed_collision_matrix);
00733     planning_scene_map_[current_planning_scene_name_].setPlanningScene(planning_scene);
00734   }
00735 
00736   alter_allowed_collision_dialog_ = new QDialog(this);
00737   alter_allowed_collision_dialog_->setWindowTitle("Alter Allowed Collision");
00738 
00739   QGridLayout* layout = new QGridLayout(alter_allowed_collision_dialog_);
00740   
00741   QLabel* col_1_label = new QLabel(alter_allowed_collision_dialog_);
00742   col_1_label->setText("First entity");
00743 
00744   QLabel* col_2_label = new QLabel(alter_allowed_collision_dialog_);
00745   col_2_label->setText("Second entity");
00746 
00747   QLabel* col_3_label = new QLabel(alter_allowed_collision_dialog_);
00748   col_3_label->setText("Status");
00749 
00750   QLabel* col_4_label = new QLabel(alter_allowed_collision_dialog_);
00751   col_4_label->setText("Operation");
00752 
00753   layout->addWidget(col_1_label, 0, 0);
00754   layout->addWidget(col_2_label, 0, 1);
00755   layout->addWidget(col_3_label, 0, 2);
00756   layout->addWidget(col_4_label, 0, 3);
00757 
00758   first_allowed_collision_line_edit_ = new QLineEdit(alter_allowed_collision_dialog_);
00759   second_allowed_collision_line_edit_ = new QLineEdit(alter_allowed_collision_dialog_);
00760 
00761   layout->addWidget(first_allowed_collision_line_edit_, 1, 0);
00762   layout->addWidget(second_allowed_collision_line_edit_, 1, 1);
00763 
00764   connect(first_allowed_collision_line_edit_,SIGNAL(textEdited(const QString&)), this, SLOT(entityListsEdited()));
00765   connect(second_allowed_collision_line_edit_,SIGNAL(textEdited(const QString&)), this, SLOT(entityListsEdited()));
00766 
00767   first_allowed_collision_list_ = new QListWidget(alter_allowed_collision_dialog_);
00768   second_allowed_collision_list_ = new QListWidget(alter_allowed_collision_dialog_);
00769   
00770   for(unsigned int i = 0; i < planning_scene.allowed_collision_matrix.link_names.size(); i++) {
00771     if(cm_->getKinematicModel()->hasLinkModel(planning_scene.allowed_collision_matrix.link_names[i])) {
00772       first_allowed_collision_list_->addItem(planning_scene.allowed_collision_matrix.link_names[i].c_str());
00773       second_allowed_collision_list_->addItem(planning_scene.allowed_collision_matrix.link_names[i].c_str());
00774     }
00775   }
00776   addSpecialEntries(first_allowed_collision_list_);
00777   addSpecialEntries(second_allowed_collision_list_);
00778 
00779   connect(first_allowed_collision_list_,SIGNAL(itemSelectionChanged()), this, SLOT(firstEntityListSelected()));
00780   connect(second_allowed_collision_list_,SIGNAL(itemSelectionChanged()), this, SLOT(secondEntityListSelected()));
00781 
00782 
00783   layout->addWidget(first_allowed_collision_list_, 2, 0);
00784   layout->addWidget(second_allowed_collision_list_, 2, 1);
00785 
00786   allowed_status_line_edit_ = new QLineEdit(alter_allowed_collision_dialog_);
00787   allowed_status_line_edit_->setMinimumWidth(125);
00788   allowed_status_line_edit_->setMaximumWidth(125);
00789   allowed_status_line_edit_->setAlignment(Qt::AlignCenter);
00790   layout->addWidget(allowed_status_line_edit_, 1, 2);
00791   
00792   QPushButton* enable_collision_button = new QPushButton(alter_allowed_collision_dialog_);
00793   enable_collision_button->setText("Enable");
00794   enable_collision_button->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
00795 
00796   QPushButton* disable_collision_button = new QPushButton(alter_allowed_collision_dialog_);
00797   disable_collision_button->setText("Disable");
00798   disable_collision_button->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
00799 
00800   connect(enable_collision_button, SIGNAL(clicked()), this, SLOT(enableCollisionClicked()));
00801   connect(disable_collision_button, SIGNAL(clicked()), this, SLOT(disableCollisionClicked()));
00802   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00803   
00804   QPushButton* reset_default_button = new QPushButton(alter_allowed_collision_dialog_);
00805   reset_default_button->setText("Reset");
00806   reset_default_button->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
00807 
00808   connect(reset_default_button, SIGNAL(clicked()), this, SLOT(resetAllowedCollisionClicked()));
00809 
00810   layout->addWidget(enable_collision_button, 1, 3);
00811   layout->addWidget(disable_collision_button, 2, 3, Qt::AlignTop);
00812   layout->addWidget(reset_default_button, 2, 3, Qt::AlignBottom);
00813 
00814   alter_allowed_collision_dialog_->setLayout(layout);
00815 
00816   alter_allowed_collision_dialog_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Minimum);
00817 }
00818 
00819 void WarehouseViewer::addSpecialEntries(QListWidget* list_widget) {
00820   list_widget->addItem("------Groups---------");
00821   std::vector<std::string> group_names;
00822   cm_->getKinematicModel()->getModelGroupNames(group_names);
00823   for(unsigned int i = 0; i < group_names.size(); i++) {
00824     list_widget->addItem(group_names[i].c_str());
00825   }
00826   list_widget->addItem("------Objects---------");
00827   std::vector<std::string> object_names;
00828   cm_->getCollisionObjectNames(object_names);
00829   for(unsigned int i = 0; i < object_names.size(); i++) {
00830     list_widget->addItem(object_names[i].c_str());
00831   }
00832   list_widget->addItem("------Attached Objects---------");
00833   std::vector<std::string> attached_object_names;
00834   cm_->getAttachedCollisionObjectNames(attached_object_names);
00835   for(unsigned int i = 0; i < attached_object_names.size(); i++) {
00836     list_widget->addItem(attached_object_names[i].c_str());
00837   }
00838   list_widget->addItem("------Special-------");
00839   list_widget->addItem("COLLISION_SET_ALL");
00840   list_widget->addItem("COLLISION_SET_OBJECTS");
00841   list_widget->addItem("COLLISION_SET_ATTACHED_OBJECTS");
00842 }
00843 
00844 void WarehouseViewer::getEntryList(const std::string& s1, 
00845                                    std::vector<std::string>& sv1)
00846 {
00847   bool all = false;
00848   if(s1 == "COLLISION_SET_ALL") {
00849     all = true;
00850   } else if(s1 == "COLLISION_SET_OBJECTS") {
00851     cm_->getCollisionObjectNames(sv1);
00852   } else if(s1 == "COLLISION_SET_ATTACHED_OBJECTS") {
00853     cm_->getAttachedCollisionObjectNames(sv1);
00854   } else if(cm_->getKinematicModel()->hasModelGroup(s1)) {
00855     sv1 = cm_->getKinematicModel()->getModelGroup(s1)->getGroupLinkNames();
00856   } else {
00857     sv1.push_back(s1);
00858   }
00859   if(all) {
00860     collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getCurrentAllowedCollisionMatrix();
00861     acm.getAllEntryNames(sv1);
00862   }
00863 }
00864 
00865 void WarehouseViewer::resetAllowedCollisionClicked() 
00866 {
00867   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00868   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = cm_->getDefaultAllowedCollisionMatrix(); 
00869   
00870   planning_environment::convertFromACMToACMMsg(acm, planning_scene.allowed_collision_matrix);
00871   planning_scene_map_[current_planning_scene_name_].setPlanningScene(planning_scene);
00872   sendPlanningScene(planning_scene_map_[current_planning_scene_name_]);
00873   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00874 }
00875 
00876 void WarehouseViewer::firstEntityListSelected() {
00877   first_allowed_collision_line_edit_->setText(first_allowed_collision_list_->selectedItems()[0]->text());
00878   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00879 }
00880 
00881 void WarehouseViewer::secondEntityListSelected() {
00882   second_allowed_collision_line_edit_->setText(second_allowed_collision_list_->selectedItems()[0]->text());
00883   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00884 }
00885 
00886 void WarehouseViewer::entityListsEdited() {
00887   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00888 }
00889 
00890 void WarehouseViewer::enableCollisionClicked() {
00891   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00892   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = planning_environment::convertFromACMMsgToACM(planning_scene.allowed_collision_matrix);
00893   QString qs1 = first_allowed_collision_line_edit_->text();
00894   QString qs2 = second_allowed_collision_line_edit_->text();
00895 
00896   std::vector<std::string> first_list;
00897   std::vector<std::string> second_list;
00898 
00899   getEntryList(qs1.toStdString(), first_list); 
00900   getEntryList(qs2.toStdString(), second_list);
00901 
00902   bool all_enabled = true;
00903 
00904   for(unsigned int i = 0; i < first_list.size(); i++) {
00905     for(unsigned int j = 0; j < second_list.size(); j++) {
00906       bool allowed = false;
00907       if(acm.getAllowedCollision(first_list[i], second_list[j], allowed)) {
00908         if(allowed) {
00909           all_enabled = false;
00910           break;
00911         }
00912       }
00913     }
00914   }
00915   if(all_enabled) return;
00916   
00917   acm.changeEntry(first_list, second_list, false);
00918   planning_environment::convertFromACMToACMMsg(acm, planning_scene.allowed_collision_matrix);
00919   planning_scene_map_[current_planning_scene_name_].setPlanningScene(planning_scene);
00920   sendPlanningScene(planning_scene_map_[current_planning_scene_name_]);
00921   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00922 }
00923 
00924 void WarehouseViewer::disableCollisionClicked() {
00925   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00926   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = planning_environment::convertFromACMMsgToACM(planning_scene.allowed_collision_matrix);
00927   QString qs1 = first_allowed_collision_line_edit_->text();
00928   QString qs2 = second_allowed_collision_line_edit_->text();
00929 
00930   std::vector<std::string> first_list;
00931   std::vector<std::string> second_list;
00932 
00933   getEntryList(qs1.toStdString(), first_list); 
00934   getEntryList(qs2.toStdString(), second_list);
00935 
00936   bool all_disabled = true;
00937 
00938   for(unsigned int i = 0; i < first_list.size(); i++) {
00939     for(unsigned int j = 0; j < second_list.size(); j++) {
00940       bool allowed = false;
00941       if(acm.getAllowedCollision(first_list[i], second_list[j], allowed)) {
00942         if(!allowed) {
00943           all_disabled = false;
00944           break;
00945         }
00946       }
00947     }
00948   }
00949   
00950   if(all_disabled) return;
00951 
00952   acm.changeEntry(first_list, second_list, true);
00953   planning_environment::convertFromACMToACMMsg(acm, planning_scene.allowed_collision_matrix);
00954   planning_scene_map_[current_planning_scene_name_].setPlanningScene(planning_scene);
00955   sendPlanningScene(planning_scene_map_[current_planning_scene_name_]);
00956   setEnabledDisabledDisplay(first_allowed_collision_line_edit_->text(), second_allowed_collision_line_edit_->text());
00957 }
00958 
00959 void WarehouseViewer::setEnabledDisabledDisplay(const QString& qs1, const QString& qs2) {
00960   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
00961   collision_space::EnvironmentModel::AllowedCollisionMatrix acm = planning_environment::convertFromACMMsgToACM(planning_scene.allowed_collision_matrix);
00962   std::vector<std::string> first_list;
00963   std::vector<std::string> second_list;
00964   
00965   getEntryList(qs1.toStdString(), first_list); 
00966   getEntryList(qs2.toStdString(), second_list);
00967 
00968   bool all_disabled = true;
00969   bool all_enabled = true;
00970   bool some_found = false;
00971 
00972   for(unsigned int i = 0; i < first_list.size(); i++) {
00973     for(unsigned int j = 0; j < second_list.size(); j++) {
00974       bool allowed = false;
00975       if(acm.getAllowedCollision(first_list[i], second_list[j], allowed)) {
00976         some_found = true;
00977         if(allowed) {
00978           all_enabled = false;
00979         } else {
00980           all_disabled = false;
00981         }
00982       } 
00983     }
00984   }
00985   if(!some_found) {
00986     allowed_status_line_edit_->setText("NO ENTRY");
00987   } else if(!all_disabled && !all_enabled) {
00988     allowed_status_line_edit_->setText("MIXED");
00989   } else if(all_disabled) {
00990     allowed_status_line_edit_->setText("DISABLED");
00991   } else {
00992     allowed_status_line_edit_->setText("ENABLED");
00993   }
00994 }
00995 
00996 void WarehouseViewer::refreshSceneButtonPressed()
00997 {
00998   if(current_planning_scene_name_ != "" )
00999   {
01000     sendPlanningScene(planning_scene_map_[current_planning_scene_name_]);
01001   }
01002   else
01003   {
01004     QMessageBox msg(QMessageBox::Warning,"Refresh", "No planning scene loaded!");
01005     msg.addButton(QMessageBox::Ok);
01006     msg.exec();
01007   }
01008 }
01009 
01010 void WarehouseViewer::executeButtonPressed()
01011 {
01012   if(selected_trajectory_name_ != "")
01013   {
01014     executeTrajectory(selected_motion_plan_name_,
01015                       selected_trajectory_name_);
01016   }
01017   else
01018   {
01019     QMessageBox msg(QMessageBox::Warning, "Execute Trajectory", "No Trajectory Selected!");
01020     msg.addButton("Ok", QMessageBox::AcceptRole);
01021     msg.exec();
01022   }
01023 }
01024 
01025 void WarehouseViewer::deleteSelectedMotionPlan()
01026 {
01027   if(selected_motion_plan_name_ != "")
01028   {
01029     lockScene();
01030     std::vector<unsigned int> erased_trajectories;
01031     deleteMotionPlanRequest(motion_plan_map_[selected_motion_plan_name_].getId(), erased_trajectories);
01032     unlockScene();
01033   }
01034   else
01035   {
01036     QMessageBox msg(QMessageBox::Warning, "Delete Motion Plan Request", "No Motion Plan Request Selected!");
01037     msg.addButton("Ok", QMessageBox::AcceptRole);
01038     msg.exec();
01039   }
01040 }
01041 
01042 void WarehouseViewer::deleteSelectedTrajectory()
01043 {
01044   if(selected_trajectory_name_ != "")
01045   {
01046     int index_of_deleted = -1;
01047     QList<QTreeWidgetItem*> selected = trajectory_tree_->selectedItems();
01048     if(selected.size() > 0)
01049     {
01050       index_of_deleted = trajectory_tree_->indexOfTopLevelItem( selected[0] );
01051     }
01052 
01053     lockScene();
01054     unsigned int mpr_id = motion_plan_map_[selected_motion_plan_name_].getId();
01055     unsigned int traj_id = trajectory_map_[selected_motion_plan_name_][selected_trajectory_name_].getId();
01056     deleteTrajectory( mpr_id, traj_id );
01057     emit updateTables();
01058     unlockScene();
01059 
01060     int index_to_select = index_of_deleted;
01061 
01062     if( index_to_select >= trajectory_tree_->topLevelItemCount() )
01063     {
01064       index_to_select--;
01065     }
01066     if( index_to_select >= 0 )
01067     {
01068       trajectory_tree_->topLevelItem( index_to_select )->setSelected( true );
01069     }
01070   }
01071   else
01072   {
01073     QMessageBox msg(QMessageBox::Warning, "Delete Trajectory", "No Trajectory Selected!");
01074     msg.addButton("Ok", QMessageBox::AcceptRole);
01075     msg.exec();
01076   }
01077 }
01078 
01079 void WarehouseViewer::createNewMotionPlanPressed()
01080 {
01081   new_request_dialog_->open();
01082 }
01083 
01084 void WarehouseViewer::trajectoryEditChanged()
01085 {
01086   if(selected_trajectory_name_ != "")
01087   {
01088     TrajectoryData& trajectory = trajectory_map_[selected_motion_plan_name_][selected_trajectory_name_];
01089     trajectory.setCurrentPoint(trajectory_point_edit_->value());
01090     trajectory_slider_->setValue(trajectory_point_edit_->value());
01091     if( !trajectory.isVisible() )
01092     {
01093       trajectory.setVisible(true);
01094       setSelectedTrajectoryCheckboxVisible();
01095     }
01096     if( trajectory.getCurrentState() == NULL )
01097     {
01098       trajectory.setCurrentState(new KinematicState(*robot_state_));
01099     }
01100   }
01101 }
01102 
01103 void WarehouseViewer::setupPlanningSceneDialog()
01104 {
01105   planning_scene_table_ = new QTableWidget(load_planning_scene_dialog_);
01106   QVBoxLayout* layout = new QVBoxLayout(load_planning_scene_dialog_);
01107   layout->addWidget(planning_scene_table_);
01108   load_scene_progress_ = new QProgressBar(load_planning_scene_dialog_);
01109   layout->addWidget(load_scene_progress_);
01110 
01111   new_planning_scene_button_ = new QPushButton(load_planning_scene_dialog_);
01112   new_planning_scene_button_->setText("New...");
01113   new_planning_scene_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01114 
01115   load_planning_scene_button_ = new QPushButton(load_planning_scene_dialog_);
01116   load_planning_scene_button_->setText("Load...");
01117   load_planning_scene_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01118 
01119   remove_planning_scene_button_ = new QPushButton(load_planning_scene_dialog_);
01120   remove_planning_scene_button_->setText("Remove...");
01121   remove_planning_scene_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01122 
01123   refresh_planning_scene_button_ = new QPushButton(load_planning_scene_dialog_);
01124   refresh_planning_scene_button_->setText("Refresh...");
01125   refresh_planning_scene_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01126 
01127   connect(new_planning_scene_button_, SIGNAL(clicked()), this, SLOT(newButtonPressed()));
01128   connect(load_planning_scene_button_, SIGNAL(clicked()), this, SLOT(loadButtonPressed()));
01129   connect(refresh_planning_scene_button_, SIGNAL(clicked()), this, SLOT(refreshButtonPressed()));
01130   connect(remove_planning_scene_button_, SIGNAL(clicked()), this, SLOT(removePlanningSceneButtonPressed()));
01131 
01132   load_motion_plan_requests_box_ = new QCheckBox(load_planning_scene_dialog_);
01133   load_motion_plan_requests_box_->setChecked(true);
01134   load_motion_plan_requests_box_->setText("Load Motion Plan Requests");
01135   load_trajectories_box_ = new QCheckBox(load_planning_scene_dialog_);
01136   load_trajectories_box_->setChecked(true);
01137   load_trajectories_box_->setText("Load Trajectories");
01138 
01139   layout->addWidget(load_motion_plan_requests_box_);
01140   layout->addWidget(load_trajectories_box_);
01141   layout->addWidget(new_planning_scene_button_);
01142   layout->addWidget(load_planning_scene_button_);
01143   layout->addWidget(refresh_planning_scene_button_);
01144   layout->addWidget(remove_planning_scene_button_);
01145 
01146   load_planning_scene_dialog_->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
01147   load_planning_scene_dialog_->setLayout(layout);
01148 }
01149 
01150 void WarehouseViewer::quit()
01151 {
01152   QMessageBox msgBox(QMessageBox::Warning, "Quit?", "Are you sure you want to quit? Unsaved changes will be lost.");
01153   QPushButton *quitButton = msgBox.addButton("Quit", QMessageBox::ActionRole);
01154   QPushButton *saveQuitButton = msgBox.addButton("Save And Quit", QMessageBox::ActionRole);
01155   msgBox.addButton(QMessageBox::Cancel);
01156 
01157   msgBox.exec();
01158 
01159   if (msgBox.clickedButton() == quitButton)
01160   {
01161     quit_threads_ = true;
01162     delete this;
01163     return;
01164   }
01165   else if (msgBox.clickedButton() == saveQuitButton)
01166   {
01167     saveCurrentPlanningScene(false);
01168     quit_threads_ = true;
01169     delete this;
01170     return;
01171   }
01172   else
01173   {
01174     return;
01175   }
01176 }
01177 
01178 void WarehouseViewer::motionPlanTableSelection()
01179 {
01180   QList<QTreeWidgetItem*> selected = motion_plan_tree_->selectedItems();
01181 
01182   if(selected.size() > 0)
01183   {
01184     const QString& name = selected[0]->toolTip(0);
01185     selectMotionPlan(name.toStdString());
01186   }
01187 }
01188 
01189 void WarehouseViewer::selectMotionPlan(std::string ID)
01190 {
01191   selected_motion_plan_name_ = ID;
01192   selected_trajectory_name_ = "";
01193   createTrajectoryTable();
01194   selected_request_label_->setText(QString::fromStdString("Selected Request: "+selected_motion_plan_name_ ));
01195 
01196 
01197   if(motion_plan_map_[selected_motion_plan_name_].getTrajectories().size() > 0)
01198   {
01199     unsigned int id = *(motion_plan_map_[selected_motion_plan_name_].getTrajectories().begin()); 
01200     selectTrajectory(getTrajectoryNameFromId(id));
01201   }
01202 }
01203 
01204 void WarehouseViewer::createMotionPlanTable()
01205 {
01206   if(motion_plan_map_.find(selected_motion_plan_name_) == motion_plan_map_.end()) {
01207     selected_request_label_->setText("Selected Request: None");
01208     selected_motion_plan_name_ = "";
01209   } 
01210 
01211   if(current_planning_scene_name_ != "")
01212   {
01213     PlanningSceneData& planningSceneData = planning_scene_map_[current_planning_scene_name_];
01214 
01215     motion_plan_tree_->clear();
01216     motion_plan_tree_->setColumnCount(4);
01217     motion_plan_tree_->setColumnWidth(0, 150);
01218 
01219     unsigned int count = 0;
01220     for(std::set<unsigned int>::iterator it = planningSceneData.getRequests().begin();
01221         it != planningSceneData.getRequests().end();
01222         it++, count++) {
01223 
01224       if(motion_plan_map_.find(getMotionPlanRequestNameFromId(*it)) == motion_plan_map_.end()) {
01225         ROS_INFO_STREAM("Bad matt 1");
01226       }
01227       MotionPlanRequestData& requestData = motion_plan_map_[getMotionPlanRequestNameFromId(*it)];
01228 
01229 
01230       QTreeWidgetItem* nameItem = new QTreeWidgetItem(QStringList(QString::fromStdString(requestData.getName())));
01231       nameItem->setToolTip(0,nameItem->text(0));
01232       nameItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01233       motion_plan_tree_->insertTopLevelItem(count, nameItem);
01234 
01235       QStringList sourceList;
01236       sourceList.append("Source");
01237       sourceList.append(QString::fromStdString(requestData.getSource()));
01238       QTreeWidgetItem* sourceItem = new QTreeWidgetItem(sourceList);
01239       sourceItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01240       sourceItem->setToolTip(0, nameItem->text(0));
01241       sourceItem->setToolTip(1, nameItem->text(0));
01242       nameItem->insertChild(0, sourceItem);
01243 
01244       QStringList collisionList;
01245       collisionList.append("Collisions");
01246       QTreeWidgetItem* collisionItem = new QTreeWidgetItem(collisionList);
01247       collisionItem->setToolTip(0, nameItem->text(0));
01248       QCheckBox* collisionsVisible = new QCheckBox(motion_plan_tree_);
01249       collisionsVisible->setToolTip(nameItem->text(0));
01250       nameItem->insertChild(1, collisionItem);
01251       motion_plan_tree_->setItemWidget(collisionItem, 1, collisionsVisible);
01252       collisionsVisible->setChecked(requestData.areCollisionsVisible());
01253 
01254       connect(collisionsVisible, SIGNAL(clicked(bool)), this, SLOT(motionPlanCollisionVisibleButtonClicked(bool)));
01255 
01256       QStringList startList;
01257       startList.append("Start Position");
01258       startList.append("");
01259       startList.append("Color");
01260       startList.append("");
01261       QTreeWidgetItem* startItem = new QTreeWidgetItem(startList);
01262       startItem->setToolTip(0, nameItem->text(0));
01263       QCheckBox* startVisible = new QCheckBox(motion_plan_tree_);
01264       startVisible->setText("Visible");
01265       startVisible->setToolTip(nameItem->text(0));
01266       nameItem->insertChild(2, startItem);
01267       motion_plan_tree_->setItemWidget(startItem, 1, startVisible);
01268       startVisible->setChecked(requestData.isStartVisible());
01269 
01270       connect(startVisible, SIGNAL(clicked(bool)), this, SLOT(motionPlanStartVisibleButtonClicked(bool)));
01271 
01272       QPushButton* startColorButton = new QPushButton(motion_plan_tree_);
01273       std::stringstream startColorStream;
01274       startColorStream<< "(" << (int)(requestData.getStartColor().r*255) <<" , ";
01275       startColorStream << (int)(requestData.getStartColor().g*255) << " , ";
01276       startColorStream << (int)(requestData.getStartColor().b*255) << ")";
01277       startColorButton->setText(QString::fromStdString(startColorStream.str()));
01278       startColorButton->setToolTip(nameItem->text(0));
01279       startColorButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01280       connect(startColorButton, SIGNAL(clicked()), this, SLOT(motionPlanStartColorButtonClicked()));
01281       motion_plan_tree_->setItemWidget(startItem, 3, startColorButton);
01282 
01283 
01284       QStringList endList;
01285       endList.append("End Position");
01286       endList.append("");
01287       endList.append("Color");
01288       endList.append("");
01289       QTreeWidgetItem* endItem = new QTreeWidgetItem(endList);
01290       endItem->setToolTip(0, nameItem->text(0));
01291       QCheckBox* endVisible = new QCheckBox(motion_plan_tree_);
01292       endVisible->setText("Visible");
01293       endVisible->setToolTip(nameItem->text(0));
01294       nameItem->insertChild(3, endItem);
01295       motion_plan_tree_->setItemWidget(endItem, 1, endVisible);
01296       endVisible->setChecked(requestData.isEndVisible());
01297 
01298       connect(endVisible, SIGNAL(clicked(bool)), this, SLOT(motionPlanEndVisibleButtonClicked(bool)));
01299 
01300       QPushButton* endColorButton = new QPushButton(motion_plan_tree_);
01301       std::stringstream endColorStream;
01302       endColorStream<< "(" << (int)(requestData.getGoalColor().r*255) <<" , ";
01303       endColorStream << (int)(requestData.getGoalColor().g*255) << " , ";
01304       endColorStream << (int)(requestData.getGoalColor().b*255) << ")";
01305       endColorButton->setText(QString::fromStdString(endColorStream.str()));
01306       endColorButton->setToolTip(nameItem->text(0));
01307       endColorButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01308       connect(endColorButton, SIGNAL(clicked()), this, SLOT(motionPlanEndColorButtonClicked()));
01309       motion_plan_tree_->setItemWidget(endItem, 3, endColorButton);
01310 
01311       QStringList controlList;
01312       controlList.append("Joint Control");
01313       QTreeWidgetItem* controlItem = new QTreeWidgetItem(controlList);
01314       controlItem->setToolTip(0, nameItem->text(0));
01315       nameItem->insertChild(4, controlItem);
01316       QCheckBox* controlsVisible = new QCheckBox(motion_plan_tree_);
01317       controlsVisible->setToolTip(nameItem->text(0));
01318       connect(controlsVisible, SIGNAL(clicked(bool)), this, SLOT(motionPlanJointControlsActiveButtonClicked(bool)));
01319       motion_plan_tree_->setItemWidget(controlItem,1, controlsVisible);
01320 
01321       QStringList path_constraints_list;
01322       path_constraints_list.append("Has Path Constraints");
01323       QTreeWidgetItem* pathItem = new QTreeWidgetItem(path_constraints_list);
01324       pathItem->setToolTip(0, nameItem->text(0));
01325       nameItem->insertChild(5, pathItem);
01326       QCheckBox* path_check_box = new QCheckBox(motion_plan_tree_);
01327       path_check_box->setToolTip(nameItem->text(0));
01328       path_check_box->setChecked(requestData.hasPathConstraints());
01329       connect(path_check_box, SIGNAL(clicked(bool)), this, SLOT(motionPlanHasPathConstraintsButtonClicked(bool)));
01330       motion_plan_tree_->setItemWidget(pathItem,1, path_check_box);
01331 
01332       QPushButton* setPathConstraintsButton = new QPushButton(motion_plan_tree_);
01333       setPathConstraintsButton->setText("Set Path Constraints");
01334       setPathConstraintsButton->setToolTip(nameItem->text(0));
01335       setPathConstraintsButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01336       connect(setPathConstraintsButton, SIGNAL(clicked()), this, SLOT(setPathConstraintsButtonClicked()));
01337       motion_plan_tree_->setItemWidget(pathItem, 3, setPathConstraintsButton);
01338       
01339       QStringList renderTypeList;
01340       renderTypeList.append("Model Rendering Mode");
01341       QTreeWidgetItem* renderTypeItem = new QTreeWidgetItem(renderTypeList);
01342       renderTypeItem->setToolTip(0, nameItem->text(0));
01343 
01344       QComboBox* renderTypeBox = new QComboBox(motion_plan_tree_);
01345       QStringList items;
01346       items.append("Collision Mesh");
01347       items.append("Visual Mesh");
01348       items.append("Padding Mesh");
01349       renderTypeBox->addItems(items);
01350       connect(renderTypeBox, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(motionPlanRenderTypeChanged(const QString&)));
01351       renderTypeBox->setToolTip(nameItem->text(0));
01352       nameItem->insertChild(6, renderTypeItem);
01353       motion_plan_tree_->setItemWidget(renderTypeItem, 1, renderTypeBox);
01354 
01355       switch(requestData.getRenderType())
01356       {
01357         case CollisionMesh:
01358           renderTypeBox->setCurrentIndex(0);
01359           break;
01360         case VisualMesh:
01361           renderTypeBox->setCurrentIndex(1);
01362           break;
01363         case PaddingMesh:
01364           renderTypeBox->setCurrentIndex(2);
01365           break;
01366       }
01367     }
01368   }
01369 }
01370 
01371 void WarehouseViewer::motionPlanCollisionVisibleButtonClicked(bool checked)
01372 {
01373   QObject* sender = QObject::sender();
01374   QCheckBox* button = qobject_cast<QCheckBox*> (sender);
01375 
01376   if(button != NULL)
01377   {
01378     std::string mprID = button->toolTip().toStdString();
01379     MotionPlanRequestData& data = motion_plan_map_[mprID];
01380     data.setCollisionsVisible(checked);
01381   }
01382 }
01383 
01384 void WarehouseViewer::motionPlanStartColorButtonClicked()
01385 {
01386   QObject* sender = QObject::sender();
01387   QPushButton* button = qobject_cast<QPushButton*>(sender);
01388 
01389   if(button != NULL)
01390   {
01391     std::string trajectoryID = button->toolTip().toStdString();
01392     MotionPlanRequestData& data = motion_plan_map_[trajectoryID];
01393     QColor col(data.getStartColor().r*255, data.getStartColor().g*255, data.getStartColor().b*255);
01394     QColor colorSelected = QColorDialog::getColor(col, this);
01395     if(!colorSelected.isValid())
01396     {
01397       return;
01398     }
01399     std::stringstream colorStream;
01400     colorStream<< "(" << colorSelected.red()<<" , ";
01401     colorStream << colorSelected.green()<< " , ";
01402     colorStream << colorSelected.blue() << ")";
01403 
01404     button->setText(QString::fromStdString(colorStream.str()));
01405 
01406     std_msgs::ColorRGBA trajColor;
01407     trajColor.r = colorSelected.redF();
01408     trajColor.g = colorSelected.greenF();
01409     trajColor.b = colorSelected.blueF();
01410     trajColor.a = 0.5f;
01411     data.setStartColor(trajColor);
01412     data.refreshColors();
01413   }
01414 }
01415 
01416 void WarehouseViewer::motionPlanEndColorButtonClicked()
01417 {
01418   QObject* sender = QObject::sender();
01419   QPushButton* button = qobject_cast<QPushButton*>(sender);
01420 
01421   if(button != NULL)
01422   {
01423     std::string trajectoryID = button->toolTip().toStdString();
01424     MotionPlanRequestData& data = motion_plan_map_[trajectoryID];
01425     QColor col(data.getGoalColor().r*255, data.getGoalColor().g*255, data.getGoalColor().b*255);
01426     QColor colorSelected = QColorDialog::getColor(col, this);
01427     if(!colorSelected.isValid())
01428     {
01429       return;
01430     }
01431     std::stringstream colorStream;
01432     colorStream<< "(" << colorSelected.red()<<" , ";
01433     colorStream << colorSelected.green()<< " , ";
01434     colorStream << colorSelected.blue() << ")";
01435 
01436     button->setText(QString::fromStdString(colorStream.str()));
01437 
01438     std_msgs::ColorRGBA trajColor;
01439     trajColor.r = colorSelected.redF();
01440     trajColor.g = colorSelected.greenF();
01441     trajColor.b = colorSelected.blueF();
01442     trajColor.a = 0.5f;
01443     data.setGoalColor(trajColor);
01444     data.refreshColors();
01445 
01446   }
01447 }
01448 
01449 void WarehouseViewer::motionPlanStartVisibleButtonClicked(bool checked)
01450 {
01451   QObject* sender = QObject::sender();
01452   QCheckBox* button = qobject_cast<QCheckBox*> (sender);
01453 
01454   if(button != NULL)
01455   {
01456     std::string mprID = button->toolTip().toStdString();
01457     MotionPlanRequestData& data = motion_plan_map_[mprID];
01458     data.setStartVisible(checked);
01459     data.setJointControlsVisible(data.areJointControlsVisible(), this);
01460     setIKControlsVisible(mprID, StartPosition, button->isChecked());
01461   }
01462 
01463 
01464 }
01465 
01466 void WarehouseViewer::motionPlanEndVisibleButtonClicked(bool checked)
01467 {
01468   QObject* sender = QObject::sender();
01469   QCheckBox* button = qobject_cast<QCheckBox*> (sender);
01470 
01471   if(button != NULL)
01472   {
01473     std::string mprID = button->toolTip().toStdString();
01474     MotionPlanRequestData& data = motion_plan_map_[mprID];
01475     data.setEndVisible(checked);
01476     data.setJointControlsVisible(data.areJointControlsVisible(), this);
01477     setIKControlsVisible(mprID, GoalPosition, button->isChecked());
01478   }
01479 }
01480 
01481 void WarehouseViewer::createNewMotionPlanRequest(std::string group_name, std::string end_effector_name)
01482 {
01483 
01484   if(current_planning_scene_name_ != "")
01485   {
01486     unsigned int motion_plan_id;
01487     createMotionPlanRequest(*robot_state_, *robot_state_, 
01488                             group_name, end_effector_name,
01489                             getPlanningSceneIdFromName(current_planning_scene_name_), 
01490                             create_request_from_robot_box_->isChecked(), 
01491                             motion_plan_id);
01492     selectMotionPlan(getMotionPlanRequestNameFromId(motion_plan_id));
01493   } 
01494   else
01495   {
01496     QMessageBox msg(QMessageBox::Warning, "Create Request", "No Planning Scene Loaded!");
01497     msg.addButton("Ok", QMessageBox::AcceptRole);
01498     msg.exec();
01499   }
01500 }
01501 
01502 void WarehouseViewer::savePlanningSceneSlot() {
01503   saveCurrentPlanningScene(false);
01504 }
01505 
01506 void WarehouseViewer::copyPlanningSceneSlot() {
01507   saveCurrentPlanningScene(true);
01508   trajectory_tree_->clear();
01509   updateJointStates();
01510   createMotionPlanTable();
01511   planning_scene_map_[current_planning_scene_name_].getRobotState(robot_state_);
01512 }
01513 
01514 void WarehouseViewer::saveCurrentPlanningScene(bool copy)
01515 {
01516   ROS_DEBUG_STREAM("Current planning scene id is " << current_planning_scene_name_);
01517   ROS_DEBUG_STREAM("Hostname is " << planning_scene_map_[current_planning_scene_name_].getHostName());
01518   savePlanningScene(planning_scene_map_[current_planning_scene_name_], copy);
01519   QMessageBox msgBox(QMessageBox::Information, "Saved", "Saved planning scene successfully.");
01520   msgBox.addButton(QMessageBox::Ok);
01521   msgBox.exec();
01522 }
01523 
01524 void WarehouseViewer::createNewPlanningSceneSlot() {
01525   createNewPlanningSceneConfirm();
01526 }
01527 
01528 bool WarehouseViewer::createNewPlanningSceneConfirm()
01529 {
01530   if(planning_scene_initialized_) {
01531     QMessageBox msgBox(QMessageBox::Warning, "Create New Planning Scene", "Are you sure you want to create a new planning scene? Unsaved changes to the current planning scene will be lost.");
01532     QPushButton *createButton= msgBox.addButton("Create New Without Saving Changes", QMessageBox::ActionRole);
01533     QPushButton *saveCreateButton = msgBox.addButton("Save Changes Before Creating New", QMessageBox::ActionRole);
01534     msgBox.addButton(QMessageBox::Cancel);
01535 
01536     msgBox.exec();
01537     if (msgBox.clickedButton() == saveCreateButton)
01538     {
01539       saveCurrentPlanningScene(false);
01540       setCurrentPlanningScene(createNewPlanningScene(), true, true);
01541       motion_plan_tree_->clear();
01542       trajectory_tree_->clear();
01543       ROS_INFO("Created a new planning scene: %s", current_planning_scene_name_.c_str());
01544     }
01545     else if (msgBox.clickedButton() != createButton)
01546     {
01547       return false;
01548     }
01549   }
01550   setCurrentPlanningScene(createNewPlanningScene(), true, true);
01551   planning_scene_initialized_ = true;
01552   motion_plan_tree_->clear();
01553   trajectory_tree_->clear();
01554   ROS_INFO("Created a new planning scene: %s", current_planning_scene_name_.c_str());
01555   return true;
01556 }
01557 
01558 void WarehouseViewer::updateState()
01559 {
01560   emit updateTables();
01561 }
01562 
01563 void WarehouseViewer::updateStateTriggered()
01564 {
01565   lockScene();
01566   createMotionPlanTable();
01567   createTrajectoryTable();
01568   unlockScene();
01569 }
01570 
01571 void WarehouseViewer::primaryPlannerTriggered()
01572 {
01573   if(set_primary_planner_action_->isChecked()) {
01574     set_secondary_planner_action_->setChecked(false);
01575     active_planner_index_ = 1;
01576   } else if(!set_secondary_planner_action_->isChecked()){
01577     set_primary_planner_action_->setChecked(true);
01578   }
01579 }
01580 
01581 void WarehouseViewer::secondaryPlannerTriggered()
01582 {
01583   if(set_secondary_planner_action_->isChecked()) {
01584     set_primary_planner_action_->setChecked(false);
01585     active_planner_index_ = 2;
01586   } else if(!set_primary_planner_action_->isChecked()){
01587     set_secondary_planner_action_->setChecked(true);
01588   } 
01589 }
01590 
01591 void WarehouseViewer::primaryFilterTriggered()
01592 {
01593   if(set_primary_filter_action_->isChecked()) {
01594     set_secondary_filter_action_->setChecked(false);
01595     use_primary_filter_ = true;
01596   } else if(!set_secondary_filter_action_->isChecked()){
01597     set_primary_filter_action_->setChecked(true);
01598   }
01599 }
01600 
01601 void WarehouseViewer::secondaryFilterTriggered()
01602 {
01603   if(set_secondary_filter_action_->isChecked()) {
01604     set_primary_filter_action_->setChecked(false);
01605     use_primary_filter_ = false;
01606   } else if(!set_primary_filter_action_->isChecked()){
01607     set_secondary_filter_action_->setChecked(true);
01608   }
01609 }
01610 
01611 void WarehouseViewer::popupLoadPlanningScenes()
01612 {
01613   if(!warehouse_data_loaded_once_)
01614   {
01615     refreshButtonPressed();
01616   }
01617   load_planning_scene_dialog_->exec();
01618 }
01619 
01620 void WarehouseViewer::refreshButtonPressed()
01621 {
01622   if(table_load_thread_ != NULL)
01623   {
01624     table_load_thread_->wait();
01625     delete table_load_thread_;
01626     table_load_thread_ = NULL;
01627   }
01628   table_load_thread_= new TableLoadThread(this);
01629   table_load_thread_->start();
01630 }
01631 
01632 void WarehouseViewer::newButtonPressed() 
01633 {
01634   if(createNewPlanningSceneConfirm()) {
01635     load_planning_scene_dialog_->close();
01636   }
01637 }
01638 
01639 void WarehouseViewer::loadButtonPressed()
01640 {
01641   if(planning_scene_initialized_) {
01642     QMessageBox msgBox(QMessageBox::Information, "Load New Planning Scene", "Are you sure you want to load a new planning scene? Unsaved changes to the current planning scene will be lost.");
01643     QPushButton *createButton= msgBox.addButton("Load New Without Saving Changes", QMessageBox::ActionRole);
01644     QPushButton *saveCreateButton = msgBox.addButton("Save Changes Before Loading New", QMessageBox::ActionRole);
01645     msgBox.addButton(QMessageBox::Cancel);
01646     
01647     msgBox.exec();
01648     
01649     if (msgBox.clickedButton() == saveCreateButton)
01650     {
01651       saveCurrentPlanningScene(false);
01652     }
01653     else if (msgBox.clickedButton() != createButton)
01654     {
01655       return;
01656     }
01657   }
01658 
01659   QList<QTableWidgetItem*> items = planning_scene_table_->selectedItems();
01660 
01661   if(items.size() > 0)
01662   {
01663     QTableWidgetItem* item = items[0];
01664     QTableWidgetItem* nameItem = planning_scene_table_->item(item->row(),1);
01665     PlanningSceneData& data = planning_scene_map_[nameItem->text().toStdString()];
01666     loadPlanningScene(data.getTimeStamp(), data.getId());
01667     //this will blow away the above reference, so redoing
01668     data = planning_scene_map_[nameItem->text().toStdString()];
01669     setCurrentPlanningScene(nameItem->text().toStdString(), load_motion_plan_requests_box_->isChecked(), load_trajectories_box_->isChecked());
01670     trajectory_tree_->clear();
01671     updateJointStates();
01672     createMotionPlanTable();
01673     data.getRobotState(robot_state_);
01674     planning_scene_initialized_ = true;
01675     load_planning_scene_dialog_->close();
01676   }
01677 }
01678 
01679 void WarehouseViewer::removePlanningSceneButtonPressed()
01680 {
01681   QMessageBox msgBox(QMessageBox::Information, "Remove Planning Scene", "This will permanently remove the indicated planning scene(s) and all associated data from the warehouse.");
01682   msgBox.addButton(QMessageBox::Cancel);
01683 
01684   QPushButton *deleteButton= msgBox.addButton("Remove", QMessageBox::ActionRole);
01685 
01686   msgBox.exec();
01687 
01688   if (msgBox.clickedButton() != deleteButton)
01689   {
01690     return;
01691   }
01692 
01693   QList<QTableWidgetItem*> items = planning_scene_table_->selectedItems();
01694   
01695   for(int i = 0; i < items.size(); i++) {
01696     QTableWidgetItem* item = items[i];
01697     QTableWidgetItem* nameItem = planning_scene_table_->item(item->row(),1);
01698     PlanningSceneData& data = planning_scene_map_[nameItem->text().toStdString()];
01699     move_arm_warehouse_logger_reader_->removePlanningSceneAndAssociatedDataFromWarehouse(data.getHostName(), data.getId());
01700     planning_scene_map_.erase(nameItem->text().toStdString());
01701     planning_scene_table_->removeRow(item->row());
01702   }
01703 }
01704 
01705 void WarehouseViewer::replanButtonPressed()
01706 {
01707   if(selected_motion_plan_name_ != "")
01708   {
01709     unsigned int new_traj;
01710     planToRequest(motion_plan_map_[selected_motion_plan_name_], new_traj);
01711     createTrajectoryTable();
01712     selectTrajectory(getTrajectoryNameFromId(new_traj));
01713     playTrajectory(motion_plan_map_[selected_motion_plan_name_], 
01714                    trajectory_map_[selected_motion_plan_name_][getTrajectoryNameFromId(new_traj)]);
01715   }
01716   else
01717   {
01718     QMessageBox msg(QMessageBox::Warning, "Plan Trajectory", "No Motion Plan Request Selected!");
01719     msg.addButton("Ok", QMessageBox::AcceptRole);
01720     msg.exec();
01721   }
01722 }
01723 
01724 void WarehouseViewer::trajectoryTableSelection()
01725 {
01726   QList<QTreeWidgetItem*> selected = trajectory_tree_->selectedItems();
01727 
01728   if(selected.size() > 0)
01729   {
01730     QString name = selected[0]->toolTip(0);
01731     selectTrajectory(name.toStdString());
01732   }
01733 }
01734 
01735 void WarehouseViewer::selectTrajectory(std::string ID)
01736 {
01737   if(ID == "") {
01738     ROS_WARN_STREAM("Empty trajectory being selected - that's bad");
01739     return;
01740   }
01741   selected_trajectory_name_ = ID;
01742   TrajectoryData& trajectory = trajectory_map_[selected_motion_plan_name_][selected_trajectory_name_];
01743   unsigned int point = trajectory.getCurrentPoint();
01744   int sz = ((int)trajectory.getTrajectory().points.size()) - 1;
01745   trajectory_slider_->setMaximum(sz);
01746   trajectory_slider_->setMinimum(0);
01747   trajectory_slider_->setValue(point);
01748 
01749   trajectory_point_edit_->setRange(0,sz);
01750   trajectory_point_edit_->setValue(point);
01751 
01752   selected_trajectory_label_->setText(QString::fromStdString(ID));
01753   selected_trajectory_source_label_->setText(QString::fromStdString(trajectory.getSource()));
01754 
01755   setCommonTrajectoryInfo();
01756 
01757   if(trajectory.getSource() == "Planner" || trajectory.getSource() == "planner")
01758   {
01759     if( trajectory.trajectory_error_code_.val == ArmNavigationErrorCodes::SUCCESS )
01760     {
01761       setPlannedTrajectoryInfo(true, trajectory);
01762     }
01763   }
01764   else if (trajectory.getSource() == "Trajectory Filterer" || trajectory.getSource() == "filter")
01765   {
01766     if( trajectory.trajectory_error_code_.val == ArmNavigationErrorCodes::SUCCESS )
01767     {
01768       setFilteredTrajectoryInfo(true, trajectory);
01769     }
01770   }
01771   else if(trajectory.getSource() == "Robot Monitor" || trajectory.getSource() == "monitor")
01772   {
01773     if( trajectory.trajectory_error_code_.val == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL )
01774     {
01775       setExecutedTrajectoryInfo(true, trajectory);
01776     }
01777   }
01778   else if(trajectory.getSource() == "Overshoot Monitor")
01779   {
01780     setOvershootTrajectoryInfo(true, trajectory);
01781   }
01782 
01783   // Find the selected trajectory in the tree and make it visibly selected.
01784   for(int i = 0; i < trajectory_tree_->topLevelItemCount(); i++)
01785   {
01786     QTreeWidgetItem* item = trajectory_tree_->topLevelItem(i);
01787 
01788     if(item->text(0).toStdString() == selected_trajectory_name_)
01789     {
01790       item->setSelected( true );
01791     }
01792   }
01793 }
01794 
01795 void WarehouseViewer::setSelectedTrajectoryCheckboxVisible()
01796 {
01797   // Set checkbox to visible.
01798   for(int i = 0; i < trajectory_tree_->topLevelItemCount(); i++)
01799   {
01800     QTreeWidgetItem* item = trajectory_tree_->topLevelItem(i);
01801 
01802     if(item->text(0).toStdString() == selected_trajectory_name_)
01803     {
01804       for(int j = 0; j < item->childCount(); j++)
01805       {
01806         QTreeWidgetItem* child = item->child(j);
01807         QCheckBox* box = dynamic_cast<QCheckBox*>(trajectory_tree_->itemWidget(child, 0));
01808 
01809         if(box != NULL)
01810         {
01811           if(box->text().toStdString() == "Visible")
01812           {
01813             box->setChecked(true);
01814             break;
01815           }
01816         }
01817       }
01818       break;
01819     }
01820   }
01821 }
01822 
01823 void WarehouseViewer::playButtonPressed()
01824 {
01825   if(selected_trajectory_name_ != "")
01826   {
01827     TrajectoryData& trajectory = trajectory_map_[selected_motion_plan_name_][selected_trajectory_name_];
01828     playTrajectory(motion_plan_map_[getMotionPlanRequestNameFromId(trajectory.getMotionPlanRequestId())], 
01829                    trajectory);
01830     std::stringstream ss;
01831     ss << trajectory.trajectory_error_code_.val;
01832 
01833     setSelectedTrajectoryCheckboxVisible();
01834   }
01835   else
01836   {
01837     QMessageBox msg(QMessageBox::Warning, "Play Trajectory", "No Trajectory Selected!");
01838     msg.addButton("Ok", QMessageBox::AcceptRole);
01839     msg.exec();
01840   }
01841 }
01842 
01843 void WarehouseViewer::filterButtonPressed()
01844 {
01845   if(selected_trajectory_name_ != "")
01846   {
01847     TrajectoryData& trajectory = trajectory_map_[selected_motion_plan_name_][selected_trajectory_name_];
01848     unsigned int filterID;
01849     MotionPlanRequestData& data = motion_plan_map_[getMotionPlanRequestNameFromId(trajectory.getMotionPlanRequestId())];
01850     filterTrajectory(data,trajectory, filterID);
01851     playTrajectory(data, trajectory_map_[selected_motion_plan_name_][getTrajectoryNameFromId(filterID)]);
01852     std::stringstream ss;
01853     ss << trajectory.trajectory_error_code_.val;
01854     createTrajectoryTable();
01855     selectTrajectory(getTrajectoryNameFromId(filterID));
01856   }
01857   else
01858   {
01859     QMessageBox msg(QMessageBox::Warning, "Filter Trajectory", "No Trajectory Selected!");
01860     msg.addButton("Ok", QMessageBox::AcceptRole);
01861     msg.exec();
01862   }
01863 }
01864 
01865 void WarehouseViewer::trajectorySliderChanged(int nv)
01866 {
01867   if(selected_trajectory_name_ != "")
01868   {
01869     trajectory_point_edit_->setValue(nv);
01870   }
01871   else if( nv != 0 )
01872   {
01873     QMessageBox msg(QMessageBox::Warning, "Control Trajectory", "No Trajectory Selected!");
01874     msg.addButton("Ok", QMessageBox::AcceptRole);
01875     msg.exec();
01876   }
01877 }
01878 
01879 void WarehouseViewer::selectedTrajectoryCurrentPointChanged( unsigned int new_current_point )
01880 {
01881   emit selectedTrajectoryPointChanged( new_current_point );
01882 }
01883 
01884 void WarehouseViewer::onSelectedTrajectoryPointChanged( unsigned int new_current_point )
01885 {
01886   trajectory_slider_->setValue( new_current_point );
01887   trajectory_point_edit_->setValue( new_current_point );
01888 }
01889 
01890 void WarehouseViewer::createTrajectoryTable()
01891 {
01892   if(selected_motion_plan_name_ == "")
01893   {
01894     trajectory_tree_->clear();
01895     trajectory_tree_->setColumnCount(2);
01896     trajectory_tree_->setColumnWidth(0, 200);
01897     selected_trajectory_name_ = "";
01898     selected_trajectory_label_->setText("None");
01899     selected_trajectory_error_label_->setText("");
01900     trajectory_slider_->setMaximum(0);
01901     trajectory_slider_->setMinimum(0);
01902     trajectory_point_edit_->setRange(0,0);
01903     return;
01904   }
01905 
01906   if(!hasTrajectory(selected_motion_plan_name_,
01907                     selected_trajectory_name_)) {
01908     selected_trajectory_name_ = "";
01909     selected_trajectory_label_->setText("None");
01910     selected_trajectory_error_label_->setText("");
01911     trajectory_slider_->setMaximum(0);
01912     trajectory_slider_->setMinimum(0);
01913     trajectory_point_edit_->setRange(0,0);
01914   } else {
01915     selectTrajectory(selected_trajectory_name_);
01916   }
01917 
01918   if(motion_plan_map_.find(selected_motion_plan_name_) == motion_plan_map_.end()) {
01919     ROS_INFO_STREAM("Going to be generating empty MPR");
01920   }
01921 
01922   MotionPlanRequestData& data = motion_plan_map_[selected_motion_plan_name_];
01923   trajectory_tree_->clear();
01924   trajectory_tree_->setColumnCount(2);
01925   trajectory_tree_->setColumnWidth(0, 200);
01926 
01927   unsigned int count = 0;
01928   for(std::set<unsigned int>::iterator it = data.getTrajectories().begin();
01929       it != data.getTrajectories().end();
01930       it++, count++) {
01931     TrajectoryData& trajectory = 
01932       trajectory_map_[selected_motion_plan_name_][getTrajectoryNameFromId(*it)];
01933 
01934     QTreeWidgetItem* nameItem = new QTreeWidgetItem(QStringList(QString::fromStdString(trajectory.getName())));
01935     nameItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01936     nameItem->setToolTip(0, nameItem->text(0));
01937     trajectory_tree_->insertTopLevelItem(count, nameItem);
01938 
01939     QStringList collisionList;
01940     collisionList.append("");
01941     QTreeWidgetItem* collisionItem = new QTreeWidgetItem(collisionList);
01942     collisionItem->setToolTip(0, nameItem->text(0));
01943     QCheckBox* collisionsVisibleBox = new QCheckBox(trajectory_tree_);
01944     collisionsVisibleBox->setText("Show Collisions");
01945     collisionsVisibleBox->setChecked(trajectory.areCollisionsVisible());
01946     collisionsVisibleBox->setToolTip(nameItem->text(0));
01947     nameItem->insertChild(0, collisionItem);
01948     trajectory_tree_->setItemWidget(collisionItem, 0, collisionsVisibleBox);
01949     connect(collisionsVisibleBox, SIGNAL(clicked(bool)), this, SLOT(trajectoryCollisionsVisibleButtonClicked(bool)));
01950 
01951     QStringList visibleList;
01952     visibleList.append("");
01953     QTreeWidgetItem* visibleItem = new QTreeWidgetItem(visibleList);
01954     visibleItem->setToolTip(0, nameItem->text(0));
01955     QCheckBox* visibleBox = new QCheckBox(trajectory_tree_);
01956     visibleBox->setText("Visible");
01957     visibleBox->setChecked(trajectory.isVisible());
01958     nameItem->insertChild(1, visibleItem);
01959     trajectory_tree_->setItemWidget(visibleItem, 0, visibleBox);
01960     visibleBox->setToolTip(nameItem->text(0));
01961     connect(visibleBox, SIGNAL(clicked(bool)), this, SLOT(trajectoryVisibleButtonClicked(bool)));
01962 
01963     QStringList colorList;
01964     colorList.append("Color");
01965     colorList.append("");
01966     QTreeWidgetItem* colorItem = new QTreeWidgetItem(colorList);
01967     colorItem->setToolTip(0, nameItem->text(0));
01968     QPushButton* colorButton = new QPushButton(trajectory_tree_);
01969 
01970     std::stringstream colorStream;
01971     colorStream<< "(" << (int)(trajectory.getColor().r*255) <<" , ";
01972     colorStream << (int)(trajectory.getColor().g*255) << " , ";
01973     colorStream << (int)(trajectory.getColor().b*255) << ")";
01974 
01975     colorButton->setText(QString::fromStdString(colorStream.str()));
01976     colorButton->setToolTip(nameItem->text(0));
01977     colorButton->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
01978     connect(colorButton, SIGNAL(clicked()), this, SLOT(trajectoryColorButtonClicked()));
01979 
01980     nameItem->insertChild(2, colorItem);
01981     trajectory_tree_->setItemWidget(colorItem, 1, colorButton);
01982 
01983     QStringList trajectoryTypeList;
01984     trajectoryTypeList.append("Trajectory Rendering Mode");
01985     QTreeWidgetItem* trajectoryRenderTypeItem = new QTreeWidgetItem(trajectoryTypeList);
01986     trajectoryRenderTypeItem->setToolTip(0, nameItem->text(0));
01987     QComboBox* trajectoryRenderTypeBox = new QComboBox(trajectory_tree_);
01988     QStringList traj_items;
01989     traj_items.append("Kinematic");
01990     traj_items.append("Temporal");
01991     trajectoryRenderTypeBox->addItems(traj_items);
01992     trajectoryRenderTypeBox->setToolTip(nameItem->text(0));
01993     connect(trajectoryRenderTypeBox, SIGNAL(currentIndexChanged(int)), this, SLOT(trajectoryRenderTypeChanged(const int&)));
01994     nameItem->insertChild(3,trajectoryRenderTypeItem);
01995     trajectory_tree_->setItemWidget(trajectoryRenderTypeItem, 1, trajectoryRenderTypeBox);
01996 
01997     switch(trajectory.getTrajectoryRenderType())
01998     {
01999       case Kinematic:
02000         trajectoryRenderTypeBox->setCurrentIndex(0);
02001         break;
02002       case Temporal:
02003         trajectoryRenderTypeBox->setCurrentIndex(1);
02004         break;
02005     }
02006 
02007     QStringList renderTypeList;
02008     renderTypeList.append("Model Rendering Mode");
02009     QTreeWidgetItem* renderTypeItem = new QTreeWidgetItem(renderTypeList);
02010     renderTypeItem->setToolTip(0, nameItem->text(0));
02011 
02012     QComboBox* renderTypeBox = new QComboBox(trajectory_tree_);
02013     QStringList items;
02014     items.append("Collision Mesh");
02015     items.append("Visual Mesh");
02016     items.append("Padding Mesh");
02017     renderTypeBox->addItems(items);
02018     renderTypeBox->setToolTip(nameItem->text(0));
02019     connect(renderTypeBox, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(modelRenderTypeChanged(const QString&)));
02020 
02021     nameItem->insertChild(4, renderTypeItem);
02022     trajectory_tree_->setItemWidget(renderTypeItem, 1, renderTypeBox);
02023 
02024     switch(trajectory.getRenderType())
02025     {
02026       case CollisionMesh:
02027         renderTypeBox->setCurrentIndex(0);
02028         break;
02029       case VisualMesh:
02030         renderTypeBox->setCurrentIndex(1);
02031         break;
02032       case PaddingMesh:
02033         renderTypeBox->setCurrentIndex(2);
02034         break;
02035     }
02036   }
02037 }
02038 
02039 void WarehouseViewer::trajectoryCollisionsVisibleButtonClicked(bool checked)
02040 {
02041   QObject* sender = QObject::sender();
02042   QCheckBox* button = qobject_cast<QCheckBox*> (sender);
02043 
02044   if(button != NULL)
02045   {
02046     std::string trajectoryID = button->toolTip().toStdString();
02047     TrajectoryData& data = trajectory_map_[selected_motion_plan_name_][trajectoryID];
02048     data.setCollisionsVisible(checked);
02049   }
02050 }
02051 
02052 void WarehouseViewer::trajectoryVisibleButtonClicked(bool checked)
02053 {
02054   QObject* sender = QObject::sender();
02055   QCheckBox* button = qobject_cast<QCheckBox*> (sender);
02056 
02057   if(button != NULL)
02058   {
02059     std::string trajectoryID = button->toolTip().toStdString();
02060     TrajectoryData& data = trajectory_map_[selected_motion_plan_name_][trajectoryID];
02061     data.setVisible(checked);
02062   }
02063 }
02064 
02065 void WarehouseViewer::trajectoryColorButtonClicked()
02066 {
02067   QObject* sender = QObject::sender();
02068   QPushButton* button = qobject_cast<QPushButton*>(sender);
02069 
02070   if(button != NULL)
02071   {
02072     std::string trajectoryID = button->toolTip().toStdString();
02073     TrajectoryData& data = trajectory_map_[selected_motion_plan_name_][trajectoryID];
02074     QColor col(data.getColor().r*255, data.getColor().g*255, data.getColor().b*255);
02075     QColor colorSelected = QColorDialog::getColor(col, this);
02076     if(!colorSelected.isValid())
02077     {
02078       return;
02079     }
02080     std::stringstream colorStream;
02081     colorStream<< "(" << colorSelected.red()<<" , ";
02082     colorStream << colorSelected.green()<< " , ";
02083     colorStream << colorSelected.blue() << ")";
02084 
02085     button->setText(QString::fromStdString(colorStream.str()));
02086 
02087     std_msgs::ColorRGBA trajColor;
02088     trajColor.r = colorSelected.redF();
02089     trajColor.g = colorSelected.greenF();
02090     trajColor.b = colorSelected.blueF();
02091     trajColor.a = 0.5f;
02092     data.setColor(trajColor);
02093     data.refreshColors();
02094   }
02095 }
02096 
02097 void WarehouseViewer::onPlanningSceneLoaded(int scene, int numScenes)
02098 {
02099   emit changeProgress((int)((100.0f)*((float)(scene + 1)/ (float)numScenes)));
02100 }
02101 
02102 void WarehouseViewer::createPlanningSceneTable()
02103 {
02104   loadAllWarehouseData();
02105   warehouse_data_loaded_once_ = true;
02106   planning_scene_table_->clear();
02107   int count = 0;
02108   for(map<string, PlanningSceneData>::iterator it = planning_scene_map_.begin(); it != planning_scene_map_.end(); it++)
02109   {
02110     count ++;
02111   }
02112   planning_scene_table_->setRowCount(count);
02113   planning_scene_table_->setColumnCount(4);
02114   QStringList labels;
02115   labels.append("Host");
02116   labels.append("Name");
02117   labels.append("Timestamp");
02118   labels.append("Notes");
02119 
02120   ROS_INFO("Num Planning Scenes: %d", planning_scene_table_->rowCount());
02121 
02122   int r = 0;
02123   for(map<string, PlanningSceneData>::iterator it = planning_scene_map_.begin(); it != planning_scene_map_.end(); it++)
02124   {
02125     PlanningSceneData& data = it->second;
02126 
02127     QTableWidgetItem* hostItem = new QTableWidgetItem(QString::fromStdString(data.getHostName()));
02128     hostItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02129     planning_scene_table_->setItem(r, 0, hostItem);
02130 
02131     PlanningSceneNameTableItem* nameItem = new PlanningSceneNameTableItem(QString::fromStdString(data.getName()));
02132     nameItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02133     planning_scene_table_->setItem(r, 1, nameItem);
02134 
02135     QDateTime time;
02136     time.setTime_t((unsigned int)(data.getTimeStamp().toSec()));
02137     stringstream timestampStream;
02138     timestampStream << time.toString().toStdString();
02139 
02140     PlanningSceneDateTableItem* timeItem = new PlanningSceneDateTableItem(QString::fromStdString(timestampStream.str()));
02141     timeItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02142     planning_scene_table_->setItem(r, 2, timeItem);
02143 
02144     stringstream notesStream;
02145 
02146     vector<ArmNavigationErrorCodes>& errorCodes = data.getErrorCodes();
02147 
02148 
02149     if(errorCodes.size() == 0)
02150     {
02151       notesStream << "No Outcomes";
02152     }
02153     else
02154     {
02155       notesStream << "Last Outcome: " << armNavigationErrorCodeToString(errorCodes.back());
02156     }
02157 
02158     QTableWidgetItem* notesItem = new QTableWidgetItem(QString::fromStdString(notesStream.str()));
02159     notesItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02160     planning_scene_table_->setItem(r, 3, notesItem);
02161 
02162     if(errorCodes.size() != 0)
02163     {
02164       if(errorCodes.back().val != ArmNavigationErrorCodes::SUCCESS)
02165       {
02166         notesItem->setTextColor(QColor(180, 0, 0));
02167       }
02168     }
02169 
02170     r++;
02171   }
02172 
02173   planning_scene_table_->sortByColumn(1, Qt::DescendingOrder);
02174   connect(planning_scene_table_->horizontalHeader(), SIGNAL(sectionClicked(int)), this, SLOT(planningSceneTableHeaderClicked(int)));
02175   planning_scene_table_->horizontalHeader()->resizeSections(QHeaderView::Stretch);
02176   planning_scene_table_->setHorizontalHeaderLabels(labels);
02177   planning_scene_table_->setMinimumWidth(1000);
02178 
02179   emit allScenesLoaded();
02180 }
02181 
02182 void WarehouseViewer::refreshPlanningSceneDialog()
02183 {
02184   planning_scene_table_->horizontalHeader()->resizeSections(QHeaderView::Stretch);
02185 }
02186 
02187 void WarehouseViewer::planningSceneTableHeaderClicked(int col) {
02188   planning_scene_table_->sortByColumn(col);
02189 }
02190 
02191 void WarehouseViewer::motionPlanJointControlsActiveButtonClicked(bool checked)
02192 {
02193   QObject* sender = QObject::sender();
02194   QCheckBox* box = dynamic_cast<QCheckBox*>(sender);
02195 
02196   if(box == NULL)
02197   {
02198     return;
02199   }
02200 
02201   std::string ID = box->toolTip().toStdString();
02202 
02203   if(motion_plan_map_.find(ID) == motion_plan_map_.end())
02204   {
02205     return;
02206   }
02207 
02208   MotionPlanRequestData& data = motion_plan_map_[ID];
02209   data.setJointControlsVisible(checked, this);
02210   interactive_marker_server_->applyChanges();
02211 }
02212 
02213 void WarehouseViewer::createNewObjectPressed()
02214 {
02215   collision_object_name_->setText(generateNewCollisionObjectId().c_str());
02216   new_object_dialog_->show();
02217 }
02218 
02219 void WarehouseViewer::createNewObjectDialog()
02220 {
02221   new_object_dialog_ = new QDialog(this);
02222   QVBoxLayout* layout = new QVBoxLayout(new_object_dialog_);
02223   QGroupBox* panel = new QGroupBox(new_object_dialog_);
02224   panel->setTitle("New Collision Object");
02225 
02226   QVBoxLayout* panelLayout = new QVBoxLayout(panel);
02227 
02228   collision_object_name_ = new QLineEdit(new_object_dialog_);
02229   panelLayout->addWidget(collision_object_name_);
02230 
02231   collision_object_type_box_ = new QComboBox(new_object_dialog_);
02232   collision_object_type_box_->addItem("Box");
02233   collision_object_type_box_->addItem("Cylinder");
02234   collision_object_type_box_->addItem("Sphere");
02235   collision_object_type_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02236 
02237   panelLayout->addWidget(collision_object_type_box_);
02238 
02239   QGroupBox* scaleBox = new QGroupBox(panel);
02240   scaleBox->setTitle("Scale (x,y,z) cm");
02241   QHBoxLayout* scaleLayout = new QHBoxLayout(scaleBox);
02242 
02243   collision_object_scale_x_box_ = new QSpinBox(scaleBox);
02244   collision_object_scale_y_box_ = new QSpinBox(scaleBox);
02245   collision_object_scale_z_box_ = new QSpinBox(scaleBox);
02246   collision_object_scale_x_box_->setRange(1, 10000);
02247   collision_object_scale_y_box_->setRange(1, 10000);
02248   collision_object_scale_z_box_->setRange(1, 10000);
02249   collision_object_scale_x_box_->setValue(10);
02250   collision_object_scale_y_box_->setValue(10);
02251   collision_object_scale_z_box_->setValue(10);
02252   collision_object_scale_x_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02253   collision_object_scale_y_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02254   collision_object_scale_z_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02255 
02256   scaleLayout->addWidget(collision_object_scale_x_box_);
02257   scaleLayout->addWidget(collision_object_scale_y_box_);
02258   scaleLayout->addWidget(collision_object_scale_z_box_);
02259 
02260   scaleBox->setLayout(scaleLayout);
02261   panelLayout->addWidget(scaleBox);
02262 
02263   QGroupBox* posBox = new QGroupBox(panel);
02264   posBox->setTitle("Position (x,y,z) cm");
02265   QHBoxLayout* posLayout = new QHBoxLayout(posBox);
02266 
02267 
02268   collision_object_pos_x_box_ = new QSpinBox(posBox);
02269   collision_object_pos_y_box_ = new QSpinBox(posBox);
02270   collision_object_pos_z_box_ = new QSpinBox(posBox);
02271   collision_object_pos_x_box_->setRange(-10000, 10000);
02272   collision_object_pos_y_box_->setRange(-10000, 10000);
02273   collision_object_pos_z_box_->setRange(-10000, 10000);
02274   collision_object_pos_x_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02275   collision_object_pos_y_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02276   collision_object_pos_z_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02277 
02278   posLayout->addWidget(collision_object_pos_x_box_);
02279   posLayout->addWidget(collision_object_pos_y_box_);
02280   posLayout->addWidget(collision_object_pos_z_box_);
02281 
02282   posBox->setLayout(posLayout);
02283   panelLayout->addWidget(posBox);
02284 
02285   panel->setLayout(panelLayout);
02286   layout->addWidget(panel);
02287 
02288   object_color_button_ = new QPushButton(new_object_dialog_);
02289 
02290   std::stringstream colorStream;
02291   colorStream<< "Color: (" << (int)(last_collision_object_color_.r*255) <<" , ";
02292   colorStream << (int)(last_collision_object_color_.g*255) << " , ";
02293   colorStream << (int)(last_collision_object_color_.b*255) << ")";
02294 
02295   object_color_button_->setText(QString::fromStdString(colorStream.str()));
02296   object_color_button_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
02297   connect(object_color_button_, SIGNAL(clicked()), this, SLOT(objectColorButtonPressed()));
02298 
02299   make_object_button_ = new QPushButton(new_object_dialog_);
02300   make_object_button_->setText("Create...");
02301   make_object_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02302   connect(make_object_button_, SIGNAL(clicked()), this, SLOT(createObjectConfirmedPressed()));
02303 
02304   layout->addWidget(object_color_button_);
02305   layout->addWidget(make_object_button_);
02306   new_object_dialog_->setLayout(layout);
02307 
02308 }
02309 
02310 void WarehouseViewer::createNewMeshPressed()
02311 {
02312   mesh_object_name_->setText(generateNewCollisionObjectId().c_str());
02313   new_mesh_dialog_->show();
02314 }
02315 
02316 void WarehouseViewer::meshFileSelected(const QString& filename)
02317 {
02318   mesh_filename_field_->setText(filename);
02319 }
02320 
02321 void WarehouseViewer::createMeshConfirmedPressed()
02322 {
02323   geometry_msgs::Pose pose;
02324   pose.position.x = (float)mesh_object_pos_x_box_->value() / 100.0f;
02325   pose.position.y = (float)mesh_object_pos_y_box_->value() / 100.0f;
02326   pose.position.z = (float)mesh_object_pos_z_box_->value() / 100.0f;
02327   pose.orientation.x = 0;
02328   pose.orientation.y = 0;
02329   pose.orientation.z = 0;
02330   pose.orientation.w = 1;
02331 
02332   tf::Vector3 scale;
02333   scale.setX(mesh_object_scale_x_box_->value());
02334   scale.setY(mesh_object_scale_y_box_->value());
02335   scale.setZ(mesh_object_scale_z_box_->value());
02336 
02337   createMeshObject(mesh_object_name_->text().toStdString(), pose, "file://"+mesh_filename_field_->text().toStdString(), scale, last_mesh_object_color_);
02338   new_mesh_dialog_->hide();
02339 }
02340 
02341 void WarehouseViewer::createNewMeshDialog()
02342 {
02343   new_mesh_dialog_ = new QDialog(this);
02344   QVBoxLayout* layout = new QVBoxLayout(new_mesh_dialog_);
02345   QGroupBox* panel = new QGroupBox(new_mesh_dialog_);
02346   panel->setTitle("New Mesh Collision Object");
02347 
02348   QVBoxLayout* panelLayout = new QVBoxLayout(panel);
02349   mesh_object_name_ = new QLineEdit(this);
02350   panelLayout->addWidget(mesh_object_name_);
02351 
02352   mesh_filename_field_ = new QLineEdit(this);
02353   mesh_filename_field_->setText("<mesh_filename>");
02354   panelLayout->addWidget(mesh_filename_field_);
02355 
02356   QPushButton* selectFileButton = new QPushButton(this);
02357   selectFileButton->setText("Select Mesh file ...");
02358 
02359   file_selector_ = new QFileDialog(this);
02360   file_selector_->setFileMode(QFileDialog::ExistingFile);
02361   file_selector_->setOption(QFileDialog::ReadOnly, true);
02362   QStringList filters;
02363   filters << "Mesh files (*.stl *.stla *.stlb *.dae *.ply)";
02364   file_selector_->setNameFilters(filters);
02365 
02366   connect(file_selector_, SIGNAL(fileSelected(const QString&)), this, SLOT(meshFileSelected(const QString&)));
02367   connect(selectFileButton, SIGNAL(clicked()), file_selector_, SLOT(open()));
02368   panelLayout->addWidget(selectFileButton);
02369 
02370   QGroupBox* posBox = new QGroupBox(panel);
02371   posBox->setTitle("Position (x,y,z) cm");
02372   QHBoxLayout* posLayout = new QHBoxLayout(posBox);
02373 
02374   mesh_object_pos_x_box_ = new QSpinBox(posBox);
02375   mesh_object_pos_y_box_ = new QSpinBox(posBox);
02376   mesh_object_pos_z_box_ = new QSpinBox(posBox);
02377   mesh_object_pos_x_box_->setRange(-10000, 10000);
02378   mesh_object_pos_y_box_->setRange(-10000, 10000);
02379   mesh_object_pos_z_box_->setRange(-10000, 10000);
02380   mesh_object_pos_x_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02381   mesh_object_pos_y_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02382   mesh_object_pos_z_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02383 
02384   posLayout->addWidget(mesh_object_pos_x_box_);
02385   posLayout->addWidget(mesh_object_pos_y_box_);
02386   posLayout->addWidget(mesh_object_pos_z_box_);
02387   posBox->setLayout(posLayout);
02388 
02389   QGroupBox* scaleBox = new QGroupBox(panel);
02390   scaleBox->setTitle("Scale (x,y,z)");
02391   QHBoxLayout* scaleLayout = new QHBoxLayout(scaleBox);
02392 
02393   mesh_object_scale_x_box_ = new QDoubleSpinBox(scaleBox);
02394   mesh_object_scale_y_box_ = new QDoubleSpinBox(scaleBox);
02395   mesh_object_scale_z_box_ = new QDoubleSpinBox(scaleBox);
02396   mesh_object_scale_x_box_->setDecimals(4);
02397   mesh_object_scale_x_box_->setSingleStep(.0001);
02398   mesh_object_scale_y_box_->setDecimals(4);
02399   mesh_object_scale_y_box_->setSingleStep(.0001);
02400   mesh_object_scale_z_box_->setDecimals(4);
02401   mesh_object_scale_z_box_->setSingleStep(.0001);
02402   mesh_object_scale_x_box_->setRange(0, 10.0);
02403   mesh_object_scale_y_box_->setRange(0, 10.0);
02404   mesh_object_scale_z_box_->setRange(0, 10.0);
02405   mesh_object_scale_x_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02406   mesh_object_scale_y_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02407   mesh_object_scale_z_box_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02408   mesh_object_scale_x_box_->setValue(1.0);
02409   mesh_object_scale_y_box_->setValue(1.0);
02410   mesh_object_scale_z_box_->setValue(1.0);
02411 
02412   scaleLayout->addWidget(mesh_object_scale_x_box_);
02413   scaleLayout->addWidget(mesh_object_scale_y_box_);
02414   scaleLayout->addWidget(mesh_object_scale_z_box_);
02415 
02416   scaleBox->setLayout(scaleLayout);
02417 
02418   panelLayout->addWidget(posBox);
02419   panelLayout->addWidget(scaleBox);
02420 
02421   panel->setLayout(panelLayout);
02422   layout->addWidget(panel);
02423 
02424   mesh_color_button_ = new QPushButton(new_mesh_dialog_);
02425 
02426   std::stringstream colorStream;
02427   colorStream<< "Color: (" << (int)(last_mesh_object_color_.r*255) <<" , ";
02428   colorStream << (int)(last_mesh_object_color_.g*255) << " , ";
02429   colorStream << (int)(last_mesh_object_color_.b*255) << ")";
02430 
02431   mesh_color_button_->setText(QString::fromStdString(colorStream.str()));
02432   mesh_color_button_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
02433   connect(mesh_color_button_, SIGNAL(clicked()), this, SLOT(meshColorButtonPressed()));
02434 
02435   make_mesh_button_ = new QPushButton(new_mesh_dialog_);
02436   make_mesh_button_->setText("Create...");
02437   make_mesh_button_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02438   connect(make_mesh_button_, SIGNAL(clicked()), this, SLOT(createMeshConfirmedPressed()));
02439 
02440   layout->addWidget(mesh_color_button_);
02441   layout->addWidget(make_mesh_button_);
02442   new_mesh_dialog_->setLayout(layout);
02443 
02444 }
02445 
02446 void WarehouseViewer::meshColorButtonPressed()
02447 {
02448 
02449   QColor selected = QColorDialog::getColor(QColor(last_mesh_object_color_.r*255, last_mesh_object_color_.g*255, last_mesh_object_color_.b*255), new_object_dialog_);
02450 
02451   if(selected.isValid())
02452   {
02453     last_mesh_object_color_.r = selected.redF();
02454     last_mesh_object_color_.g = selected.greenF();
02455     last_mesh_object_color_.b = selected.blueF();
02456     last_mesh_object_color_.a = 1.0;
02457 
02458     std::stringstream colorStream;
02459     colorStream<< "Color: (" << (int)(last_mesh_object_color_.r*255) <<" , ";
02460     colorStream << (int)(last_mesh_object_color_.g*255) << " , ";
02461     colorStream << (int)(last_mesh_object_color_.b*255) << ")";
02462 
02463     mesh_color_button_->setText(QString::fromStdString(colorStream.str()));
02464   }
02465 }
02466 
02467 void WarehouseViewer::objectColorButtonPressed()
02468 {
02469 
02470   QColor selected = QColorDialog::getColor(QColor(last_collision_object_color_.r*255, last_collision_object_color_.g*255, last_collision_object_color_.b*255), new_object_dialog_);
02471 
02472   if(selected.isValid())
02473   {
02474     last_collision_object_color_.r = selected.redF();
02475     last_collision_object_color_.g = selected.greenF();
02476     last_collision_object_color_.b = selected.blueF();
02477     last_collision_object_color_.a = 1.0;
02478 
02479     std::stringstream colorStream;
02480     colorStream<< "Color: (" << (int)(last_collision_object_color_.r*255) <<" , ";
02481     colorStream << (int)(last_collision_object_color_.g*255) << " , ";
02482     colorStream << (int)(last_collision_object_color_.b*255) << ")";
02483 
02484     object_color_button_->setText(QString::fromStdString(colorStream.str()));
02485   }
02486 }
02487 
02488 void WarehouseViewer::createObjectConfirmedPressed()
02489 {
02490   geometry_msgs::Pose pose;
02491   pose.position.x = (float)collision_object_pos_x_box_->value() / 100.0f;
02492   pose.position.y = (float)collision_object_pos_y_box_->value() / 100.0f;
02493   pose.position.z = (float)collision_object_pos_z_box_->value() / 100.0f;
02494   pose.orientation.x = 0;
02495   pose.orientation.y = 0;
02496   pose.orientation.z = 0;
02497   pose.orientation.w = 1;
02498 
02499   std::string name = collision_object_type_box_->currentText().toStdString();
02500   PlanningSceneEditor::GeneratedShape shape;
02501   if(name == "Box")
02502   {
02503     shape = PlanningSceneEditor::Box;
02504   }
02505   if(name == "Cylinder")
02506   {
02507     shape = PlanningSceneEditor::Cylinder;
02508   }
02509   if(name == "Sphere")
02510   {
02511     shape = PlanningSceneEditor::Sphere;
02512   }
02513 
02514 
02515   createCollisionObject(collision_object_name_->text().toStdString(),
02516                         pose, shape, (float)collision_object_scale_x_box_->value() / 100.0f,
02517                         (float)collision_object_scale_y_box_->value() / 100.0f,
02518                         (float)collision_object_scale_z_box_->value() / 100.0f, last_collision_object_color_);
02519   new_object_dialog_->hide();
02520 }
02521 
02522 void WarehouseViewer::createRequestDialog()
02523 {
02524   new_request_dialog_ = new QDialog(this);
02525   QVBoxLayout* layout = new QVBoxLayout(new_request_dialog_);
02526   QGroupBox* box = new QGroupBox(new_request_dialog_);
02527   QVBoxLayout* boxLayout = new QVBoxLayout(box);
02528   box->setTitle("New Motion Plan Request");
02529   QLabel* boxLabel = new QLabel(box);
02530   boxLabel->setText("Planning Group:");
02531   request_group_name_box_ = new QComboBox(box);
02532 
02533   if(params_.right_arm_group_ != "none")
02534   {
02535     request_group_name_box_->addItem(QString::fromStdString(params_.right_arm_group_));
02536   }
02537 
02538   if(params_.left_arm_group_ != "none")
02539   {
02540     request_group_name_box_->addItem(QString::fromStdString(params_.left_arm_group_));
02541   }
02542 
02543   create_request_from_robot_box_ = new QCheckBox(box);
02544   create_request_from_robot_box_->setChecked(params_.use_robot_data_);
02545   create_request_from_robot_box_->setText("Start From Current Robot State");
02546 
02547   boxLayout->addWidget(boxLabel);
02548   boxLayout->addWidget(request_group_name_box_);
02549   boxLayout->addWidget(create_request_from_robot_box_);
02550 
02551   QPushButton* createRequestButton = new QPushButton(box);
02552   createRequestButton->setText("Create ...");
02553   createRequestButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02554   boxLayout->addWidget(createRequestButton);
02555 
02556   connect(createRequestButton, SIGNAL(clicked()), this, SLOT(createRequestPressed()));
02557   layout->addWidget(box);
02558   new_request_dialog_->setLayout(layout);
02559 }
02560 
02561 void WarehouseViewer::createRequestPressed()
02562 {
02563   std::string group_name = request_group_name_box_->currentText().toStdString();
02564   std::string end_effector_name = "";
02565 
02566   if(group_name == params_.right_arm_group_)
02567   {
02568     end_effector_name = params_.right_ik_link_;
02569   }
02570   else if(group_name == params_.left_arm_group_)
02571   {
02572     end_effector_name = params_.left_ik_link_;
02573   }
02574   else
02575   {
02576     return;
02577   }
02578   createNewMotionPlanRequest(group_name, end_effector_name);
02579   createMotionPlanTable();
02580   new_request_dialog_->close();
02581 }
02582 
02583 void WarehouseViewer::motionPlanHasPathConstraintsButtonClicked(bool checked) 
02584 {
02585   QObject* sender = QObject::sender();
02586   QCheckBox* box = dynamic_cast<QCheckBox*>(sender);
02587   
02588   if(box == NULL)
02589   {
02590     ROS_WARN_STREAM("Path constraints dispatch failed");
02591     return;
02592   }
02593   
02594   std::string ID = box->toolTip().toStdString();
02595 
02596   if(motion_plan_map_.find(ID) == motion_plan_map_.end())
02597   {
02598     ROS_WARN_STREAM("No MPR ID " << ID << " in path constraints");
02599     return;
02600   }
02601   
02602   MotionPlanRequestData& data = motion_plan_map_[ID];
02603   
02604   data.setPathConstraints(checked);
02605   std::map<std::string, double> vals;
02606   data.getGoalState()->getKinematicStateValues(vals);
02607   data.setGoalStateValues(vals);
02608 }
02609 
02610 void WarehouseViewer::setPathConstraintsButtonClicked() 
02611 {
02612   QObject* sender = QObject::sender();
02613   QPushButton* box = dynamic_cast<QPushButton*>(sender);
02614   
02615   if(box == NULL)
02616   {
02617     ROS_WARN_STREAM("Dispatch not working");
02618     return;
02619   }
02620   
02621   std::string ID = box->toolTip().toStdString();
02622 
02623   if(motion_plan_map_.find(ID) == motion_plan_map_.end())
02624   {
02625     ROS_WARN_STREAM("No motion plan id " << ID);
02626     return;
02627   }
02628   
02629   MotionPlanRequestData& data = motion_plan_map_[ID];
02630 
02631   createSetPathConstraintsDialog(data);
02632   int res = set_path_constraints_dialog_->exec();
02633   
02634   if(res == QDialog::Accepted) {
02635     data.setConstrainRoll(constrain_roll_check_box_->isChecked());
02636     data.setConstrainPitch(constrain_pitch_check_box_->isChecked());
02637     data.setConstrainYaw(constrain_yaw_check_box_->isChecked());
02638 
02639     if(constrain_roll_check_box_->isChecked()) {
02640       data.setRollTolerance(constrain_roll_tolerance_->value());
02641     }
02642     if(constrain_pitch_check_box_->isChecked()) {
02643       data.setPitchTolerance(constrain_pitch_tolerance_->value());
02644     }
02645     if(constrain_yaw_check_box_->isChecked()) {
02646       data.setYawTolerance(constrain_yaw_tolerance_->value());
02647     }
02648     std::map<std::string, double> vals;
02649     data.getGoalState()->getKinematicStateValues(vals);
02650     data.setGoalStateValues(vals);
02651   }
02652 
02653   delete set_path_constraints_dialog_;
02654 
02655 }
02656 
02657 void WarehouseViewer::createSetPathConstraintsDialog(MotionPlanRequestData& data) {
02658   set_path_constraints_dialog_ = new QDialog(this);
02659   std::stringstream window_title;
02660   window_title << "Set RPY Path Constraints for " << data.getName();
02661   set_path_constraints_dialog_->setWindowTitle(QString::fromStdString(window_title.str()));
02662 
02663   QVBoxLayout* layout = new QVBoxLayout(set_path_constraints_dialog_);
02664 
02665   QGridLayout* grid_layout = new QGridLayout(set_path_constraints_dialog_);
02666 
02667   QLabel* con_label = new QLabel(set_path_constraints_dialog_);
02668   con_label->setText("Enable Constraints");
02669   
02670   QLabel* tol_label = new QLabel(set_path_constraints_dialog_);
02671   tol_label->setText("Absolute Tolerance");
02672   
02673   grid_layout->addWidget(con_label, 0, 0);
02674   grid_layout->addWidget(tol_label, 0, 1);
02675 
02676   constrain_roll_check_box_ = new QCheckBox(set_path_constraints_dialog_);
02677   constrain_roll_check_box_->setChecked(data.getConstrainRoll());
02678   constrain_roll_check_box_->setText("Constrain Roll");
02679 
02680   constrain_pitch_check_box_ = new QCheckBox(set_path_constraints_dialog_);
02681   constrain_pitch_check_box_->setChecked(data.getConstrainPitch());
02682   constrain_pitch_check_box_->setText("Constrain Pitch");
02683 
02684   constrain_yaw_check_box_ = new QCheckBox(set_path_constraints_dialog_);
02685   constrain_yaw_check_box_->setChecked(data.getConstrainYaw());
02686   constrain_yaw_check_box_->setText("Constrain Yaw");
02687 
02688   grid_layout->addWidget(constrain_roll_check_box_, 1, 0);
02689   grid_layout->addWidget(constrain_pitch_check_box_, 2, 0);
02690   grid_layout->addWidget(constrain_yaw_check_box_, 3, 0);
02691   
02692   constrain_roll_tolerance_ = new QDoubleSpinBox(set_path_constraints_dialog_);
02693   constrain_roll_tolerance_->setDecimals(2);
02694   constrain_roll_tolerance_->setRange(0, M_PI);
02695   constrain_roll_tolerance_->setSingleStep(.01);
02696   constrain_roll_tolerance_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02697   constrain_roll_tolerance_->setValue(data.getRollTolerance());
02698   constrain_roll_tolerance_->setEnabled(constrain_roll_check_box_->isChecked());
02699 
02700   constrain_pitch_tolerance_ = new QDoubleSpinBox(set_path_constraints_dialog_);
02701   constrain_pitch_tolerance_->setDecimals(2);
02702   constrain_pitch_tolerance_->setRange(0, M_PI);
02703   constrain_pitch_tolerance_->setSingleStep(.01);
02704   constrain_pitch_tolerance_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02705   constrain_pitch_tolerance_->setValue(data.getPitchTolerance());
02706   constrain_pitch_tolerance_->setEnabled(constrain_pitch_check_box_->isChecked());
02707 
02708   constrain_yaw_tolerance_ = new QDoubleSpinBox(set_path_constraints_dialog_);
02709   constrain_yaw_tolerance_->setDecimals(2);
02710   constrain_yaw_tolerance_->setRange(0, M_PI);
02711   constrain_yaw_tolerance_->setSingleStep(.01);
02712   constrain_yaw_tolerance_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02713   constrain_yaw_tolerance_->setValue(data.getYawTolerance());
02714   constrain_yaw_tolerance_->setEnabled(constrain_yaw_check_box_->isChecked());
02715 
02716   connect(constrain_roll_check_box_, SIGNAL(clicked(bool)), constrain_roll_tolerance_, SLOT(setEnabled(bool)));
02717   connect(constrain_pitch_check_box_, SIGNAL(clicked(bool)), constrain_pitch_tolerance_, SLOT(setEnabled(bool)));
02718   connect(constrain_yaw_check_box_, SIGNAL(clicked(bool)), constrain_yaw_tolerance_, SLOT(setEnabled(bool)));
02719 
02720   grid_layout->addWidget(constrain_roll_tolerance_, 1, 1);  
02721   grid_layout->addWidget(constrain_pitch_tolerance_, 2, 1);  
02722   grid_layout->addWidget(constrain_yaw_tolerance_, 3, 1);  
02723 
02724   layout->addLayout(grid_layout);
02725 
02726   QDialogButtonBox* qdb = new QDialogButtonBox(QDialogButtonBox::Cancel | QDialogButtonBox::Ok);
02727   connect(qdb, SIGNAL(accepted()), set_path_constraints_dialog_, SLOT(accept()));
02728   connect(qdb, SIGNAL(rejected()), set_path_constraints_dialog_, SLOT(reject()));
02729 
02730   layout->addWidget(qdb);
02731 
02732   set_path_constraints_dialog_->setLayout(layout);
02733 }
02734 
02735 void WarehouseViewer::modelRenderTypeChanged(const QString& type)
02736 {
02737   QObject* sender = QObject::sender();
02738   QComboBox* box = dynamic_cast<QComboBox*>(sender);
02739   std::string ID = box->toolTip().toStdString();
02740 
02741   if(!hasTrajectory(selected_motion_plan_name_,
02742                    ID)) {
02743     ROS_WARN_STREAM("Changing render type when we don't have trajectory");
02744     return;
02745   }
02746 
02747   TrajectoryData& data = trajectory_map_[selected_motion_plan_name_][ID];
02748 
02749   if(type == "Visual Mesh")
02750   {
02751     data.setRenderType(VisualMesh);
02752   }
02753   else if(type == "Collision Mesh")
02754   {
02755     data.setRenderType(CollisionMesh);
02756   }
02757   else
02758   {
02759     data.setRenderType(PaddingMesh);
02760   }
02761 }
02762 
02763 void WarehouseViewer::trajectoryRenderTypeChanged(const int& index)
02764 {
02765   QObject* sender = QObject::sender();
02766   QComboBox* box = dynamic_cast<QComboBox*>(sender);
02767   std::string ID = box->toolTip().toStdString();
02768 
02769   if(!hasTrajectory(selected_motion_plan_name_,
02770                    ID)) {
02771     ROS_WARN_STREAM("Changing trajectory render type when we don't have trajectory");
02772     return;
02773   }
02774 
02775   TrajectoryData& data = trajectory_map_[selected_motion_plan_name_][ID];
02776 
02777   if(index == 0)
02778   {
02779     data.setTrajectoryRenderType(Kinematic);
02780   }
02781   else if(index == 1)
02782   {
02783     data.setTrajectoryRenderType(Temporal);
02784   }
02785   else
02786   {
02787     data.setTrajectoryRenderType(Kinematic);
02788   }
02789 }
02790 
02791 
02792 void WarehouseViewer::motionPlanRenderTypeChanged(const QString& type)
02793 {
02794   QObject* sender = QObject::sender();
02795   QComboBox* box = dynamic_cast<QComboBox*>(sender);
02796   std::string ID = box->toolTip().toStdString();
02797 
02798   if(motion_plan_map_.find(ID) == motion_plan_map_.end())
02799   {
02800     return;
02801   }
02802 
02803   MotionPlanRequestData& data = motion_plan_map_[ID];
02804 
02805   if(type == "Visual Mesh")
02806   {
02807     data.setRenderType(VisualMesh);
02808   }
02809   else if(type == "Collision Mesh")
02810   {
02811     data.setRenderType(CollisionMesh);
02812   }
02813   else
02814   {
02815     data.setRenderType(PaddingMesh);
02816   }
02817 }
02818 
02819 void WarehouseViewer::planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode)
02820 {
02821   if(errorCode.val != ArmNavigationErrorCodes::SUCCESS)
02822   {
02823     emit plannerFailure(errorCode.val);
02824   } else {
02825     selectTrajectory(selected_trajectory_name_);
02826   }
02827 }
02828 
02829 
02830 void WarehouseViewer::filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode)
02831 {
02832   if(errorCode.val != ArmNavigationErrorCodes::SUCCESS)
02833   {
02834     emit filterFailure(errorCode.val);
02835   } else {
02836     selectTrajectory(selected_trajectory_name_);
02837   }
02838 }
02839 
02840 void WarehouseViewer::attachObject(const std::string& name)
02841 {
02842   createAttachObjectDialog(name);
02843   int res = attach_object_dialog_->exec();
02844   if(res == QDialog::Accepted) {
02845     std::vector<std::string> touch_links;
02846     for(int i = 0; i < added_touch_links_->count(); i++) {
02847       touch_links.push_back(added_touch_links_->item(i)->text().toStdString());
02848       
02849     }
02850     attachCollisionObject(name, attach_link_box_->currentText().toStdString(), touch_links);
02851     changeToAttached(name);
02852   }
02853   delete attach_object_dialog_;
02854 }
02855 
02856 void WarehouseViewer::createAttachObjectDialog(const std::string& name)
02857 {
02858   if(current_planning_scene_name_ == "") {
02859     return;
02860   }
02861   
02862   arm_navigation_msgs::PlanningScene planning_scene = planning_scene_map_[current_planning_scene_name_].getPlanningScene();
02863 
02864   std::stringstream ss;
02865   ss << "Attach object " << name;
02866   
02867   attach_object_dialog_ = new QDialog(this);
02868   attach_object_dialog_->setWindowTitle(ss.str().c_str());
02869   
02870   QVBoxLayout* layout = new QVBoxLayout(attach_object_dialog_);
02871 
02872   QGroupBox* panel = new QGroupBox(attach_object_dialog_);
02873   panel->setTitle("Link for attaching");
02874 
02875   QVBoxLayout* panel_layout = new QVBoxLayout(panel);
02876   attach_link_box_= new QComboBox(attach_object_dialog_);
02877 
02878   std::vector<std::string> link_names;
02879   cm_->getKinematicModel()->getLinkModelNames(link_names);
02880   for(unsigned int i = 0; i < link_names.size(); i++) {
02881     attach_link_box_->addItem(link_names[i].c_str());
02882   }
02883   panel_layout->addWidget(attach_link_box_);
02884   panel->setLayout(panel_layout);
02885   layout->addWidget(panel);
02886 
02887   QGroupBox* grid_panel = new QGroupBox(attach_object_dialog_);
02888   QGridLayout* grid = new QGridLayout(grid_panel);
02889   
02890   QLabel* all_label = new QLabel(attach_object_dialog_);
02891   all_label->setText("Links and groups");
02892 
02893   QLabel* touch_links = new QLabel(attach_object_dialog_);
02894   touch_links->setText("Touch links");
02895 
02896   grid->addWidget(all_label, 0, 0);
02897   grid->addWidget(touch_links, 0, 2);
02898 
02899   QPushButton* add_button = new QPushButton(attach_object_dialog_);
02900   add_button->setText("Add ->");
02901   
02902   connect(add_button, SIGNAL(clicked()), this, SLOT(addTouchLinkClicked()));
02903 
02904   QPushButton* remove_button = new QPushButton(attach_object_dialog_);
02905   remove_button->setText("Remove");
02906 
02907   connect(remove_button, SIGNAL(clicked()), this, SLOT(removeTouchLinkClicked()));
02908 
02909   grid->addWidget(add_button, 1, 1, Qt::AlignTop);  
02910   grid->addWidget(remove_button, 1, 1, Qt::AlignBottom);
02911 
02912   possible_touch_links_ = new QListWidget(attach_object_dialog_);
02913   possible_touch_links_->setSelectionMode(QAbstractItemView::ExtendedSelection);
02914   for(unsigned int i = 0; i < link_names.size(); i++) {
02915     possible_touch_links_->addItem(link_names[i].c_str());
02916   }
02917   possible_touch_links_->addItem("------Groups---------");
02918   std::vector<std::string> group_names;
02919   cm_->getKinematicModel()->getModelGroupNames(group_names);
02920   for(unsigned int i = 0; i < group_names.size(); i++) {
02921     possible_touch_links_->addItem(group_names[i].c_str());
02922   }
02923   added_touch_links_ = new QListWidget(attach_object_dialog_);
02924   added_touch_links_->setSelectionMode(QAbstractItemView::ExtendedSelection);
02925 
02926   grid->addWidget(possible_touch_links_, 1, 0);
02927   grid->addWidget(added_touch_links_, 1, 2);
02928 
02929   layout->addWidget(grid_panel);
02930 
02931   QDialogButtonBox* qdb = new QDialogButtonBox();
02932   QPushButton* cancel_button = new QPushButton("Cancel");
02933   QPushButton* attach_button = new QPushButton("Attach");
02934   qdb->addButton(cancel_button, QDialogButtonBox::RejectRole);
02935   qdb->addButton(attach_button, QDialogButtonBox::AcceptRole);
02936   connect(qdb, SIGNAL(accepted()), attach_object_dialog_, SLOT(accept()));
02937   connect(qdb, SIGNAL(rejected()), attach_object_dialog_, SLOT(reject()));
02938 
02939   layout->addWidget(qdb, Qt::AlignRight);
02940 
02941   attach_object_dialog_->setLayout(layout);
02942 }
02943 
02944 void WarehouseViewer::addTouchLinkClicked() 
02945 {
02946   QList<QListWidgetItem *> l = possible_touch_links_->selectedItems();
02947 
02948   for(int i = 0; i < l.size(); i++) {
02949     bool found = false;
02950     for(int j = 0; j < added_touch_links_->count(); j++) {
02951       if(l[i]->text() == added_touch_links_->item(j)->text()) {
02952         found = true;
02953         break;
02954       }
02955     }
02956     if(!found && l[i]->text().toStdString() != std::string("------Groups---------")) {
02957       added_touch_links_->addItem(l[i]->text());
02958     }
02959   }
02960 }
02961 
02962 void WarehouseViewer::removeTouchLinkClicked()
02963 {
02964   qDeleteAll(added_touch_links_->selectedItems());
02965 }
02966 
02967 void WarehouseViewer::editRobotStatePressed()
02968 {
02969   createRobotStateEditor();
02970   edit_robot_state_dialog_->exec();
02971   delete edit_robot_state_dialog_;
02972 }
02973 
02974 void WarehouseViewer::createRobotStateEditor() {
02975 
02976   edit_robot_state_dialog_ = new QDialog(this);
02977   edit_robot_state_dialog_->setWindowTitle("Edit robot state");
02978   
02979   QVBoxLayout* layout = new QVBoxLayout(edit_robot_state_dialog_);
02980 
02981   QGroupBox* panel = new QGroupBox(edit_robot_state_dialog_);
02982   panel->setTitle("Joint state to edit");
02983 
02984   QVBoxLayout* panel_layout = new QVBoxLayout(panel);
02985   edit_joint_box_= new QComboBox(edit_robot_state_dialog_);
02986 
02987   const std::vector<planning_models::KinematicModel::JointModel*>& joint_models = cm_->getKinematicModel()->getJointModels();
02988   for(unsigned int i = 0; i < joint_models.size(); i++) {
02989     if(joint_models[i]->getComputatationOrderMapIndex().size() == 1) {
02990       edit_joint_box_->addItem(joint_models[i]->getName().c_str());
02991     }
02992   }
02993 
02994   QGridLayout* slider_layout = new QGridLayout(panel);
02995 
02996   QLabel* lower_label = new QLabel(edit_robot_state_dialog_);
02997   lower_label->setText("Lower bound");
02998 
02999   QLabel* upper_label = new QLabel(edit_robot_state_dialog_);
03000   upper_label->setText("Upper bound");
03001 
03002   QLabel* current_label = new QLabel(edit_robot_state_dialog_);
03003   current_label->setText("Current value");
03004 
03005   lower_bound_edit_window_ = new QLineEdit(edit_robot_state_dialog_);
03006   upper_bound_edit_window_ = new QLineEdit(edit_robot_state_dialog_);
03007   current_value_window_ = new QLineEdit(edit_robot_state_dialog_);
03008 
03009   lower_bound_edit_window_->setReadOnly(true);
03010   upper_bound_edit_window_->setReadOnly(true);
03011   lower_bound_edit_window_->setMinimumWidth(125);
03012   lower_bound_edit_window_->setMaximumWidth(125);
03013   upper_bound_edit_window_->setMinimumWidth(125);
03014   upper_bound_edit_window_->setMaximumWidth(125);
03015 
03016   joint_state_slider_ = new QSlider(Qt::Horizontal, edit_robot_state_dialog_);
03017   joint_state_slider_->setMinimum(0);
03018   joint_state_slider_->setMaximum(0);
03019 
03020   slider_layout->addWidget(lower_label, 0, 0);
03021   slider_layout->addWidget(current_label, 0, 1);
03022   slider_layout->addWidget(upper_label, 0, 2);
03023 
03024   slider_layout->addWidget(lower_bound_edit_window_, 1, 0);
03025   slider_layout->addWidget(joint_state_slider_, 1, 1);
03026   slider_layout->addWidget(upper_bound_edit_window_, 1, 2);
03027   
03028   slider_layout->addWidget(current_value_window_, 2, 1);
03029 
03030   slider_layout->setColumnStretch(1, 100);
03031 
03032   panel_layout->addWidget(edit_joint_box_);
03033   panel_layout->addLayout(slider_layout);
03034 
03035   panel->setLayout(panel_layout);
03036   layout->addWidget(panel);
03037 
03038   edit_robot_state_dialog_->setLayout(layout);
03039 
03040   //getting reasonable stuff in there
03041   editJointBoxChanged(edit_joint_box_->currentText());
03042 
03043   connect(edit_joint_box_, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(editJointBoxChanged(const QString&)));
03044   connect(joint_state_slider_, SIGNAL(valueChanged(int)), this, SLOT(jointStateSliderChanged(int)));
03045 }
03046 
03047 void WarehouseViewer::editJointBoxChanged(const QString& joint) 
03048 {
03049 
03050   std::map<std::string, double> current_joint_state_values;
03051   robot_state_->getKinematicStateValues(current_joint_state_values);
03052 
03053   double current_value = current_joint_state_values[joint.toStdString()];
03054 
03055   const planning_models::KinematicModel::JointModel* jm = cm_->getKinematicModel()->getJointModel(joint.toStdString());
03056 
03057   std::pair<double, double> bounds; 
03058   jm->getVariableBounds(jm->getName(), bounds);
03059 
03060   std::stringstream lb;
03061   lb << bounds.first;
03062   std::stringstream ub;
03063   ub << bounds.second;
03064 
03065   std::stringstream cur;
03066   cur << current_value;
03067 
03068   lower_bound_edit_window_->setText(lb.str().c_str());
03069   upper_bound_edit_window_->setText(ub.str().c_str());
03070   current_value_window_->setText(cur.str().c_str());
03071 
03072   joint_state_slider_->setMinimum(bounds.first*1000);
03073   joint_state_slider_->setMaximum(bounds.second*1000);
03074   joint_state_slider_->setValue(current_value*1000);
03075 }
03076 
03077 void WarehouseViewer::jointStateSliderChanged(int nv) 
03078 {
03079   std::map<std::string, double> current_joint_state_values;
03080   robot_state_->getKinematicStateValues(current_joint_state_values);
03081 
03082   current_joint_state_values[edit_joint_box_->currentText().toStdString()] = nv/1000.0;
03083 
03084   std::stringstream cur;
03085   cur << nv/1000.0;
03086   current_value_window_->setText(cur.str().c_str());
03087 
03088   robot_state_->setKinematicState(current_joint_state_values);
03089 
03090   planning_environment::convertKinematicStateToRobotState(*robot_state_,
03091                                                           ros::Time::now(),
03092                                                           cm_->getWorldFrameId(),
03093                                                           planning_scene_map_[current_planning_scene_name_].getPlanningScene().robot_state);
03094   sendPlanningScene(planning_scene_map_[current_planning_scene_name_]);
03095 }
03096  
03097 void WarehouseViewer::popupPlannerFailure(int value)
03098 {
03099   ArmNavigationErrorCodes errorCode;
03100   errorCode.val = value;
03101   std::string failure  = "Planning Failed: " + armNavigationErrorCodeToString(errorCode);
03102   QMessageBox msg(QMessageBox::Critical, "Planning Failed!", QString::fromStdString(failure));
03103   msg.addButton("Ok", QMessageBox::AcceptRole);
03104   msg.exec();
03105 }
03106 
03107 void WarehouseViewer::popupFilterFailure(int value)
03108 {
03109   ArmNavigationErrorCodes errorCode;
03110   errorCode.val = value;
03111   std::string failure  = "Filter Failed: " + armNavigationErrorCodeToString(errorCode);
03112   QMessageBox msg(QMessageBox::Critical, "Filter Failed!", QString::fromStdString(failure));
03113   msg.addButton("Ok", QMessageBox::AcceptRole);
03114   msg.exec();
03115 }
03116 
03117 void marker_function()
03118 {
03119   unsigned int counter = 0;
03120   while(ros::ok())
03121   {
03122     if(inited)
03123     {
03124       if(counter % CONTROL_SPEED == 0)
03125       {
03126         counter = 1;
03127         psv->sendMarkers();
03128       }
03129       else
03130       {
03131         counter++;
03132       }
03133 
03134       if(psv->quit_threads_)
03135       {
03136         break;
03137       }
03138     }
03139     usleep(5000);
03140   }
03141 
03142 }
03143 
03144 void spin_function()
03145 {
03146   ros::WallRate r(100.0);
03147   while(ros::ok() && !inited)
03148   {
03149     r.sleep();
03150     ros::spinOnce();
03151   }
03152   while(ros::ok() && !psv->quit_threads_)
03153   {
03154     r.sleep();
03155     ros::spinOnce();
03156   }
03157 }
03158 
03159 void quit(int sig)
03160 {
03161   if(psv != NULL)
03162   {
03163     delete psv;
03164   }
03165 }
03166 
03167 int main(int argc, char** argv)
03168 {
03169   ros::init(argc, argv, "planning_scene_warehouse_viewer", ros::init_options::NoSigintHandler);
03170 
03171   boost::thread spin_thread(boost::bind(&spin_function));
03172   boost::thread marker_thread(boost::bind(&marker_function));
03173 
03174   QApplication* app = new QApplication(argc, argv);
03175   planning_scene_utils::PlanningSceneParameters params;
03176   param<string>("set_planning_scene_diff_name", params.set_planning_scene_diff_name_, SET_PLANNING_SCENE_DIFF_NAME);
03177   param<string>("left_ik_name", params.left_ik_name_, LEFT_IK_NAME);
03178   param<string>("left_interpolate_service_name", params.left_interpolate_service_name_, LEFT_INTERPOLATE_SERVICE_NAME);
03179   param<string>("non_coll_left_ik_name", params.non_coll_left_ik_name_, NON_COLL_LEFT_IK_NAME);
03180   param<string>("non_coll_right_ik_name", params.non_coll_right_ik_name_, NON_COLL_RIGHT_IK_NAME);
03181   param<string>("planner_1_service_name", params.planner_1_service_name_, PLANNER_1_SERVICE_NAME);
03182   param<string>("planner_2_service_name", params.planner_2_service_name_, PLANNER_2_SERVICE_NAME);
03183   param<string>("proximity_space_planner_name", params.proximity_space_planner_name_, PROXIMITY_SPACE_PLANNER_NAME);
03184   param<string>("proximity_space_service_name",  params.proximity_space_service_name_, PROXIMITY_SPACE_SERVICE_NAME);
03185   param<string>("proximity_space_validity_name",  params.proximity_space_validity_name_,  PROXIMITY_SPACE_VALIDITY_NAME);
03186   param<string>("right_ik_name", params.right_ik_name_, RIGHT_IK_NAME);
03187   param<string>("right_interpolate_service_name", params.right_interpolate_service_name_, RIGHT_INTERPOLATE_SERVICE_NAME);
03188   param<string>("trajectory_filter_service_name", params.trajectory_filter_1_service_name_, TRAJECTORY_FILTER_1_SERVICE_NAME);
03189   param<string>("trajectory_filter_2_service_name", params.trajectory_filter_2_service_name_, TRAJECTORY_FILTER_2_SERVICE_NAME);
03190   param<string>("vis_topic_name", params.vis_topic_name_ , VIS_TOPIC_NAME);
03191   param<string>("right_ik_link", params.right_ik_link_ , RIGHT_IK_LINK);
03192   param<string>("left_ik_link", params.left_ik_link_ , LEFT_IK_LINK);
03193   param<string>("right_arm_group", params.right_arm_group_ , RIGHT_ARM_GROUP);
03194   param<string>("left_arm_group", params.left_arm_group_ , LEFT_ARM_GROUP);
03195   param<string>("right_redundancy", params.right_redundancy_ , RIGHT_ARM_REDUNDANCY);
03196   param<string>("left_redundancy", params.left_redundancy_ , LEFT_ARM_REDUNDANCY);
03197   param<string>("execute_left_trajectory", params.execute_left_trajectory_ , EXECUTE_LEFT_TRAJECTORY);
03198   param<string>("execute_right_trajectory", params.execute_right_trajectory_ , EXECUTE_RIGHT_TRAJECTORY);
03199   param<string>("list_controllers_service", params.list_controllers_service_, LIST_CONTROLLERS_SERVICE);
03200   param<string>("load_controllers_service", params.load_controllers_service_, LOAD_CONTROLLERS_SERVICE);
03201   param<string>("unload_controllers_service", params.unload_controllers_service_, UNLOAD_CONTROLLERS_SERVICE);
03202   param<string>("switch_controllers_service", params.switch_controllers_service_, SWITCH_CONTROLLERS_SERVICE);
03203   param<string>("gazebo_robot_model", params.gazebo_model_name_, GAZEBO_ROBOT_MODEL);
03204   param<string>("robot_description_param", params.robot_description_param_, ROBOT_DESCRIPTION_PARAM);
03205   param<bool>("use_robot_data", params.use_robot_data_, false);
03206   params.sync_robot_state_with_gazebo_ = false;
03207 
03208   ParameterDialog* dialog = new ParameterDialog(params);
03209   dialog->exec();
03210   dialog->updateParams();
03211 
03212   QImage image;
03213   if(chdir(ros::package::getPath("move_arm_warehouse").c_str()) != 0)
03214   {
03215     ROS_ERROR("FAILED TO CHANGE PACKAGE TO %s", ros::package::getPath("move_arm_warehouse").c_str());
03216   }
03217   if(!image.load("./res/splash.png"))
03218   {
03219     ROS_ERROR("FAILED TO LOAD ./res/splash.png");
03220   }
03221 
03222   QSplashScreen screen(QPixmap::fromImage(image));
03223   screen.show();
03224   app->processEvents();
03225   psv = new WarehouseViewer(NULL, dialog->params_);
03226   app->setActiveWindow(psv);
03227   psv->show();
03228   screen.close();
03229   inited = true;
03230 
03231   int ret = app->exec();
03232   psv->quit_threads_ = true;
03233   spin_thread.join();
03234   marker_thread.join();
03235   return ret;
03236 
03237 }


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Fri Dec 6 2013 21:12:34