00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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: ");
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
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
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
00428
00429
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
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
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
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
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
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 }