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