Go to the documentation of this file.00001 #ifndef PLANNING_DESCRIPTION_CONFIGURATION_thisH
00002 #define PLANNING_DESCRIPTION_CONFIGURATION_thisH
00003 #include <termios.h>
00004 #include <signal.h>
00005 #include <cstdio>
00006 #include <cstdlib>
00007 #include <unistd.h>
00008 #include <math.h>
00009
00010 #include <ros/ros.h>
00011 #include <ros/package.h>
00012
00013 #ifndef Q_MOC_RUN
00014 #include <planning_environment/models/collision_models.h>
00015 #include <planning_environment/models/model_utils.h>
00016 #include <tf/transform_broadcaster.h>
00017 #include <collision_space/environmentODE.h>
00018 #include <rosgraph_msgs/Clock.h>
00019 #include <planning_environment/util/collision_operations_generator.h>
00020 #endif
00021 #include <qt4/QtGui/qwidget.h>
00022 #include <qt4/QtGui/qwizard.h>
00023 #include <qt4/QtGui/qlabel.h>
00024 #include <qt4/QtGui/qcombobox.h>
00025 #include <qt4/QtGui/qtreewidget.h>
00026 #include <qt4/QtGui/qtablewidget.h>
00027 #include <qt4/QtGui/qlineedit.h>
00028 #include <qt4/QtGui/qgridlayout.h>
00029 #include <qt4/QtGui/qgroupbox.h>
00030 #include <qt4/QtGui/qformlayout.h>
00031 #include <qt4/QtGui/qpushbutton.h>
00032 #include <qt4/QtGui/qcheckbox.h>
00033 #include <qt4/QtGui/qdialog.h>
00034 #include <qt4/QtGui/qfiledialog.h>
00035 #include <tinyxml.h>
00036 #include <qt4/Qt/qthread.h>
00037 #include <qt4/QtGui/qprogressbar.h>
00038
00039 static const std::string VIS_TOPIC_NAME = "planning_description_configuration_wizard";
00040 static const unsigned int CONTROL_SPEED = 10;
00041 static const double DEFAULT_ACCELERATION = 1.0;
00042
00043 class OutputWizardPage;
00044 class KinematicChainWizardPage;
00045 class JointCollectionWizardPage;
00046 class SetupGroupsWizardPage;
00047 class CollisionsWizardPage;
00048
00049 inline std::vector<int> getSelectedRows(QTableWidget* table) {
00050 QList<QTableWidgetItem*> selected = table->selectedItems();
00051
00052 std::vector<int> rows;
00053 for(int i = 0; i < selected.size(); i++)
00054 {
00055 bool rowExists = false;
00056 int r = selected[i]->row();
00057 for(size_t j = 0; j < rows.size(); j++)
00058 {
00059 if((int)j == r)
00060 {
00061 rowExists = true;
00062 }
00063 }
00064
00065 if(!rowExists)
00066 {
00067 rows.push_back(r);
00068 }
00069 }
00070 return rows;
00071 }
00072
00073
00074 class PlanningDescriptionConfigurationWizard: public QWizard
00075 {
00076 Q_OBJECT
00077
00078 public:
00079 enum WizardMode
00080 {
00081 Easy, Advanced
00082 };
00083
00084 enum WizardPage
00085 {
00086 StartPage,
00087 SetupGroupsPage,
00088 KinematicChainsPage,
00089 JointCollectionsPage,
00090 SelectDOFPage,
00091 AdjacentLinkPage,
00092 AlwaysInCollisionPage,
00093 OftenInCollisionPage,
00094 OccasionallyInCollisionPage,
00095 OutputFilesPage,
00096 DefaultInCollisionPage
00097 };
00098
00099 enum GroupAddStatus {
00100 GroupAddSuccess,
00101 GroupAddCancel,
00102 GroupAddFailed
00103 };
00104
00105 PlanningDescriptionConfigurationWizard(const std::string& urdf_package, const std::string& urdf_path,
00106 QWidget* parent = NULL);
00107 ~PlanningDescriptionConfigurationWizard();
00108 void deleteKinematicStates();
00109 bool setupWithWorldFixedFrame(const std::string& world_fixed_frame, const std::string& joint_type);
00110 void emitWorldJointYAML();
00111 void outputConfigAndLaunchRviz();
00112 GroupAddStatus addGroup(const planning_models::KinematicModel::GroupConfig& group_config);
00113 void removeGroup(const std::string& name);
00114 inline void popupWaitWarning() { popupGenericWarning("Please wait..."); }
00115 void popupGenericWarning(const char* text);
00116 void popupFileFailure(const char* reason);
00117 void emitGroupYAML();
00118 void outputJointLimitsYAML();
00119 visualization_msgs::Marker
00120 transformEnvironmentModelContactInfoMarker(const collision_space::EnvironmentModel::Contact& c);
00121 void outputPlanningDescriptionYAML();
00122 void outputOMPLGroupYAML();
00123 void outputOMPLLaunchFile();
00124 void outputTrajectoryFilterLaunch();
00125 void outputPlanningEnvironmentLaunch();
00126 void outputKinematicsLaunchFiles();
00127 void outputMoveGroupLaunchFiles();
00128 void outputPlanningComponentVisualizerLaunchFile();
00129 void outputArmNavigationLaunchFile();
00130 void updateCollisionsInCurrentState();
00131 void sendMarkers();
00132 void sendTransforms();
00133 bool isInited() const;
00134 planning_environment::CollisionOperationsGenerator* getOperationsGenerator();
00135 std::string getRobotName();
00136 const planning_models::KinematicModel* getKinematicModel() {
00137 return kmodel_;
00138 }
00139
00140 void setCurrentShowGroup(const std::string& s) {
00141 lock_.lock();
00142 current_show_group_ = s;
00143 lock_.unlock();
00144 }
00145
00146 void setCurrentShowLink(const std::string& s) {
00147 lock_.lock();
00148 current_show_link_ = s;
00149 lock_.unlock();
00150 }
00151
00152 WizardMode getWizardMode() const {
00153 return wizard_mode_;
00154 }
00155
00156 void setDisableMap(const planning_environment::CollisionOperationsGenerator::DisableType disable_type,
00157 const std::vector<planning_environment::CollisionOperationsGenerator::StringPair>& pairs)
00158 {
00159 disable_map_[disable_type] = pairs;
00160 }
00161
00162 void
00163 visualizeCollision(std::vector<planning_environment::CollisionOperationsGenerator::CollidingJointValues>& jointValues,
00164 std::vector<planning_environment::CollisionOperationsGenerator::StringPair>& pairs,
00165 int& index, std_msgs::ColorRGBA& color);
00166
00167
00168 signals:
00169 void changeProgress(int progress);
00170 void changeLabel(const char* name);
00171
00172 public slots:
00173 void easyButtonToggled(bool checkState);
00174 void hardButtonToggled(bool checkState);
00175
00176 void labelChanged(const char* name);
00177
00178 void verySafeButtonToggled(bool checkState);
00179 void safeButtonToggled(bool checkState);
00180 void normalButtonToggled(bool checkState);
00181 void fastButtonToggled(bool checkState);
00182 void veryFastButtonToggled(bool checkState);
00183
00184 void dofSelectionTableChanged();
00185 void dofTogglePushed();
00186 void writeFiles();
00187 void autoConfigure();
00188 void update();
00189
00190 protected:
00191
00192 virtual int nextId() const;
00193 void setupQtPages();
00194 void initStartPage();
00195 void initSetupGroupsPage();
00196
00197 void initSelectDofPage();
00198 void createDofPageTable();
00199 void initOutputFilesPage();
00200
00201 bool inited_;
00202
00203 ros::NodeHandle nh_;
00204 boost::shared_ptr<urdf::Model> urdf_;
00205 planning_models::KinematicModel* kmodel_;
00206 planning_environment::CollisionModels* cm_;
00207 planning_environment::CollisionOperationsGenerator* ops_gen_;
00208 planning_models::KinematicState* robot_state_;
00209 collision_space::EnvironmentModel* ode_collision_model_;
00210 visualization_msgs::MarkerArray collision_markers_;
00211 planning_models::KinematicModel::MultiDofConfig world_joint_config_;
00212 std::map<planning_environment::CollisionOperationsGenerator::DisableType,
00213 std::vector<planning_environment::CollisionOperationsGenerator::StringPair> > disable_map_;
00214
00215 std::string current_show_group_;
00216 std::string current_show_link_;
00217
00218 tf::TransformBroadcaster transform_broadcaster_;
00219 ros::Publisher vis_marker_publisher_;
00220 ros::Publisher vis_marker_array_publisher_;
00221
00222 boost::recursive_mutex lock_;
00223
00224 YAML::Emitter* emitter_;
00225
00226 std::string package_directory_;
00227 std::string dir_name_;
00228 std::string yaml_outfile_name_, full_yaml_outfile_name_;
00229 std::string launch_outfile_name_, full_launch_outfile_name_;
00230
00231 std::string urdf_package_, urdf_path_;
00232
00233 QWizardPage* start_page_;
00234 SetupGroupsWizardPage* setup_groups_page_;
00235 KinematicChainWizardPage* kinematic_chain_page_;
00236 JointCollectionWizardPage* joint_collections_page_;
00237 QWizardPage* select_dof_page_;
00238 CollisionsWizardPage* adjacent_link_page_;
00239 CollisionsWizardPage* always_in_collision_page_;
00240 CollisionsWizardPage* default_in_collision_page_;
00241 CollisionsWizardPage* often_in_collision_page_;
00242 CollisionsWizardPage* occasionally_in_collision_page_;
00243
00244 OutputWizardPage* output_wizard_page_;
00245
00246 QTableWidget* dof_selection_table_;
00247 QCheckBox* group_selection_done_box_;
00248
00249 QDialog* file_failure_dialog_;
00250 QLabel* file_failure_reason_;
00251 QLineEdit* package_path_field_;
00252 QDialog* generic_dialog_;
00253 QLabel* generic_dialog_label_;
00254
00255 QDialog* confirm_group_replace_dialog_;
00256 QLabel* confirm_group_text_;
00257
00258 int progress_;
00259
00260 WizardMode wizard_mode_;
00261
00262 };
00263
00264 class AutoConfigureThread : public QThread
00265 {
00266 public:
00267 PlanningDescriptionConfigurationWizard* wizard_;
00268 AutoConfigureThread(PlanningDescriptionConfigurationWizard* wizard) : QThread(wizard), wizard_(wizard)
00269 {
00270 connect(wizard_, SIGNAL(changeProgress(int)), wizard_, SLOT(update()));
00271 connect(wizard_, SIGNAL(changeLabel(const char*)), wizard_, SLOT(labelChanged(const char*)));
00272 }
00273
00274 void run()
00275 {
00276 wizard_->autoConfigure();
00277 }
00278 };
00279
00280 class SetupGroupsWizardPage : public QWizardPage
00281 {
00282
00283 Q_OBJECT
00284
00285 public:
00286
00287 SetupGroupsWizardPage(PlanningDescriptionConfigurationWizard *parent);
00288
00289 void updateGroupTable();
00290
00291 virtual int nextId() const;
00292
00293 public slots:
00294
00295 void deleteGroupButtonClicked();
00296 void groupTableClicked();
00297 void addKinematicChainGroup();
00298 void addJointCollectionGroup();
00299
00300
00301 protected:
00302
00303 virtual bool validatePage() {
00304 parent_->setCurrentShowGroup("");
00305 return true;
00306 }
00307
00308 PlanningDescriptionConfigurationWizard *parent_;
00309
00310 int next_from_groups_id_;
00311
00312 QLineEdit* first_group_field_;
00313
00314 QTableWidget* current_group_table_;
00315
00316 };
00317
00318 class KinematicChainWizardPage : public QWizardPage
00319 {
00320 Q_OBJECT
00321
00322 public:
00323
00324 KinematicChainWizardPage(PlanningDescriptionConfigurationWizard *parent);
00325
00326 std::string getChainNameField() {
00327 return chain_name_field_->text().toStdString();
00328 }
00329
00330 std::string getBaseLinkField() {
00331 return base_link_field_->text().toStdString();
00332 }
00333
00334 std::string getTipLinkField() {
00335 return tip_link_field_->text().toStdString();
00336 }
00337
00338 bool getReturnToGroups() const {
00339 return return_to_groups_;
00340 }
00341
00342 public slots:
00343
00344 void baseLinkTreeClick();
00345 void tipLinkTreeClick();
00346 void showTreeLink();
00347
00348 void resetPage() {
00349 good_group_dialog_->done(0);
00350 }
00351
00352 protected:
00353
00354 virtual bool validatePage();
00355
00356 void createLinkTree(const planning_models::KinematicModel* kmodel);
00357 void addLinktoTreeRecursive(const planning_models::KinematicModel::LinkModel* link,
00358 const planning_models::KinematicModel::LinkModel* parent);
00359
00360 bool addLinkChildRecursive(QTreeWidgetItem* parent, const planning_models::KinematicModel::LinkModel* link,
00361 const std::string& parentName);
00362
00363 PlanningDescriptionConfigurationWizard *parent_;
00364
00365 bool return_to_groups_;
00366
00367 QDialog* good_group_dialog_;
00368 QDialog* not_ok_dialog_;
00369
00370 QTreeWidget* link_tree_;
00371
00372 QLineEdit* chain_name_field_;
00373 QLineEdit* base_link_field_;
00374 QLineEdit* tip_link_field_;
00375
00376 };
00377
00378 class JointCollectionWizardPage : public QWizardPage
00379 {
00380 Q_OBJECT
00381
00382 public:
00383
00384 JointCollectionWizardPage(PlanningDescriptionConfigurationWizard *parent);
00385
00386 bool getReturnToGroups() const {
00387 return return_to_groups_;
00388 }
00389
00390 public slots:
00391
00392 void resetPage() {
00393 good_group_dialog_->done(0);
00394 }
00395
00396 void selectJointButtonClicked();
00397 void deselectJointButtonClicked();
00398
00399 protected:
00400
00401 void createJointCollectionTables();
00402
00403 virtual bool validatePage();
00404
00405 PlanningDescriptionConfigurationWizard *parent_;
00406
00407 bool return_to_groups_;
00408
00409 QLineEdit* first_joint_field_;
00410
00411 QDialog* good_group_dialog_;
00412 QDialog* not_ok_dialog_;
00413
00414 QTableWidget* joint_table_;
00415 QTableWidget* selected_joint_table_;
00416 QLineEdit* joint_group_name_field_;
00417
00418 };
00419
00420 class CollisionsWizardPage : public QWizardPage
00421 {
00422 Q_OBJECT
00423
00424 public:
00425
00426 CollisionsWizardPage(PlanningDescriptionConfigurationWizard *parent,
00427 planning_environment::CollisionOperationsGenerator::DisableType);
00428
00429
00430 public slots:
00431
00432 void tableClicked();
00433 void tableChanged();
00434 void generateCollisionTable();
00435 void toggleTable();
00436
00437 protected:
00438
00439 virtual bool validatePage();
00440 PlanningDescriptionConfigurationWizard *parent_;
00441 QTableWidget* collision_table_;
00442 planning_environment::CollisionOperationsGenerator::DisableType disable_type_;
00443
00444 std::vector<planning_environment::CollisionOperationsGenerator::CollidingJointValues>
00445 in_collision_joint_values_;
00446 std::vector<planning_environment::CollisionOperationsGenerator::StringPair> collision_pairs_;
00447 std::vector<planning_environment::CollisionOperationsGenerator::StringPair> enable_pairs_;
00448 std::vector<planning_environment::CollisionOperationsGenerator::StringPair> extra_disable_pairs_;
00449 std::vector<planning_environment::CollisionOperationsGenerator::StringPair> disable_pairs_;
00450
00451 bool allow_enable_;
00452 bool show_percentages_;
00453 bool is_clickable_;
00454 bool coll_default_enabled_;
00455 };
00456
00457 class OutputWizardPage : public QWizardPage
00458 {
00459 Q_OBJECT
00460
00461 public:
00462 OutputWizardPage(PlanningDescriptionConfigurationWizard *parent = 0);
00463
00464 std::string getPackagePathField() const {
00465 return package_path_field_->text().toStdString();
00466 }
00467
00468 void setSuccessfulGeneration() {
00469 successful_generation_ = true;
00470 successful_creation_dialog_->show();
00471 }
00472
00473 void updateProgressBar(unsigned int progress) {
00474 progress_bar_->setValue(progress);
00475 }
00476
00477 void setProgressLabel(const char* label) {
00478 progress_label_->setText(label);
00479 }
00480
00481 public slots:
00482
00483 void fileSelected(const QString& file)
00484 {
00485 package_path_field_->setText(file);
00486 ROS_INFO_STREAM("Setting file to " << package_path_field_->text().toStdString());
00487 }
00488
00489 protected:
00490
00491 virtual bool validatePage() {
00492 if(!successful_generation_) {
00493 int val = really_exit_dialog_->exec();
00494 if(val == QDialog::Accepted) {
00495 return true;
00496 } else {
00497 return false;
00498 }
00499 }
00500 return true;
00501 }
00502
00503 private:
00504
00505 QProgressBar* progress_bar_;
00506 QLabel* progress_label_;
00507 bool successful_generation_;
00508
00509 QLineEdit* package_path_field_;
00510 QFileDialog* file_selector_;
00511
00512 QDialog* really_exit_dialog_;
00513 QDialog* successful_creation_dialog_;
00514 };
00515
00516 #endif