planning_description_configuration_wizard.h
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


planning_environment
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:09:24