planning_description_configuration_wizard.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the <ORGANIZATION> nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: E. Gil Jones
00031 
00032 #include <planning_environment/tools/planning_description_configuration_wizard.h>
00033 #include <qt4/QtGui/qapplication.h>
00034 #include <qt4/QtGui/qradiobutton.h>
00035 #include <qt4/QtGui/qdialogbuttonbox.h>
00036 #include <qt4/QtCore/qtextstream.h>
00037 #include <QHeaderView>
00038 #include <boost/thread.hpp>
00039 
00040 using namespace std;
00041 using namespace planning_environment;
00042 using namespace planning_models;
00043 using namespace visualization_msgs;
00044 using namespace collision_space;
00045 
00046 PlanningDescriptionConfigurationWizard::PlanningDescriptionConfigurationWizard(const string& urdf_package,
00047                                                                                const string& urdf_path, QWidget* parent) :
00048   QWizard(parent), inited_(false), world_joint_config_("world_joint"), urdf_package_(urdf_package),
00049   urdf_path_(urdf_path)
00050 {
00051   setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
00052   emitter_ = NULL;
00053   progress_ = 0;
00054   package_directory_ = "";
00055   string full_urdf_path = ros::package::getPath(urdf_package_)+"/"+urdf_path_;
00056 
00057   ROS_INFO_STREAM("full path name is " << full_urdf_path);
00058 
00059   urdf_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
00060   bool urdf_ok = urdf_->initFile(full_urdf_path);
00061 
00062   if(!urdf_ok)
00063   {
00064     ROS_WARN_STREAM("Urdf file " << full_urdf_path << " not ok");
00065     return;
00066   }
00067 
00068   //making directories
00069   dir_name_ = getRobotName() + "_arm_navigation";
00070 
00071   yaml_outfile_name_ = getRobotName() + "_planning_description.yaml";
00072   full_yaml_outfile_name_ = dir_name_ + "/config/" + yaml_outfile_name_;
00073   launch_outfile_name_ = getRobotName() + "_planning_environment.launch";
00074   full_launch_outfile_name_ = dir_name_ + "/launch/" + launch_outfile_name_;
00075 
00076   //pushing to the param server
00077   string com = "rosparam set robot_description -t " + full_urdf_path;
00078 
00079   int ok = system(com.c_str());
00080 
00081   if(ok != 0)
00082   {
00083     ROS_WARN_STREAM("Setting parameter system call not ok");
00084     return;
00085   }
00086 
00087   kmodel_ = NULL;
00088   cm_ = NULL;
00089   ops_gen_ = NULL;
00090   robot_state_ = NULL;
00091   ode_collision_model_ = NULL;
00092 
00093   if(!setupWithWorldFixedFrame("", "Floating"))
00094   {
00095     return;
00096   }
00097 
00098   outputConfigAndLaunchRviz();
00099 
00100   vis_marker_publisher_ = nh_.advertise<Marker> (VIS_TOPIC_NAME, 128);
00101   vis_marker_array_publisher_ = nh_.advertise<MarkerArray> (VIS_TOPIC_NAME + "_array", 128);
00102 
00103   wizard_mode_ = PlanningDescriptionConfigurationWizard::Easy;
00104   ode_collision_model_ = new EnvironmentModelODE();
00105 
00106   const vector<KinematicModel::LinkModel*>& coll_links = kmodel_->getLinkModelsWithCollisionGeometry();
00107 
00108   vector<string> coll_names;
00109   for(unsigned int i = 0; i < coll_links.size(); i++)
00110   {
00111     coll_names.push_back(coll_links[i]->getName());
00112   }
00113   EnvironmentModel::AllowedCollisionMatrix default_collision_matrix(coll_names, false);
00114   map<string, double> default_link_padding_map;
00115   ode_collision_model_->setRobotModel(kmodel_, default_collision_matrix, default_link_padding_map, 0.0, 1.0);
00116 
00117   cm_ = new CollisionModels(urdf_, kmodel_, ode_collision_model_);
00118   ops_gen_ = new CollisionOperationsGenerator(cm_);
00119 
00120 
00121   setupQtPages();
00122   inited_ = true;
00123 }
00124 
00125 PlanningDescriptionConfigurationWizard::~PlanningDescriptionConfigurationWizard()
00126 {
00127   if(cm_ != NULL)
00128   {
00129     delete cm_;
00130   }
00131   if(ops_gen_ != NULL)
00132   {
00133     delete ops_gen_;
00134   }
00135   if(emitter_ != NULL) {
00136     delete emitter_;
00137   }
00138 }
00139 
00140 void PlanningDescriptionConfigurationWizard::deleteKinematicStates()
00141 {
00142   if(robot_state_ != NULL)
00143   {
00144     delete robot_state_;
00145     robot_state_ = NULL;
00146   }
00147 }
00148 
00149 bool PlanningDescriptionConfigurationWizard::setupWithWorldFixedFrame(const string& world_fixed_frame,
00150                                                                       const string& joint_type)
00151 {
00152   lock_.lock();
00153 
00154   vector<KinematicModel::GroupConfig> gcs;
00155   vector<KinematicModel::MultiDofConfig> multi_dof_configs;
00156   const urdf::Link *root = urdf_->getRoot().get();
00157 
00158   string ff;
00159 
00160   if(world_fixed_frame.empty())
00161   {
00162     ff = root->name;
00163   }
00164   else
00165   {
00166     ff = world_fixed_frame;
00167   }
00168 
00169   ROS_INFO_STREAM("Setting world frame to " << ff);
00170 
00171   //now this should work with an n=on-identity transform
00172   world_joint_config_.type = joint_type;
00173   world_joint_config_.parent_frame_id = ff;
00174   world_joint_config_.child_frame_id = root->name;
00175   multi_dof_configs.push_back(world_joint_config_);
00176 
00177   deleteKinematicStates();
00178 
00179   if(cm_ != NULL)
00180   {
00181     delete cm_;
00182   }
00183   if(ops_gen_ != NULL)
00184   {
00185     delete ops_gen_;
00186   }
00187   kmodel_ = new KinematicModel(*urdf_, gcs, multi_dof_configs);
00188 
00189   if(kmodel_->getRoot() == NULL)
00190   {
00191     ROS_INFO_STREAM("Kinematic root is NULL");
00192     lock_.unlock();
00193     return false;
00194   }
00195 
00196   robot_state_ = new KinematicState(kmodel_);
00197   robot_state_->setKinematicStateToDefault();
00198 
00199   lock_.unlock();
00200 
00201   return true;
00202 }
00203 
00204 void PlanningDescriptionConfigurationWizard::emitWorldJointYAML()
00205 {
00206 
00207   (*emitter_) << YAML::Key << "multi_dof_joints";
00208   (*emitter_) << YAML::Value << YAML::BeginSeq;
00209   (*emitter_) << YAML::BeginMap;
00210   (*emitter_) << YAML::Key << "name" << YAML::Value << world_joint_config_.name;
00211   (*emitter_) << YAML::Key << "type" << YAML::Value << world_joint_config_.type;
00212   (*emitter_) << YAML::Key << "parent_frame_id" << YAML::Value << world_joint_config_.parent_frame_id;
00213   (*emitter_) << YAML::Key << "child_frame_id" << YAML::Value << world_joint_config_.child_frame_id;
00214   (*emitter_) << YAML::EndMap;
00215   (*emitter_) << YAML::EndSeq;
00216 }
00217 
00218 void PlanningDescriptionConfigurationWizard::outputConfigAndLaunchRviz()
00219 {
00220 
00221   string template_name = ros::package::getPath("planning_environment")
00222     + "/config/planning_description_configuration_wizard.vcg";
00223 
00224   ifstream template_file(template_name.c_str());
00225 
00226   ofstream ofile("planning_description_configuration_wizard.vcg");
00227 
00228   char ch;
00229   char buf[80];
00230   while(template_file && template_file.get(ch))
00231   {
00232     if(ch != '$')
00233     {
00234       ofile.put(ch);
00235     }
00236     else
00237     {
00238       template_file.getline(buf, 80, '$');
00239       if(template_file.eof() || template_file.bad())
00240       {
00241         ROS_ERROR_STREAM("Bad template file");
00242         break;
00243       }
00244       ofile << kmodel_->getRoot()->getParentFrameId();
00245     }
00246   }
00247 
00248   ofstream touch_file("vcg_ready");
00249 }
00250 
00251 PlanningDescriptionConfigurationWizard::GroupAddStatus 
00252 PlanningDescriptionConfigurationWizard::addGroup(const KinematicModel::GroupConfig& group_config)
00253 {
00254   lock_.lock();
00255   
00256   if(kmodel_->hasModelGroup(group_config.name_))
00257   {
00258     QString t;
00259     QTextStream(&t) << "There is already a planning group named " << group_config.name_.c_str() << ". Replace this group?";
00260     confirm_group_text_->setText(t);
00261     int val = confirm_group_replace_dialog_->exec();
00262     if(val == QDialog::Rejected) {
00263       lock_.unlock();
00264       return GroupAddCancel;
00265     } 
00266     kmodel_->removeModelGroup(group_config.name_);
00267   }
00268   deleteKinematicStates();
00269   bool group_ok = kmodel_->addModelGroup(group_config);
00270   robot_state_ = new KinematicState(kmodel_);
00271   robot_state_->setKinematicStateToDefault();
00272 
00273   if(group_ok)
00274   {
00275     setup_groups_page_->updateGroupTable();
00276     current_show_group_ = group_config.name_;
00277   }
00278   else
00279   {
00280     current_show_group_ = "";
00281   }
00282 
00283   lock_.unlock();
00284   if(group_ok) {
00285     return GroupAddSuccess;
00286   }
00287   return GroupAddFailed;
00288 }
00289 
00290 void PlanningDescriptionConfigurationWizard::removeGroup(const std::string& name) 
00291 {
00292   lock_.lock();
00293   if(!kmodel_->hasModelGroup(name)) {
00294     lock_.unlock();
00295     return;
00296   }
00297   deleteKinematicStates();
00298   kmodel_->removeModelGroup(name);
00299 
00300   robot_state_ = new KinematicState(kmodel_);
00301   robot_state_->setKinematicStateToDefault();
00302 
00303   if(current_show_group_ == name) {
00304     current_show_group_ = "";
00305   }
00306   
00307   lock_.unlock();
00308 }
00309 
00310 void PlanningDescriptionConfigurationWizard::popupFileFailure(const char* reason)
00311 {
00312   file_failure_reason_->setText(reason);
00313   file_failure_dialog_->exec();
00314 }
00315 
00316 void PlanningDescriptionConfigurationWizard::emitGroupYAML()
00317 {
00318   (*emitter_) << YAML::Key << "groups";
00319   (*emitter_) << YAML::Value << YAML::BeginSeq;
00320 
00321   const map<string, KinematicModel::GroupConfig>& group_config_map = kmodel_->getJointModelGroupConfigMap();
00322 
00323   for(map<string, KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin(); it
00324         != group_config_map.end(); it++)
00325   {
00326     (*emitter_) << YAML::BeginMap;
00327     (*emitter_) << YAML::Key << "name" << YAML::Value << it->first;
00328     if(!it->second.base_link_.empty())
00329     {
00330       (*emitter_) << YAML::Key << "base_link" << YAML::Value << it->second.base_link_;
00331       (*emitter_) << YAML::Key << "tip_link" << YAML::Value << it->second.tip_link_;
00332     }
00333     else
00334     {
00335       if(!it->second.subgroups_.empty())
00336       {
00337         (*emitter_) << YAML::Key << "subgroups";
00338         (*emitter_) << YAML::Value << YAML::BeginSeq;
00339         for(unsigned int i = 0; i < it->second.subgroups_.size(); i++)
00340         {
00341           (*emitter_) << it->second.subgroups_[i];
00342         }
00343         (*emitter_) << YAML::EndSeq;
00344       }
00345       if(!it->second.joints_.empty())
00346       {
00347         (*emitter_) << YAML::Key << "joints";
00348         (*emitter_) << YAML::Value << YAML::BeginSeq;
00349         for(unsigned int i = 0; i < it->second.joints_.size(); i++)
00350         {
00351           (*emitter_) << it->second.joints_[i];
00352         }
00353         (*emitter_) << YAML::EndSeq;
00354       }
00355     }
00356     (*emitter_) << YAML::EndMap;
00357   }
00358   (*emitter_) << YAML::EndSeq;
00359 }
00360 
00361 // void setupGroupSubgroupCollection(const string& new_group_name) {
00362 //   while(1) {
00363 //     clear();
00364 //     vector<string> group_names;
00365 //     kmodel_->getModelGroupNames(group_names);
00366 //     vector<bool> is_included(group_names.size(), false);
00367 //     for(unsigned int i = 0; i < group_names.size(); i++) {
00368 //       printw("%d) ", i);
00369 //       if(is_included[i]) {
00370 //         printw("(X)");
00371 //       } else {
00372 //         printw("( )");
00373 //       }
00374 //       printw(" %s ", group_names[i].c_str());
00375 //     }
00376 //     printw("Enter a subgroup number to toggle inclusion ");
00377 //     printw("Enter 'v' to visualize current subgroup ");
00378 //     printw("Enter 'x' to accept current subgroup ");
00379 //     refresh();
00380 //     char str[80];
00381 //     getstr(str);
00382 //     unsigned int entry;
00383 //     stringstream ss(str);
00384 //     ss >> entry;
00385 //     if(entry == 0) {
00386 
00387 //     }
00388 //   }
00389 // }
00390 
00391 void PlanningDescriptionConfigurationWizard::outputJointLimitsYAML()
00392 {
00393   map<string, bool> already;
00394   boost::shared_ptr<urdf::Model> robot_model = urdf_;
00395   YAML::Emitter emitter;
00396   emitter << YAML::BeginMap;
00397   emitter << YAML::Key << "joint_limits";
00398   emitter << YAML::Value << YAML::BeginMap;
00399   const map<string, KinematicModel::JointModelGroup*>& group_map = kmodel_->getJointModelGroupMap();
00400   for(map<string, KinematicModel::JointModelGroup*>::const_iterator it = group_map.begin(); it != group_map.end(); it++)
00401   {
00402     const vector<const KinematicModel::JointModel*>& jmv = it->second->getJointModels();
00403 
00404     for(unsigned int i = 0; i < jmv.size(); i++)
00405     {
00406       boost::shared_ptr<const urdf::Joint> urdf_joint = robot_model->getJoint(jmv[i]->getName());
00407       double max_vel = 0.0;
00408       if(urdf_joint && urdf_joint->limits)
00409       {
00410         max_vel = urdf_joint->limits->velocity;
00411       } 
00412       const map<string, pair<double, double> >& bounds_map = jmv[i]->getAllVariableBounds();
00413       for(map<string, pair<double, double> >::const_iterator it2 = bounds_map.begin(); it2 != bounds_map.end(); it2++)
00414       {
00415         if(already.find(it2->first) == already.end())
00416         {
00417           already[it2->first] = true;
00418           emitter << YAML::Key << it2->first;
00419           emitter << YAML::Value << YAML::BeginMap;
00420           emitter << YAML::Key << "has_position_limits";
00421           const KinematicModel::RevoluteJointModel* rev =
00422             dynamic_cast<const KinematicModel::RevoluteJointModel*> (jmv[i]);
00423           bool has_limits = (rev == NULL || !rev->continuous_);
00424           emitter << YAML::Value << has_limits;
00425           if(max_vel > 0.0) {
00426             emitter << YAML::Key << "has_velocity_limits" << YAML::Value << "true";
00427             emitter << YAML::Key << "max_velocity" << YAML::Value << max_vel;
00428             emitter << YAML::Key << "has_acceleration_limits" << YAML::Value << "true";
00429             emitter << YAML::Key << "max_acceleration" << YAML::Value << DEFAULT_ACCELERATION;
00430           } else {
00431             emitter << YAML::Key << "has_velocity_limits" << YAML::Value << "false";
00432             emitter << YAML::Key << "has_acceleration_limits" << YAML::Value << "false";
00433           }
00434           emitter << YAML::Key << "angle_wraparound" << YAML::Value << !has_limits;
00435           emitter << YAML::EndMap;
00436         }
00437       }
00438     }
00439   }
00440   emitter << YAML::EndMap;
00441   emitter << YAML::EndMap;
00442   ofstream outf((dir_name_ + "/config/joint_limits.yaml").c_str(), ios_base::trunc);
00443 
00444   outf << emitter.c_str();
00445 }
00446 
00447 Marker PlanningDescriptionConfigurationWizard::transformEnvironmentModelContactInfoMarker(const EnvironmentModel::Contact& c)
00448 {
00449   string ns_name;
00450   ns_name = c.body_name_1;
00451   ns_name += "+";
00452   ns_name += c.body_name_2;
00453   Marker mk;
00454   mk.header.stamp = ros::Time::now();
00455   mk.header.frame_id = cm_->getWorldFrameId();
00456   mk.ns = ns_name;
00457   mk.id = 0;
00458   mk.type = Marker::SPHERE;
00459   mk.action = Marker::ADD;
00460   mk.pose.position.x = c.pos.x();
00461   mk.pose.position.y = c.pos.y();
00462   mk.pose.position.z = c.pos.z();
00463   mk.pose.orientation.w = 1.0;
00464 
00465   mk.scale.x = mk.scale.y = mk.scale.z = 0.05;
00466   return mk;
00467 }
00468 
00469 
00470 void PlanningDescriptionConfigurationWizard::outputPlanningDescriptionYAML()
00471 {
00472   //initial map
00473   if(emitter_ != NULL) {
00474     delete emitter_;
00475   }
00476   emitter_ = new YAML::Emitter;
00477   (*emitter_) << YAML::BeginMap;
00478   emitWorldJointYAML();
00479   emitGroupYAML();
00480   //ops_gen_->performanceTestSavedResults(disable_map_);
00481   ops_gen_->outputYamlStringOfSavedResults((*emitter_), disable_map_);
00482   //end map
00483   (*emitter_) << YAML::EndMap;
00484   ofstream outf(full_yaml_outfile_name_.c_str(), ios_base::trunc);
00485 
00486   outf << emitter_->c_str();
00487 }
00488 
00489 void PlanningDescriptionConfigurationWizard::outputOMPLGroupYAML()
00490 {
00491   YAML::Emitter emitter;
00492   emitter << YAML::BeginMap;
00493   emitter << YAML::Key << "planner_configs";
00494   
00495   emitter << YAML::Value << YAML::BeginMap;
00496   
00497   emitter << YAML::Key << "SBLkConfig1";
00498   emitter << YAML::Value << YAML::BeginMap;
00499   emitter << YAML::Key << "type" << YAML::Value << "kinematic::SBL";
00500   emitter << YAML::EndMap;
00501   
00502   emitter << YAML::Key << "LBKPIECEkConfig1";
00503   emitter << YAML::Value << YAML::BeginMap;
00504   emitter << YAML::Key << "type" << YAML::Value << "kinematic::LBKPIECE";
00505   emitter << YAML::EndMap;
00506   
00507   emitter << YAML::EndMap;
00508   
00509   emitter << YAML::Key << "groups";
00510   const std::map<std::string, planning_models::KinematicModel::GroupConfig>& group_config_map = kmodel_->getJointModelGroupConfigMap();
00511   
00512   emitter << YAML::Value << YAML::BeginSeq;
00513   for(std::map<std::string, planning_models::KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin();
00514       it != group_config_map.end();
00515       it++) {
00516     emitter << it->first;
00517     if(!it->second.base_link_.empty()) {
00518       emitter << it->first+"_cartesian";
00519     }
00520   }
00521   emitter << YAML::EndSeq;
00522   
00523   for(std::map<std::string, planning_models::KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin();
00524       it != group_config_map.end();
00525       it++) {
00526     emitter << YAML::Key << it->first;
00527     emitter << YAML::Value << YAML::BeginMap;
00528     emitter << YAML::Key << "planner_type" << YAML::Value << "JointPlanner";
00529     emitter << YAML::Key << "planner_configs" << YAML::Value << YAML::BeginSeq;
00530     emitter << "SBLkConfig1" << "LBKPIECEkConfig1" << YAML::EndSeq;
00531     emitter << YAML::Key << "projection_evaluator" << YAML::Value << "joint_state";
00532     emitter << YAML::EndMap;
00533   }
00534   
00535   //now doing cartesian for any groups that are kinematic chains
00536   for(std::map<std::string, planning_models::KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin();
00537       it != group_config_map.end();
00538       it++) {
00539     if(!it->second.base_link_.empty()) {
00540       emitter << YAML::Key << it->first+"_cartesian";
00541       emitter << YAML::Value << YAML::BeginMap;
00542       emitter << YAML::Key << "parent_frame" << YAML::Value << it->second.base_link_;
00543       emitter << YAML::Key << "physical_group" << YAML::Value << it->first;
00544       emitter << YAML::Key << "planner_type" << YAML::Value << "RPYIKTaskSpacePlanner";
00545       
00546       emitter << YAML::Key << "state_spaces" << YAML::Value << YAML::BeginSeq;
00547       emitter << "x" << "y" << "z" << "roll" << "pitch" << "yaw" << YAML::EndSeq;
00548       
00549       emitter << YAML::Key << "x" << YAML::Value << YAML::BeginMap;
00550       emitter << YAML::Key << "type" << YAML::Value << "Linear";
00551       emitter << YAML::Key << "min" << YAML::Value << "-2.0";
00552       emitter << YAML::Key << "max" << YAML::Value << "2.0";
00553       emitter << YAML::EndMap;
00554       
00555       emitter << YAML::Key << "y" << YAML::Value << YAML::BeginMap;
00556       emitter << YAML::Key << "type" << YAML::Value << "Linear";
00557       emitter << YAML::Key << "min" << YAML::Value << "-2.0";
00558       emitter << YAML::Key << "max" << YAML::Value << "2.0";
00559       emitter << YAML::EndMap;
00560       
00561       emitter << YAML::Key << "z" << YAML::Value << YAML::BeginMap;
00562       emitter << YAML::Key << "type" << YAML::Value << "Linear";
00563       emitter << YAML::Key << "min" << YAML::Value << "-2.0";
00564       emitter << YAML::Key << "max" << YAML::Value << "2.0";
00565       emitter << YAML::EndMap;
00566       
00567       emitter << YAML::Key << "roll" << YAML::Value << YAML::BeginMap;
00568       emitter << YAML::Key << "type" << YAML::Value << "Revolute";
00569       emitter << YAML::EndMap;
00570       
00571       emitter << YAML::Key << "pitch" << YAML::Value << YAML::BeginMap;
00572       emitter << YAML::Key << "type" << YAML::Value << "Revolute";
00573       emitter << YAML::EndMap;
00574       
00575       emitter << YAML::Key << "yaw" << YAML::Value << YAML::BeginMap;
00576       emitter << YAML::Key << "type" << YAML::Value << "Revolute";
00577       emitter << YAML::EndMap;
00578       
00579       emitter << YAML::Key << "planner_configs" << YAML::Value << YAML::BeginSeq;
00580       emitter << "SBLkConfig1" << "LBKPIECEkConfig1" << YAML::EndSeq;
00581       
00582       emitter << YAML::Key << "kinematics_solver" << YAML::Value 
00583               << "arm_kinematics_constraint_aware/KDLArmKinematicsPlugin";
00584       emitter << YAML::Key << "tip_name" << YAML::Value << it->second.tip_link_;
00585       emitter << YAML::Key << "root_name" << YAML::Value << it->second.base_link_;
00586       emitter << YAML::Key << "projection_evaluator" << YAML::Value << "joint_state";
00587       emitter << YAML::Key << "longest_valid_segment_fraction" << YAML::Value << "0.001"; 
00588       emitter << YAML::EndMap;
00589     }
00590   }
00591   emitter << YAML::EndMap;
00592   std::ofstream outf((dir_name_+"/config/ompl_planning.yaml").c_str(), std::ios_base::trunc);
00593   
00594   outf << emitter.c_str();
00595 }
00596 
00597 void PlanningDescriptionConfigurationWizard::outputOMPLLaunchFile()
00598 {
00599   TiXmlDocument doc;
00600   TiXmlElement* launch_root = new TiXmlElement("launch");
00601   doc.LinkEndChild(launch_root);
00602 
00603   TiXmlElement *inc = new TiXmlElement("include");
00604   launch_root->LinkEndChild(inc);
00605   inc->SetAttribute("file", "$(find " + dir_name_ + ")/launch/" + launch_outfile_name_);
00606 
00607   TiXmlElement *node = new TiXmlElement("node");
00608   launch_root->LinkEndChild(node);
00609   node->SetAttribute("pkg", "ompl_ros_interface");
00610   node->SetAttribute("type", "ompl_ros");
00611   node->SetAttribute("name", "ompl_planning");
00612 
00613   TiXmlElement *p = new TiXmlElement("param");
00614   node->LinkEndChild(p);
00615   p->SetAttribute("name", "default_planner_config");
00616   p->SetAttribute("type", "string");
00617   p->SetAttribute("value", "SBLkConfig1");
00618 
00619   TiXmlElement *rp = new TiXmlElement("rosparam");
00620   node->LinkEndChild(rp);
00621   rp->SetAttribute("command", "load");
00622   rp->SetAttribute("file", "$(find " + dir_name_ + ")/config/ompl_planning.yaml");
00623   doc.SaveFile(dir_name_ + "/launch/ompl_planning.launch");
00624 }
00625 
00626 void PlanningDescriptionConfigurationWizard::outputTrajectoryFilterLaunch()
00627 {
00628   TiXmlDocument doc;
00629   TiXmlElement* launch_root = new TiXmlElement("launch");
00630   doc.LinkEndChild(launch_root);
00631 
00632   TiXmlElement *inc = new TiXmlElement("include");
00633   launch_root->LinkEndChild(inc);
00634   inc->SetAttribute("file", "$(find " + dir_name_ + ")/launch/" + launch_outfile_name_);
00635 
00636   TiXmlElement *node = new TiXmlElement("node");
00637   launch_root->LinkEndChild(node);
00638   node->SetAttribute("pkg", "trajectory_filter_server");
00639   node->SetAttribute("type", "trajectory_filter_server");
00640   node->SetAttribute("name", "trajectory_filter_server");
00641 
00642   TiXmlElement *rp = new TiXmlElement("rosparam");
00643   node->LinkEndChild(rp);
00644   rp->SetAttribute("command", "load");
00645   rp->SetAttribute("file", "$(find trajectory_filter_server)/config/filters.yaml");
00646 
00647   TiXmlElement *rp2 = new TiXmlElement("rosparam");
00648   node->LinkEndChild(rp2);
00649   rp2->SetAttribute("command", "load");
00650   rp2->SetAttribute("file", "$(find " + dir_name_ + ")/config/joint_limits.yaml");
00651 
00652   doc.SaveFile(dir_name_ + "/launch/trajectory_filter_server.launch");
00653 }
00654 
00655 void PlanningDescriptionConfigurationWizard::outputPlanningEnvironmentLaunch()
00656 {
00657   TiXmlDocument doc;
00658   TiXmlElement* launch_root = new TiXmlElement("launch");
00659   doc.LinkEndChild(launch_root);
00660 
00661   TiXmlElement *rd = new TiXmlElement("param");
00662   launch_root->LinkEndChild(rd);
00663   rd->SetAttribute("name", "robot_description");
00664   rd->SetAttribute("textfile", "$(find " + urdf_package_ + ")/" + urdf_path_);
00665 
00666   TiXmlElement *rp = new TiXmlElement("rosparam");
00667   launch_root->LinkEndChild(rp);
00668   rp->SetAttribute("command", "load");
00669   rp->SetAttribute("ns", "robot_description_planning");
00670   rp->SetAttribute("file", "$(find " + dir_name_ + ")/config/" + yaml_outfile_name_);
00671   doc.SaveFile(full_launch_outfile_name_);
00672 
00673 }
00674 
00675 void PlanningDescriptionConfigurationWizard::outputKinematicsLaunchFiles()
00676 {
00677   TiXmlDocument doc;
00678   TiXmlElement* launch_root = new TiXmlElement("launch");
00679   doc.LinkEndChild(launch_root);
00680 
00681   TiXmlElement *inc = new TiXmlElement("include");
00682   launch_root->LinkEndChild(inc);
00683   inc->SetAttribute("file", "$(find " + dir_name_ + ")/launch/" + launch_outfile_name_);
00684 
00685   const map<string, KinematicModel::GroupConfig>& group_config_map = kmodel_->getJointModelGroupConfigMap();
00686 
00687   for(map<string, KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin(); it
00688         != group_config_map.end(); it++)
00689   {
00690     if(!it->second.base_link_.empty())
00691     {
00692       TiXmlElement *node = new TiXmlElement("node");
00693       launch_root->LinkEndChild(node);
00694       node->SetAttribute("pkg", "arm_kinematics_constraint_aware");
00695       node->SetAttribute("type", "arm_kinematics_constraint_aware");
00696       node->SetAttribute("name", getRobotName() + "_" + it->first + "_kinematics");
00697 
00698       TiXmlElement *group_param = new TiXmlElement("param");
00699       node->LinkEndChild(group_param);
00700       group_param->SetAttribute("name", "group");
00701       group_param->SetAttribute("type", "string");
00702       group_param->SetAttribute("value", it->first);
00703 
00704       TiXmlElement *base_param = new TiXmlElement("param");
00705       node->LinkEndChild(base_param);
00706       base_param->SetAttribute("name", it->first + "/root_name");
00707       base_param->SetAttribute("type", "string");
00708       base_param->SetAttribute("value", it->second.base_link_);
00709 
00710       TiXmlElement *tip_param = new TiXmlElement("param");
00711       node->LinkEndChild(tip_param);
00712       tip_param->SetAttribute("name", it->first + "/tip_name");
00713       tip_param->SetAttribute("type", "string");
00714       tip_param->SetAttribute("value", it->second.tip_link_);
00715 
00716       TiXmlElement *solver_param = new TiXmlElement("param");
00717       node->LinkEndChild(solver_param);
00718       solver_param->SetAttribute("name", "kinematics_solver");
00719       solver_param->SetAttribute("type", "string");
00720       solver_param->SetAttribute("value", "arm_kinematics_constraint_aware/KDLArmKinematicsPlugin");
00721     }
00722   }
00723 
00724   doc.SaveFile(dir_name_ + "/launch/constraint_aware_kinematics.launch");
00725 }
00726 
00727 void PlanningDescriptionConfigurationWizard::outputMoveGroupLaunchFiles()
00728 {
00729   TiXmlDocument doc;
00730   TiXmlElement* launch_root = new TiXmlElement("launch");
00731   doc.LinkEndChild(launch_root);
00732 
00733   const map<string, KinematicModel::GroupConfig>& group_config_map = kmodel_->getJointModelGroupConfigMap();
00734 
00735   for(map<string, KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin(); it
00736         != group_config_map.end(); it++)
00737   {
00738     TiXmlElement *inc = new TiXmlElement("include");
00739     launch_root->LinkEndChild(inc);
00740     inc->SetAttribute("file", "$(find " + dir_name_ + ")/launch/move_"+it->first+".launch");
00741 
00742     TiXmlDocument group_doc;
00743     TiXmlElement* group_launch_root = new TiXmlElement("launch");
00744     group_doc.LinkEndChild(group_launch_root);
00745     
00746     TiXmlElement *node = new TiXmlElement("node");
00747     group_launch_root->LinkEndChild(node);
00748     node->SetAttribute("pkg", "move_arm");
00749     node->SetAttribute("type", "move_arm_simple_action");
00750     node->SetAttribute("name", getRobotName() + "_move_" + it->first);
00751     
00752     TiXmlElement *group_param = new TiXmlElement("param");
00753     node->LinkEndChild(group_param);
00754     group_param->SetAttribute("name", "group");
00755     group_param->SetAttribute("type", "string");
00756     group_param->SetAttribute("value", it->first);
00757 
00758     TiXmlElement *remap1 = new TiXmlElement("remap");
00759     node->LinkEndChild(remap1);
00760     remap1->SetAttribute("from", "arm_ik");
00761     remap1->SetAttribute("to", getRobotName()+"_"+it->first+"_kinematics/get_constraint_aware_ik");
00762 
00763     TiXmlElement *base_param = new TiXmlElement("param");
00764     node->LinkEndChild(base_param);
00765     base_param->SetAttribute("name", "controller_action_name");
00766     base_param->SetAttribute("type", "string");
00767     base_param->SetAttribute("value", it->first+"_controller/follow_joint_trajectory");   
00768     group_doc.SaveFile(dir_name_ + "/launch/move_"+it->first+".launch");
00769   }
00770   doc.SaveFile(dir_name_ + "/launch/move_groups.launch");
00771 }
00772 
00773 void PlanningDescriptionConfigurationWizard::outputPlanningComponentVisualizerLaunchFile()
00774 {
00775   //now doing planning components .vcg file
00776   string template_name_2 = ros::package::getPath("planning_environment") + "/config/planning_components_visualizer.vcg";
00777 
00778   ifstream template_file_2(template_name_2.c_str());
00779 
00780   ofstream ofile_2((dir_name_ + "/config/planning_components_visualizer.vcg").c_str());
00781 
00782   char ch;
00783   char buf[80];
00784   while(template_file_2 && template_file_2.get(ch))
00785   {
00786     if(ch != '$')
00787     {
00788       ofile_2.put(ch);
00789     }
00790     else
00791     {
00792       template_file_2.getline(buf, 80, '$');
00793       if(template_file_2.eof() || template_file_2.bad())
00794       {
00795         ROS_ERROR_STREAM("Bad template file");
00796         break;
00797       }
00798       ofile_2 << kmodel_->getRoot()->getParentFrameId();
00799     }
00800   }
00801 
00802   TiXmlDocument doc;
00803   TiXmlElement* launch_root = new TiXmlElement("launch");
00804   doc.LinkEndChild(launch_root);
00805 
00806   TiXmlElement *inc = new TiXmlElement("include");
00807   launch_root->LinkEndChild(inc);
00808   inc->SetAttribute("file", "$(find " + dir_name_ + ")/launch/" + launch_outfile_name_);
00809 
00810   TiXmlElement *pre = new TiXmlElement("include");
00811   launch_root->LinkEndChild(pre);
00812   pre->SetAttribute("file",
00813                     "$(find planning_environment)/launch/planning_environment_visualization_prerequisites.launch");
00814 
00815   TiXmlElement *kin = new TiXmlElement("include");
00816   launch_root->LinkEndChild(kin);
00817   kin->SetAttribute("file", "$(find " + dir_name_ + ")/launch/constraint_aware_kinematics.launch");
00818 
00819   TiXmlElement *ompl = new TiXmlElement("include");
00820   launch_root->LinkEndChild(ompl);
00821   ompl->SetAttribute("file", "$(find " + dir_name_ + ")/launch/ompl_planning.launch");
00822 
00823   TiXmlElement *fil = new TiXmlElement("include");
00824   launch_root->LinkEndChild(fil);
00825   fil->SetAttribute("file", "$(find " + dir_name_ + ")/launch/trajectory_filter_server.launch");
00826 
00827   TiXmlElement *rviz = new TiXmlElement("node");
00828   launch_root->LinkEndChild(rviz);
00829   rviz->SetAttribute("pkg", "rviz");
00830   rviz->SetAttribute("type", "rviz");
00831   rviz->SetAttribute("name", "rviz_planning_components");
00832   rviz->SetAttribute("args", "-d $(find " + dir_name_ + ")/config/planning_components_visualizer.vcg");
00833 
00834   TiXmlElement *vis = new TiXmlElement("node");
00835   launch_root->LinkEndChild(vis);
00836   vis->SetAttribute("pkg", "move_arm");
00837   vis->SetAttribute("type", "planning_components_visualizer");
00838   vis->SetAttribute("name", "planning_components_visualizer");
00839   vis->SetAttribute("output", "screen");
00840 
00841   TiXmlElement *state_publisher = new TiXmlElement("node");
00842   launch_root->LinkEndChild(state_publisher);
00843   state_publisher->SetAttribute("pkg", "robot_state_publisher");
00844   state_publisher->SetAttribute("type", "state_publisher");
00845   state_publisher->SetAttribute("name", "rob_st_pub");
00846 
00847   doc.SaveFile(dir_name_ + "/launch/planning_components_visualizer.launch");
00848 }
00849 
00850 void PlanningDescriptionConfigurationWizard::outputArmNavigationLaunchFile()
00851 {
00852   TiXmlDocument doc;
00853   TiXmlElement* launch_root = new TiXmlElement("launch");
00854   doc.LinkEndChild(launch_root);
00855 
00856   TiXmlElement *inc = new TiXmlElement("include");
00857   launch_root->LinkEndChild(inc);
00858   inc->SetAttribute("file", "$(find " + dir_name_ + ")/launch/" + launch_outfile_name_);
00859 
00860   TiXmlElement *inc_env = new TiXmlElement("include");
00861   launch_root->LinkEndChild(inc_env);
00862   inc_env->SetAttribute("file", "$(find planning_environment)/launch/environment_server.launch");
00863 
00864   TiXmlElement *arg1 = new TiXmlElement("arg");
00865   inc_env->LinkEndChild(arg1);
00866   arg1->SetAttribute("name", "use_monitor");
00867   arg1->SetAttribute("value", "true");
00868 
00869   TiXmlElement *arg2 = new TiXmlElement("arg");
00870   inc_env->LinkEndChild(arg2);
00871   arg2->SetAttribute("name", "use_collision_map");
00872   arg2->SetAttribute("value", "false");
00873 
00874   TiXmlElement *kin = new TiXmlElement("include");
00875   launch_root->LinkEndChild(kin);
00876   kin->SetAttribute("file", "$(find " + dir_name_ + ")/launch/constraint_aware_kinematics.launch");
00877 
00878   TiXmlElement *ompl = new TiXmlElement("include");
00879   launch_root->LinkEndChild(ompl);
00880   ompl->SetAttribute("file", "$(find " + dir_name_ + ")/launch/ompl_planning.launch");
00881 
00882   TiXmlElement *fil = new TiXmlElement("include");
00883   launch_root->LinkEndChild(fil);
00884   fil->SetAttribute("file", "$(find " + dir_name_ + ")/launch/trajectory_filter_server.launch");
00885 
00886   TiXmlElement *incgr = new TiXmlElement("include");
00887   launch_root->LinkEndChild(incgr);
00888   incgr->SetAttribute("file", "$(find " + dir_name_ + ")/launch/move_groups.launch");
00889 
00890   doc.SaveFile(dir_name_ + "/launch/"+getRobotName()+"_arm_navigation.launch");
00891 }
00892 
00893 void PlanningDescriptionConfigurationWizard::updateCollisionsInCurrentState()
00894 {
00895   lock_.lock();
00896   std_msgs::ColorRGBA default_color;
00897   default_color.a = 1.0;
00898   default_color.r = 0.0;
00899   default_color.g = .8;
00900   default_color.b = 0.04;
00901 
00902   collision_markers_.markers.clear();
00903 
00904   cm_->getAllCollisionPointMarkers(*robot_state_, collision_markers_, default_color, ros::Duration(.2));
00905   lock_.unlock();
00906 }
00907 
00908 void PlanningDescriptionConfigurationWizard::sendMarkers()
00909 {
00910   lock_.lock();
00911   vis_marker_array_publisher_.publish(collision_markers_);
00912   if(!current_show_group_.empty())
00913   {
00914     MarkerArray arr;
00915     std_msgs::ColorRGBA default_color;
00916     default_color.a = 1.0;
00917     default_color.r = 0.0;
00918     default_color.g = .8;
00919     default_color.b = 0.04;
00920 
00921     std_msgs::ColorRGBA color;
00922     color.a = 1.0;
00923     color.r = 1.0;
00924     color.g = 0.0;
00925     color.b = 1.0;
00926 
00927     const KinematicModel::JointModelGroup* jmg = kmodel_->getModelGroup(current_show_group_);
00928 
00929     if(jmg != NULL)
00930     {
00931       vector<string> group_link_names = jmg->getGroupLinkNames();
00932       cm_->getRobotMarkersGivenState(*robot_state_, arr, default_color, current_show_group_, ros::Duration(.2),
00933                                      &group_link_names, 1.02);
00934 
00935       vector<string> updated_link_model_names = jmg->getUpdatedLinkModelNames();
00936       map<string, bool> dont_include;
00937       for(unsigned int i = 0; i < group_link_names.size(); i++)
00938       {
00939         dont_include[group_link_names[i]] = true;
00940       }
00941 
00942       vector<string> ex_list;
00943       for(unsigned int i = 0; i < updated_link_model_names.size(); i++)
00944       {
00945         if(dont_include.find(updated_link_model_names[i]) == dont_include.end())
00946         {
00947           ex_list.push_back(updated_link_model_names[i]);
00948         }
00949       }
00950       cm_->getRobotMarkersGivenState(*robot_state_, arr, color, current_show_group_ + "_updated_links",
00951                                      ros::Duration(.2), &ex_list);
00952       vis_marker_array_publisher_.publish(arr);
00953     }
00954     else
00955     {
00956       ROS_ERROR("The joint model group %s did not exist!", current_show_group_.c_str());
00957     }
00958   } else if(!current_show_link_.empty()) {
00959     MarkerArray arr;
00960     std_msgs::ColorRGBA default_color;
00961     default_color.a = 1.0;
00962     default_color.r = 0.0;
00963     default_color.g = 0.0;
00964     default_color.b = 1.0;
00965     
00966     vector<string> single_link_name(1, current_show_link_);
00967     cm_->getRobotPaddedMarkersGivenState(*robot_state_, arr, default_color, current_show_link_, ros::Duration(.2),
00968                                          &single_link_name);
00969     vis_marker_array_publisher_.publish(arr);
00970   }
00971   lock_.unlock();
00972 }
00973 
00974 void PlanningDescriptionConfigurationWizard::sendTransforms()
00975 {
00976   lock_.lock();
00977   ros::WallTime cur_time = ros::WallTime::now();
00978   rosgraph_msgs::Clock c;
00979   c.clock.nsec = cur_time.nsec;
00980   c.clock.sec = cur_time.sec;
00981   vector<geometry_msgs::TransformStamped> trans_vector;
00982   getAllKinematicStateStampedTransforms(*robot_state_, trans_vector, c.clock);
00983   transform_broadcaster_.sendTransform(trans_vector);
00984   lock_.unlock();
00985 }
00986 ;
00987 
00988 bool PlanningDescriptionConfigurationWizard::isInited() const
00989 {
00990   return inited_;
00991 }
00992 
00993 CollisionOperationsGenerator* PlanningDescriptionConfigurationWizard::getOperationsGenerator()
00994 {
00995   return ops_gen_;
00996 }
00997 
00998 string PlanningDescriptionConfigurationWizard::getRobotName()
00999 {
01000   return urdf_->getName();
01001 }
01002 
01003 void PlanningDescriptionConfigurationWizard::dofTogglePushed()
01004 {
01005   ROS_INFO_STREAM("Pushed");
01006   unsigned int column = 3;
01007   vector<int> rows = getSelectedRows(dof_selection_table_);
01008   for(size_t i = 0; i < rows.size(); i++)
01009   {
01010     QCheckBox* box = dynamic_cast<QCheckBox*> (dof_selection_table_->cellWidget(rows[i], column));
01011 
01012     if(box != NULL)
01013     {
01014       if(box->isChecked())
01015       {
01016         box->setChecked(false);
01017       }
01018       else
01019       {
01020         box->setChecked(true);
01021       }
01022     }
01023   }
01024   dofSelectionTableChanged();
01025 }
01026 
01027 void PlanningDescriptionConfigurationWizard::dofSelectionTableChanged()
01028 {
01029   int xind = 0;
01030   map<string, bool> cdof_map;
01031   const vector<KinematicModel::JointModel*>& jmv = cm_->getKinematicModel()->getJointModels();
01032   for(unsigned int i = 1; i < jmv.size(); i++)
01033   {
01034     const map<string, pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
01035     for(map<string, pair<double, double> >::const_iterator it = joint_bounds.begin(); it != joint_bounds.end(); it++)
01036     {
01037       QCheckBox* checkBox = dynamic_cast<QCheckBox*> (dof_selection_table_->cellWidget(xind, 3));
01038 
01039       if(checkBox != NULL)
01040       {
01041         cdof_map[it->first] = checkBox->isChecked();
01042       }
01043       xind++;
01044     }
01045   }
01046   ops_gen_->generateSamplingStructures(cdof_map);
01047 }
01048 
01049 void PlanningDescriptionConfigurationWizard::popupGenericWarning(const char* text)
01050 {
01051   generic_dialog_label_->setText(text);
01052   generic_dialog_->show();
01053   generic_dialog_->setVisible(true);
01054   generic_dialog_->setModal(true);
01055   ROS_INFO("Showing warning: %s", text);
01056 }
01057 
01058 void PlanningDescriptionConfigurationWizard::writeFiles()
01059 {
01060   package_directory_ = output_wizard_page_->getPackagePathField();
01061   int ok = chdir(package_directory_.c_str());
01062 
01063   if(ok != 0)
01064   {
01065     ROS_WARN("Unable to change directory to %s", package_directory_.c_str());
01066     string warning = "Failed to change directory to ";
01067     warning += package_directory_;
01068     popupFileFailure(warning.c_str());
01069     return;
01070   }
01071 
01072   string del_com = "rm -rf " + dir_name_;
01073   ok = system(del_com.c_str());
01074 
01075   if(ok != 0)
01076   {
01077     ROS_WARN_STREAM("Failed to delete old package!");
01078     popupFileFailure("Failed to delete existing package.");
01079   }
01080 
01081   string mdir = "roscreate-pkg " + dir_name_;
01082   mdir += " planning_environment kinematics_base arm_kinematics_constraint_aware ompl_ros_interface ";
01083   mdir += "trajectory_filter_server constraint_aware_spline_smoother move_arm";
01084   ok = system(mdir.c_str());
01085 
01086   if(ok != 0)
01087   {
01088     ROS_WARN_STREAM("Failed to create ros package!");
01089     popupFileFailure("Creating ros package failed.");
01090     return;
01091   }
01092 
01093   mdir = "mkdir -p " + dir_name_ + "/config";
01094   ok = system(mdir.c_str());
01095   if(ok != 0)
01096   {
01097     ROS_WARN_STREAM("Making subdirectory not ok");
01098     popupFileFailure("Creating subdirectory /config failed.");
01099     return;
01100   }
01101 
01102   mdir = "mkdir -p " + dir_name_ + "/launch";
01103   ok = system(mdir.c_str());
01104   if(ok != 0)
01105   {
01106     ROS_WARN_STREAM("Making subdirectory not ok");
01107     popupFileFailure("Creating subdirectory /launch failed");
01108     return;
01109   }
01110 
01111   if(wizard_mode_ == PlanningDescriptionConfigurationWizard::Easy)
01112   {
01113     ROS_INFO("Automatically configuring.");
01114     AutoConfigureThread* thread = new AutoConfigureThread(this);
01115     thread->start();
01116   }
01117 
01118   else
01119   {
01120     outputJointLimitsYAML();
01121     output_wizard_page_->updateProgressBar(10);
01122     outputOMPLGroupYAML();
01123     output_wizard_page_->updateProgressBar(20);
01124     outputPlanningDescriptionYAML();
01125     output_wizard_page_->updateProgressBar(30);
01126     outputOMPLLaunchFile();
01127     output_wizard_page_->updateProgressBar(40);
01128     outputKinematicsLaunchFiles();
01129     output_wizard_page_->updateProgressBar(50);
01130     outputTrajectoryFilterLaunch();
01131     output_wizard_page_->updateProgressBar(65);
01132     outputPlanningEnvironmentLaunch();
01133     output_wizard_page_->updateProgressBar(75);
01134     outputMoveGroupLaunchFiles();
01135     outputArmNavigationLaunchFile();
01136     outputPlanningComponentVisualizerLaunchFile();
01137     output_wizard_page_->updateProgressBar(100);
01138 
01139     output_wizard_page_->setSuccessfulGeneration();
01140   }
01141 }
01142 
01143 void PlanningDescriptionConfigurationWizard::labelChanged(const char* label)
01144 {
01145   output_wizard_page_->setProgressLabel(label);
01146 }
01147 
01148 void PlanningDescriptionConfigurationWizard::autoConfigure()
01149 {
01150   progress_ = 0;
01152   // SETUP JOINTS
01154   const vector<KinematicModel::JointModel*>& jmv = cm_->getKinematicModel()->getJointModels();
01155   vector<bool> consider_dof;
01156   //assuming that 0th is world joint, which we don't want to include
01157   for(unsigned int i = 1; i < jmv.size(); i++)
01158   {
01159     const map<string, pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
01160     for(map<string, pair<double, double> >::const_iterator it = joint_bounds.begin(); it != joint_bounds.end(); it++)
01161     {
01162       consider_dof.push_back(true);
01163     }
01164   }
01165   int xind = 0;
01166   map<string, bool> cdof_map;
01167   for(unsigned int i = 1; i < jmv.size(); i++)
01168   {
01169     const map<string, pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
01170     for(map<string, pair<double, double> >::const_iterator it = joint_bounds.begin(); it != joint_bounds.end(); it++)
01171     {
01172       cdof_map[it->first] = consider_dof[xind++];
01173     }
01174   }
01175   ops_gen_->generateSamplingStructures(cdof_map);
01176 
01177   progress_ = 1;
01178   emit changeProgress(1);
01180   // ADJACENT PAIRS IN COLLISION
01182   vector<CollisionOperationsGenerator::StringPair> adj_links; 
01183   ops_gen_->generateAdjacentInCollisionPairs(adj_links);
01184   ops_gen_->disablePairCollisionChecking(adj_links);
01185   disable_map_[CollisionOperationsGenerator::ADJACENT] = adj_links;
01186 
01187   emit changeLabel("Finding always in collision pairs....");
01189   // ALWAYS-DEFAULT IN COLLISION
01191 
01192   vector<CollisionOperationsGenerator::StringPair> always_in_collision;
01193   vector<CollisionOperationsGenerator::CollidingJointValues> in_collision_joint_values;
01194 
01195   ops_gen_->generateAlwaysInCollisionPairs(always_in_collision, in_collision_joint_values);
01196 
01197   lock_.lock();
01198   robot_state_->setKinematicStateToDefault();
01199   lock_.unlock();
01200 
01201   progress_ = 10;
01202   emit changeProgress(10);
01203   emit changeLabel("Finding Default In Collision Pairs....");
01204 
01205   ops_gen_->disablePairCollisionChecking(always_in_collision);
01206   disable_map_[CollisionOperationsGenerator::ALWAYS] = always_in_collision;
01207   vector<CollisionOperationsGenerator::StringPair> default_in_collision;
01208   ops_gen_->generateDefaultInCollisionPairs(default_in_collision, in_collision_joint_values);
01209   ops_gen_->disablePairCollisionChecking(default_in_collision);
01210   disable_map_[CollisionOperationsGenerator::DEFAULT] = default_in_collision;
01211 
01212   progress_ = 33;
01213   emit changeProgress(33);
01214   emit changeLabel("Finding Often In Collision Pairs....");
01216   // OFTEN IN COLLISION
01218 
01219   vector<CollisionOperationsGenerator::StringPair> often_in_collision;
01220   vector<double> percentages;
01221   ops_gen_->generateOftenInCollisionPairs(often_in_collision, percentages, in_collision_joint_values);
01222   ops_gen_->disablePairCollisionChecking(often_in_collision);
01223   disable_map_[CollisionOperationsGenerator::OFTEN] = often_in_collision;
01224 
01225   progress_ = 60;
01226   emit changeProgress(60);
01227   emit changeLabel("Finding Never In Collision Pairs....");
01229   // OCC-NEVER IN COLLISION
01231 
01232   vector<CollisionOperationsGenerator::StringPair> in_collision;
01233   vector<CollisionOperationsGenerator::StringPair> not_in_collision;
01234 
01235   ops_gen_->generateOccasionallyAndNeverInCollisionPairs(in_collision, not_in_collision, percentages,
01236                                                          in_collision_joint_values);
01237   disable_map_[CollisionOperationsGenerator::NEVER] = not_in_collision;
01238   ops_gen_->disablePairCollisionChecking(not_in_collision);
01239   progress_ = 90;
01240   emit changeProgress(90);
01241   emit changeLabel("Writing Files....");
01242 
01243   outputJointLimitsYAML();
01244   outputOMPLGroupYAML();
01245   outputPlanningDescriptionYAML();
01246   outputOMPLLaunchFile();
01247   outputKinematicsLaunchFiles();
01248   outputTrajectoryFilterLaunch();
01249   outputPlanningEnvironmentLaunch();
01250   outputMoveGroupLaunchFiles();
01251   outputArmNavigationLaunchFile();
01252   outputPlanningComponentVisualizerLaunchFile();
01253   progress_ = 100;
01254   emit changeProgress(100);
01255 }
01256 
01257 int PlanningDescriptionConfigurationWizard::nextId() const
01258 {
01259   switch (currentId())
01260   {
01261   case StartPage:
01262     return SetupGroupsPage;
01263   case SetupGroupsPage:
01264     return setup_groups_page_->nextId();
01265   case KinematicChainsPage:
01266     {
01267       if(!kinematic_chain_page_->getReturnToGroups()) {
01268         if(wizard_mode_ == PlanningDescriptionConfigurationWizard::Advanced)
01269         {
01270           return SelectDOFPage;
01271         }
01272         else
01273         {
01274           return OutputFilesPage;
01275         }
01276       } else {
01277         return SetupGroupsPage;
01278       }
01279     }
01280   case JointCollectionsPage:
01281     {
01282       if(!joint_collections_page_->getReturnToGroups()) {
01283         if(wizard_mode_ == PlanningDescriptionConfigurationWizard::Advanced)
01284         {
01285           return SelectDOFPage;
01286         }
01287         else
01288         {
01289           return OutputFilesPage;
01290         }
01291       } else {
01292         return SetupGroupsPage;
01293       }
01294     }
01295   case SelectDOFPage:
01296     return AdjacentLinkPage;
01297     
01298   case AdjacentLinkPage:
01299     return AlwaysInCollisionPage;
01300     
01301   case AlwaysInCollisionPage:
01302     return DefaultInCollisionPage;
01303     
01304   case DefaultInCollisionPage:
01305     return OftenInCollisionPage;
01306     
01307   case OftenInCollisionPage:
01308     return OccasionallyInCollisionPage;
01309     
01310   case OccasionallyInCollisionPage:
01311     return OutputFilesPage;
01312     
01313   case OutputFilesPage:
01314     return -1;
01315       
01316   default:
01317     return -1;
01318   }
01319 }
01320 
01321 void PlanningDescriptionConfigurationWizard::update()
01322 {
01323   output_wizard_page_->updateProgressBar(progress_);
01324   if(progress_ >= 100)
01325   {
01326     output_wizard_page_->setSuccessfulGeneration();
01327   }
01328 
01329   QWizard::update();
01330 }
01331 
01332 void PlanningDescriptionConfigurationWizard::easyButtonToggled(bool checkState)
01333 {
01334   if(checkState)
01335   {
01336     wizard_mode_ = PlanningDescriptionConfigurationWizard::Easy;
01337   }
01338 }
01339 
01340 void PlanningDescriptionConfigurationWizard::hardButtonToggled(bool checkState)
01341 {
01342   if(checkState)
01343   {
01344     wizard_mode_ = PlanningDescriptionConfigurationWizard::Advanced;
01345   }
01346 }
01347 
01348 void PlanningDescriptionConfigurationWizard::verySafeButtonToggled(bool checkState)
01349 {
01350   if(checkState)
01351   {
01352     ops_gen_->setSafety(CollisionOperationsGenerator::VerySafe);
01353   }
01354 }
01355 
01356 void PlanningDescriptionConfigurationWizard::safeButtonToggled(bool checkState)
01357 {
01358   if(checkState)
01359   {
01360     ops_gen_->setSafety(CollisionOperationsGenerator::Safe);
01361   }
01362 }
01363 
01364 void PlanningDescriptionConfigurationWizard::normalButtonToggled(bool checkState)
01365 {
01366   if(checkState)
01367   {
01368     ops_gen_->setSafety(CollisionOperationsGenerator::Normal);
01369   }
01370 }
01371 
01372 void PlanningDescriptionConfigurationWizard::fastButtonToggled(bool checkState)
01373 {
01374   if(checkState)
01375   {
01376     ops_gen_->setSafety(CollisionOperationsGenerator::Fast);
01377   }
01378 }
01379 
01380 void PlanningDescriptionConfigurationWizard::veryFastButtonToggled(bool checkState)
01381 {
01382   if(checkState)
01383   {
01384     ops_gen_->setSafety(CollisionOperationsGenerator::VeryFast);
01385   }
01386 }
01387 
01388 void PlanningDescriptionConfigurationWizard::visualizeCollision(vector<CollisionOperationsGenerator::CollidingJointValues>& jointValues,
01389                                                                 vector<CollisionOperationsGenerator::StringPair>& pairs,
01390                                                                 int& index, std_msgs::ColorRGBA& color)
01391 {
01392 
01393   if(index >= (int)(pairs.size()) || index < 0 || pairs.size() == 0)
01394   {
01395     return;
01396   }
01397   lock_.lock();
01398 
01399   robot_state_->setKinematicState(jointValues[index]);
01400 
01401   if(!cm_->isKinematicStateInCollision(*robot_state_))
01402   {
01403     ROS_INFO_STREAM("Really should be in collision");
01404   }
01405 
01406   collision_markers_.markers.clear();
01407 
01408   vector<EnvironmentModel::Contact> coll_space_contacts;
01409   cm_->getCollisionSpace()->getAllCollisionContacts(coll_space_contacts, 1);
01410 
01411   Marker marker;
01412   Marker marker2;
01413   for(unsigned int j = 0; j < coll_space_contacts.size(); j++)
01414   {
01415     if((coll_space_contacts[j].body_name_1 == pairs[index].first && coll_space_contacts[j].body_name_2
01416         == pairs[index].second) || (coll_space_contacts[j].body_name_1 == pairs[index].second
01417                                     && coll_space_contacts[j].body_name_2 == pairs[index].first))
01418     {
01419       marker = transformEnvironmentModelContactInfoMarker(coll_space_contacts[j]);
01420       marker2 = transformEnvironmentModelContactInfoMarker(coll_space_contacts[j]);
01421       marker.id = 0;
01422       marker.id = 1;
01423       marker.color = color;
01424       marker.lifetime = ros::Duration(0.2);
01425       marker2.type = Marker::ARROW;
01426       marker2.color.r = 1.0;
01427       marker2.color.g = 0.0;
01428       marker2.color.b = 0.0;
01429       marker2.color.a = 1.0;
01430       marker2.scale.x = 0.5;
01431       marker2.scale.y = 0.5;
01432       marker2.scale.z = 0.5;
01433       marker2.pose.position.z = marker2.pose.position.z+.65;
01434       marker2.pose.orientation.x = 0.0;
01435       marker2.pose.orientation.y = 1.0;
01436       marker2.pose.orientation.z = 0.0;
01437       marker2.pose.orientation.w = 1.0;
01438       marker2.lifetime = ros::Duration(0.2);
01439       collision_markers_.markers.push_back(marker);
01440       collision_markers_.markers.push_back(marker2);
01441     } 
01442   }
01443   lock_.unlock();
01444 
01445 }
01446 
01447 void PlanningDescriptionConfigurationWizard::setupQtPages()
01448 {
01449   initStartPage();
01450 
01451   setup_groups_page_ = new SetupGroupsWizardPage(this);
01452   setPage(SetupGroupsPage, setup_groups_page_);
01453 
01454   kinematic_chain_page_ = new KinematicChainWizardPage(this);
01455   setPage(KinematicChainsPage, kinematic_chain_page_);
01456 
01457   joint_collections_page_ = new JointCollectionWizardPage(this);
01458   setPage(JointCollectionsPage, joint_collections_page_);
01459 
01460   initSelectDofPage();
01461 
01462   adjacent_link_page_ = new CollisionsWizardPage(this, CollisionOperationsGenerator::ADJACENT);
01463   setPage(AdjacentLinkPage, adjacent_link_page_);
01464 
01465   always_in_collision_page_ = new CollisionsWizardPage(this, CollisionOperationsGenerator::ALWAYS);
01466   setPage(AlwaysInCollisionPage, always_in_collision_page_);
01467 
01468   default_in_collision_page_ = new CollisionsWizardPage(this, CollisionOperationsGenerator::DEFAULT);
01469   setPage(DefaultInCollisionPage, default_in_collision_page_);
01470 
01471   often_in_collision_page_ = new CollisionsWizardPage(this, CollisionOperationsGenerator::OFTEN);
01472   setPage(OftenInCollisionPage, often_in_collision_page_);
01473 
01474   occasionally_in_collision_page_ = new CollisionsWizardPage(this, CollisionOperationsGenerator::NEVER);
01475   setPage(OccasionallyInCollisionPage, occasionally_in_collision_page_);
01476 
01477   output_wizard_page_ = new OutputWizardPage(this);
01478   setPage(OutputFilesPage, output_wizard_page_);
01479  
01480   generic_dialog_ = new QDialog(this);
01481   QVBoxLayout* gDialogLayout = new QVBoxLayout(generic_dialog_);
01482   generic_dialog_label_ = new QLabel(generic_dialog_);
01483   generic_dialog_label_->setText("Warning!");
01484   gDialogLayout->addWidget(generic_dialog_label_);
01485   generic_dialog_->setLayout(gDialogLayout);
01486 
01487   file_failure_dialog_ = new QDialog(this);
01488   QVBoxLayout* filefailureLayout = new QVBoxLayout(file_failure_dialog_);
01489   QLabel* fileFailureText = new QLabel(file_failure_dialog_);
01490   fileFailureText->setText("Failed to create files! Reason: ");
01491   file_failure_reason_ = new QLabel(this);
01492   file_failure_reason_->setText("unspecified.");
01493   filefailureLayout->addWidget(fileFailureText);
01494   filefailureLayout->addWidget(file_failure_reason_);
01495   QDialogButtonBox* file_failure_box = new QDialogButtonBox(QDialogButtonBox::Ok);
01496   filefailureLayout->addWidget(file_failure_box);
01497   connect(file_failure_box, SIGNAL(accepted()), file_failure_dialog_, SLOT(accept()));
01498   file_failure_dialog_->setLayout(filefailureLayout);
01499 
01500   confirm_group_replace_dialog_ = new QDialog(this);
01501   QVBoxLayout* confirm_group_replace_layout = new QVBoxLayout(confirm_group_replace_dialog_);
01502   confirm_group_text_ = new QLabel(this);
01503   confirm_group_text_->setText("Really replace group ");
01504   confirm_group_replace_layout->addWidget(confirm_group_text_);
01505   QDialogButtonBox* group_button_box = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel);
01506   confirm_group_replace_layout->addWidget(group_button_box);
01507   connect(group_button_box, SIGNAL(accepted()), confirm_group_replace_dialog_, SLOT(accept()));
01508   connect(group_button_box, SIGNAL(rejected()), confirm_group_replace_dialog_, SLOT(reject()));
01509   confirm_group_replace_dialog_->setLayout(confirm_group_replace_layout);
01510 }
01511 
01512 void PlanningDescriptionConfigurationWizard::initStartPage()
01513 {
01514   start_page_ = new QWizardPage(this);
01515   start_page_->setTitle("Planning Components Configuration Wizard");
01516   QHBoxLayout* layout = new QHBoxLayout(start_page_);
01517 
01518   QImage* image = new QImage();
01519   if(chdir(ros::package::getPath("planning_environment").c_str()) != 0)
01520   {
01521     ROS_ERROR("FAILED TO CHANGE PACKAGE TO %s", ros::package::getPath("planning_environment").c_str());
01522   }
01523 
01524   if(!image->load("./resources/wizard.png"))
01525   {
01526     ROS_ERROR("FAILED TO LOAD ./resources/wizard.png");
01527   }
01528   ROS_INFO("Loaded Image with %d bytes.", image->byteCount());
01529 
01530   QLabel* imageLabel = new QLabel(start_page_);
01531   imageLabel->setPixmap(QPixmap::fromImage(*image));
01532   layout->addWidget(imageLabel);
01533   imageLabel->setMinimumHeight(10);  
01534   imageLabel->setMinimumWidth(10);  
01535   start_page_->setSubTitle(
01536                                                     "Welcome to the ROS planning components configuration wizard! This wizard will guide you through"
01537                            " creating a planning configuration for your robot.\nThe robot's URDF location should have been passed into the"
01538                            " program as a command line on startup.");
01539 
01540   QGroupBox* descBox = new QGroupBox(start_page_);
01541 
01542   QLabel* label = new QLabel(descBox);
01543   QVBoxLayout* descLayout = new QVBoxLayout(descBox);
01544   label->setText("After you've selected your robot's planning groups,"
01545                  "\nand set up collision information this wizard will"
01546                  "\nautomatically generate a planning stack for your"
01547                  "\nrobot, and in no time your robot's arms will be "
01548                  "\nable to plan around obstacles efficiently!");
01549   //label->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Maximum);
01550   label->setMinimumWidth(1); 
01551   label->setMinimumHeight(1);
01552 
01553   layout->addWidget(imageLabel);
01554   descLayout->addWidget(label);
01555 
01556   label->setAlignment(Qt::AlignTop);
01557 
01558   //descBox->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Maximum);
01559   descBox->setAlignment(Qt::AlignTop);
01560 
01561   QGroupBox* modeGroupBox = new QGroupBox(descBox);
01562   modeGroupBox->setTitle("Select Mode");
01563 
01564   QVBoxLayout* modeGroupBoxLayout = new QVBoxLayout(modeGroupBox);
01565   QLabel* modeGroupBoxDesc = new QLabel(modeGroupBox);
01566   modeGroupBoxDesc->setMinimumWidth(1);
01567   modeGroupBoxDesc->setMinimumHeight(1);
01568   modeGroupBoxDesc->setText("In Easy mode, your robot will be automatically tested for self-collisions,\nand a planning"
01569                             " parameter file will be automatically generated.\nAdvanced mode allows you to manually configure collision checking.");
01570   modeGroupBoxLayout->addWidget(modeGroupBoxDesc);
01571 
01572   QRadioButton* easyButton = new QRadioButton(modeGroupBox);
01573   easyButton->setText("Easy");
01574   modeGroupBoxLayout->addWidget(easyButton);
01575   easyButton->setChecked(true);
01576 
01577   connect(easyButton, SIGNAL(toggled(bool)), this, SLOT(easyButtonToggled(bool)));
01578 
01579   QRadioButton* hardButton = new QRadioButton(modeGroupBox);
01580   hardButton->setText("Advanced");
01581   modeGroupBoxLayout->addWidget(hardButton);
01582 
01583   connect(hardButton, SIGNAL(toggled(bool)), this, SLOT(hardButtonToggled(bool)));
01584 
01585   modeGroupBox->setLayout(modeGroupBoxLayout);
01586   modeGroupBox->setAlignment(Qt::AlignTop | Qt::AlignLeft);
01587 
01588   descLayout->addWidget(modeGroupBox);
01589   modeGroupBox->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Maximum);
01590 
01591   QGroupBox* safetyGroupBox = new QGroupBox(descBox);
01592   safetyGroupBox->setTitle("Select Self-collision Sampling Density");
01593 
01594   QVBoxLayout* safetyGroupBoxLayout = new QVBoxLayout(safetyGroupBox);
01595   QLabel* safetyGroupBoxDesc = new QLabel(safetyGroupBox);
01596   safetyGroupBoxDesc->setText("Parameterizes the number of joint space samples used to "
01597                               "\ndetermine whether certain robot poses are in collision."
01598                               "\nDenser setting swill sample longer, while sparser settings" 
01599                               "\nare more likely to give efficient collision checking that "
01600                               "\nmay not detect rare self-collisions.");
01601 
01602   safetyGroupBoxDesc->setMinimumWidth(1);
01603   safetyGroupBoxDesc->setMinimumHeight(1);
01604   safetyGroupBoxDesc->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01605   safetyGroupBoxLayout->addWidget(safetyGroupBoxDesc);
01606 
01607   QRadioButton* verySafeButton = new QRadioButton(safetyGroupBox);
01608   verySafeButton->setText("Very Dense");
01609   safetyGroupBoxLayout->addWidget(verySafeButton);
01610   verySafeButton->setChecked(true);
01611 
01612   connect(verySafeButton, SIGNAL(toggled(bool)), this, SLOT(verySafeButtonToggled(bool)));
01613 
01614   QRadioButton* safeButton = new QRadioButton(safetyGroupBox);
01615   safeButton->setText("Dense");
01616   safetyGroupBoxLayout->addWidget(safeButton);
01617 
01618   connect(safeButton, SIGNAL(toggled(bool)), this, SLOT(safeButtonToggled(bool)));
01619 
01620   QRadioButton* normalButton = new QRadioButton(safetyGroupBox);
01621   normalButton->setText("Normal");
01622   safetyGroupBoxLayout->addWidget(normalButton);
01623   normalButton->setChecked(true);
01624 
01625   connect(normalButton, SIGNAL(toggled(bool)), this, SLOT(normalButtonToggled(bool)));
01626 
01627   QRadioButton* fastButton = new QRadioButton(safetyGroupBox);
01628   fastButton->setText("Sparse");
01629   safetyGroupBoxLayout->addWidget(fastButton);
01630 
01631   connect(fastButton, SIGNAL(toggled(bool)), this, SLOT(fastButtonToggled(bool)));
01632 
01633   QRadioButton* veryFastButton = new QRadioButton(safetyGroupBox);
01634   veryFastButton->setText("Very Sparse");
01635   safetyGroupBoxLayout->addWidget(veryFastButton);
01636 
01637   connect(veryFastButton, SIGNAL(toggled(bool)), this, SLOT(veryFastButtonToggled(bool)));
01638 
01639 
01640   safetyGroupBox->setLayout(safetyGroupBoxLayout);
01641   safetyGroupBox->setAlignment(Qt::AlignTop | Qt::AlignLeft);
01642 
01643   descLayout->addWidget(safetyGroupBox);
01644   //safetyGroupBox->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Maximum);
01645 
01646   layout->addWidget(descBox);
01647 
01648   setPage(StartPage, start_page_);
01649   start_page_->setLayout(layout);
01650   start_page_->setMinimumWidth(400);
01651   start_page_->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01652 }
01653 
01654 void PlanningDescriptionConfigurationWizard::initSelectDofPage()
01655 {
01656   select_dof_page_ = new QWizardPage(this);
01657   select_dof_page_->setTitle("DOF Sampling");
01658   QVBoxLayout* layout = new QVBoxLayout(select_dof_page_);
01659   select_dof_page_->setSubTitle("Select degrees of freedom to sample for collisions. The wizard will run your robot"
01660                                 " through a set of subsamples of these joints and check each pair of links for"
01661                                 " collisions. Unselected joints will remain in their default positions"
01662                                 " during these tests.");
01663 
01664   dof_selection_table_ = new QTableWidget(select_dof_page_);
01665   layout->addWidget(dof_selection_table_);
01666 
01667   QPushButton* toggleSelected = new QPushButton(select_dof_page_);
01668   toggleSelected->setText("Toggle Selected");
01669   layout->addWidget(toggleSelected);
01670   toggleSelected->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01671 
01672   connect(toggleSelected, SIGNAL(clicked()), this, SLOT(dofTogglePushed()));
01673 
01674   setPage(SelectDOFPage, select_dof_page_);
01675   select_dof_page_->setLayout(layout);
01676 
01677   createDofPageTable();
01678 
01679 }
01680 
01681 void PlanningDescriptionConfigurationWizard::createDofPageTable()
01682 {
01683   const vector<KinematicModel::JointModel*>& jmv = cm_->getKinematicModel()->getJointModels();
01684   vector<bool> consider_dof;
01685 
01686   int numDofs = 0;
01687   //assuming that 0th is world joint, which we don't want to include
01688   for(unsigned int i = 1; i < jmv.size(); i++)
01689   {
01690     const map<string, pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
01691     for(map<string, pair<double, double> >::const_iterator it = joint_bounds.begin(); it != joint_bounds.end(); it++)
01692     {
01693       consider_dof.push_back(true);
01694       numDofs++;
01695     }
01696   }
01697 
01698   dof_selection_table_->clear();
01699   dof_selection_table_->setRowCount(numDofs);
01700   dof_selection_table_->setColumnCount(4);
01701   dof_selection_table_->setColumnWidth(0, 300);
01702   dof_selection_table_->setColumnWidth(1, 300);
01703   dof_selection_table_->setColumnWidth(2, 300);
01704   dof_selection_table_->setColumnWidth(3, 300);
01705 
01706   QStringList horizontalLabels;
01707   horizontalLabels.append("DOF Name");
01708   horizontalLabels.append("Lower Bound (radians)");
01709   horizontalLabels.append("Upper Bound (radians)");
01710   horizontalLabels.append("Consider DOF?");
01711   dof_selection_table_->setHorizontalHeaderLabels(horizontalLabels);
01712 
01713   int ind = 1;
01714   for(unsigned int i = 1; i < jmv.size(); i++)
01715   {
01716     const map<string, pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
01717 
01718     for(map<string, pair<double, double> >::const_iterator it = joint_bounds.begin(); it != joint_bounds.end(); it++)
01719     {
01720       stringstream lowerStream;
01721       stringstream upperStream;
01722       lowerStream << it->second.first;
01723       upperStream << it->second.second;
01724 
01725       QTableWidgetItem* nameItem = new QTableWidgetItem(it->first.c_str());
01726       nameItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01727 
01728       QTableWidgetItem* lowerItem = new QTableWidgetItem(lowerStream.str().c_str());
01729       lowerItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01730 
01731       QTableWidgetItem* upperItem = new QTableWidgetItem(upperStream.str().c_str());
01732       upperItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01733 
01734       QCheckBox* check = new QCheckBox(dof_selection_table_);
01735       check->setChecked(consider_dof[ind - 1]);
01736 
01737       connect(check, SIGNAL(toggled(bool)), this, SLOT(dofSelectionTableChanged()));
01738       dof_selection_table_->setItem(ind - 1, 0, nameItem);
01739       dof_selection_table_->setItem(ind - 1, 1, lowerItem);
01740       dof_selection_table_->setItem(ind - 1, 2, upperItem);
01741       dof_selection_table_->setCellWidget(ind - 1, 3, check);
01742       ind++;
01743     }
01744   }
01745 
01746   dofSelectionTableChanged();
01747 }
01748           
01749 SetupGroupsWizardPage::SetupGroupsWizardPage(PlanningDescriptionConfigurationWizard *parent) 
01750   : QWizardPage(parent), next_from_groups_id_(PlanningDescriptionConfigurationWizard::OutputFilesPage)
01751 {
01752   parent_ = parent;
01753 
01754   setTitle("Planning Group Setup");
01755 
01756   QGridLayout* layout = new QGridLayout(this);
01757   setSubTitle("Select planning groups for your robot based on kinematic chains, or joint collections."
01758               " When you are finished, please check the checkbox and you can move on by pressing Next.");
01759   
01760   QGroupBox* selectGroupsBox = new QGroupBox(this);
01761   selectGroupsBox->setTitle("Current Groups");
01762 
01763   current_group_table_ = new QTableWidget(selectGroupsBox);
01764   QVBoxLayout* groupBoxLayout = new QVBoxLayout(selectGroupsBox);
01765   groupBoxLayout->addWidget(current_group_table_);
01766   selectGroupsBox->setLayout(groupBoxLayout);
01767   layout->addWidget(selectGroupsBox, 0, 0, 1, 1);
01768   QPushButton* deleteButton = new QPushButton(selectGroupsBox);
01769   deleteButton->setText("Delete");
01770   groupBoxLayout->addWidget(deleteButton);
01771   deleteButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01772   
01773   connect(current_group_table_, SIGNAL(itemClicked(QTableWidgetItem*)), this, SLOT(groupTableClicked()));
01774   connect(deleteButton, SIGNAL(clicked()), this, SLOT(deleteGroupButtonClicked()));
01775 
01776   QGroupBox* modeBox = new QGroupBox(this);
01777   modeBox->setTitle("Add groups");
01778   QVBoxLayout* modeBoxLayout = new QVBoxLayout(modeBox);
01779   QPushButton* add_kinematic_chain_button = new QPushButton(tr("&Add Kinematic Chain Group"));
01780   QPushButton* add_joint_collection_button = new QPushButton(tr("&Add Joint Collection Group"));
01781   modeBoxLayout->addWidget(add_kinematic_chain_button);
01782   modeBoxLayout->addWidget(add_joint_collection_button);
01783 
01784   first_group_field_ = new QLineEdit(this);
01785   registerField("first_group*", first_group_field_);
01786   first_group_field_->hide();
01787   
01788   connect(add_kinematic_chain_button, SIGNAL(clicked()), this, SLOT(addKinematicChainGroup()));
01789   connect(add_joint_collection_button, SIGNAL(clicked()), this, SLOT(addJointCollectionGroup()));
01790   
01791   modeBox->setLayout(modeBoxLayout);
01792   layout->addWidget(modeBox, 0, 2, 1, 1);
01793   
01794   setLayout(layout);
01795   layout->setAlignment(modeBox, Qt::AlignTop);
01796   
01797   setButtonText(QWizard::NextButton, "Done with groups");
01798 }
01799 
01800 
01801 void SetupGroupsWizardPage::groupTableClicked()
01802 {
01803   QList<QTableWidgetItem*> selected = current_group_table_->selectedItems();
01804   
01805   if(selected.size() == 0)
01806   {
01807     return;
01808   }
01809   else
01810   {
01811     QTableWidgetItem* first = selected[0];
01812     int row = first->row();
01813     QTableWidgetItem* groupName = current_group_table_->item(row, 0);
01814     parent_->setCurrentShowGroup(groupName->text().toStdString());
01815   }
01816 }
01817 
01818 
01819 void SetupGroupsWizardPage::updateGroupTable()
01820 {
01821   current_group_table_->clear();
01822   first_group_field_->clear();
01823   
01824   vector<string> modelNames;
01825   parent_->getKinematicModel()->getModelGroupNames(modelNames);
01826   current_group_table_->setRowCount((int)modelNames.size());
01827   current_group_table_->setColumnCount(1);
01828   current_group_table_->setColumnWidth(0, 300);
01829   current_group_table_->setDragEnabled(false);
01830   current_group_table_->setHorizontalHeaderItem(0, new QTableWidgetItem("Planning Groups"));
01831   if(!modelNames.empty()) {
01832     first_group_field_->setText(modelNames[0].c_str());
01833   }
01834   for(size_t i = 0; i < modelNames.size(); i++)
01835   {
01836     QTableWidgetItem* item = new QTableWidgetItem(modelNames[i].c_str());
01837     current_group_table_->setItem((int)i, 0, item);
01838     item->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
01839   }
01840 }
01841 
01842 void SetupGroupsWizardPage::deleteGroupButtonClicked()
01843 {
01844   QList<QTableWidgetItem*> itemList = current_group_table_->selectedItems();
01845 
01846   for(int i = 0; i < itemList.size(); i++)
01847   {
01848     if(parent_->getKinematicModel()->hasModelGroup(itemList[i]->text().toStdString()))
01849     {
01850       parent_->removeGroup(itemList[i]->text().toStdString());
01851     }
01852   }
01853   updateGroupTable();
01854 }
01855 
01856 int SetupGroupsWizardPage::nextId() const {
01857   if(next_from_groups_id_ == PlanningDescriptionConfigurationWizard::OutputFilesPage)
01858   {
01859     if(parent_->getWizardMode() == PlanningDescriptionConfigurationWizard::Advanced)
01860     {
01861       return PlanningDescriptionConfigurationWizard::SelectDOFPage;
01862     }
01863     else
01864     {
01865       return PlanningDescriptionConfigurationWizard::OutputFilesPage;
01866     }
01867   }
01868   return next_from_groups_id_;
01869 }
01870 
01871 void SetupGroupsWizardPage::addKinematicChainGroup() {
01872   next_from_groups_id_ = PlanningDescriptionConfigurationWizard::KinematicChainsPage;
01873   parent_->next();
01874   next_from_groups_id_ = PlanningDescriptionConfigurationWizard::OutputFilesPage;
01875 }
01876 
01877 void SetupGroupsWizardPage::addJointCollectionGroup() {
01878   next_from_groups_id_ = PlanningDescriptionConfigurationWizard::JointCollectionsPage;
01879   parent_->next();
01880   next_from_groups_id_ = PlanningDescriptionConfigurationWizard::OutputFilesPage;
01881 }
01882 
01883 KinematicChainWizardPage::KinematicChainWizardPage(PlanningDescriptionConfigurationWizard *parent) 
01884   : QWizardPage(parent), return_to_groups_(false)
01885 {
01886   parent_ = parent;
01887   setTitle("Select Kinematic Chain");
01888 
01889   QGridLayout* layout = new QGridLayout(this);
01890   setSubTitle("Select a planning group based on a kinematic chain."
01891               " Select a base link (the first link in the chain) and a tip link."
01892               " They must be connected by a direct line of joints.");
01893   
01894   QImage* image = new QImage();
01895   if(!image->load("./resources/chains.png"))
01896   {
01897     ROS_ERROR("FAILED TO LOAD ./resources/chains.png");
01898   }
01899   ROS_INFO("Loaded Image with %d bytes.", image->byteCount());
01900 
01901   QLabel* imageLabel = new QLabel(this);
01902   imageLabel->setPixmap(QPixmap::fromImage(*image));
01903   imageLabel->setAlignment(Qt::AlignTop);
01904   imageLabel->setMinimumHeight(10);  
01905   imageLabel->setMinimumWidth(10);  
01906   layout->addWidget(imageLabel, 0, 0, 0, 1);
01907   QGroupBox* treeBox = new QGroupBox(this);
01908   treeBox->setTitle("Links");
01909   layout->addWidget(treeBox, 0, 1, 1, 1);
01910 
01911   QVBoxLayout* treeLayout = new QVBoxLayout(treeBox);
01912   link_tree_ = new QTreeWidget(treeBox);
01913   connect(link_tree_, SIGNAL(itemSelectionChanged()), this, SLOT(showTreeLink()));
01914   treeLayout->addWidget(link_tree_);
01915   QPushButton* baseLinkButton = new QPushButton(treeBox);
01916   baseLinkButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01917   baseLinkButton->setText("Select Base Link");
01918   treeLayout->addWidget(baseLinkButton);
01919   QPushButton* tipLinkButton = new QPushButton(treeBox);
01920   tipLinkButton->setText("Select Tip Link");
01921   tipLinkButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
01922   treeLayout->addWidget(tipLinkButton);
01923   treeBox->setLayout(treeLayout);
01924   createLinkTree(parent->getKinematicModel());
01925 
01926   connect(baseLinkButton, SIGNAL(clicked()), this, SLOT(baseLinkTreeClick()));
01927   connect(tipLinkButton, SIGNAL(clicked()), this, SLOT(tipLinkTreeClick()));
01928   QGroupBox* chainBox = new QGroupBox(this);
01929   chainBox->setTitle("Kinematic Chain");
01930   QFormLayout* chainLayout = new QFormLayout(chainBox);
01931   chain_name_field_ = new QLineEdit(chainBox);
01932   base_link_field_ = new QLineEdit(chainBox);
01933   tip_link_field_ = new QLineEdit(chainBox);
01934   chainLayout->addRow("Chain Name", chain_name_field_);
01935   chainLayout->addRow("Base Link", base_link_field_);
01936   chainLayout->addRow("Tip Link", tip_link_field_);
01937   layout->addWidget(chainBox, 1, 1, 1, 2);
01938 
01939   registerField("chain_name*", chain_name_field_);
01940   registerField("base_link*", base_link_field_);
01941   registerField("tip_link*", tip_link_field_);
01942 
01943   setLayout(layout);
01944 
01945   good_group_dialog_ = new QDialog(this);
01946   QVBoxLayout* good_group_layout = new QVBoxLayout(good_group_dialog_);
01947   QLabel* good_dialog_text = new QLabel(good_group_dialog_);
01948   good_dialog_text->setText("Your group is valid and has been added to the planning groups.\nIt should be visualized in Rviz");
01949   good_group_layout->addWidget(good_dialog_text);
01950   QPushButton* return_to_groups_button = new QPushButton(tr("&Return To Groups"));
01951   QPushButton* add_another_kinematic_chain_button = new QPushButton(tr("&Add Another Kinematic Chain"));
01952   QPushButton* done_with_groups_button = new QPushButton(tr("&Done Adding Groups"));
01953   QDialogButtonBox* good_button_box = new QDialogButtonBox();
01954   good_group_layout->addWidget(good_button_box);
01955   good_button_box->addButton(return_to_groups_button, QDialogButtonBox::RejectRole);
01956   good_button_box->addButton(add_another_kinematic_chain_button, QDialogButtonBox::ResetRole);
01957   good_button_box->addButton(done_with_groups_button, QDialogButtonBox::AcceptRole);
01958   connect(good_button_box, SIGNAL(rejected()), good_group_dialog_, SLOT(reject()));
01959   connect(good_button_box, SIGNAL(rejected()), parent, SLOT(back()));
01960   connect(good_button_box, SIGNAL(accepted()), good_group_dialog_, SLOT(accept()));
01961   connect(add_another_kinematic_chain_button, SIGNAL(clicked()), this, SLOT(resetPage()));
01962   good_group_dialog_->setLayout(good_group_layout);
01963 
01964   not_ok_dialog_ = new QDialog(this);
01965   QVBoxLayout* notOkDialogLayout = new QVBoxLayout(not_ok_dialog_);
01966   QLabel* notOkText = new QLabel(not_ok_dialog_);
01967   notOkText->setText("Error! The planning group was invalid!");
01968   notOkDialogLayout->addWidget(notOkText);
01969   QDialogButtonBox* ok_button_box = new QDialogButtonBox(QDialogButtonBox::Ok);
01970   notOkDialogLayout->addWidget(ok_button_box);
01971   connect(ok_button_box, SIGNAL(accepted()), not_ok_dialog_, SLOT(accept()));
01972   not_ok_dialog_->setLayout(notOkDialogLayout);
01973 
01974   setButtonText(QWizard::NextButton, "Add Group");
01975 }
01976 
01977 bool KinematicChainWizardPage::validatePage() {
01978   
01979   KinematicModel::GroupConfig gc(getChainNameField(), getBaseLinkField(), getTipLinkField());
01980 
01981   PlanningDescriptionConfigurationWizard::GroupAddStatus stat = 
01982     parent_->addGroup(gc);
01983 
01984   if(stat == PlanningDescriptionConfigurationWizard::GroupAddSuccess) {
01985     int val = good_group_dialog_->exec();
01986     parent_->setCurrentShowGroup("");
01987     parent_->setCurrentShowLink("");
01988     if(val == QDialog::Accepted) {
01989       return true;
01990     } else if(val == QDialog::Rejected) {
01991       return false;
01992     } else {
01993       return false;
01994     }
01995   } else if(stat == PlanningDescriptionConfigurationWizard::GroupAddFailed) {
01996     not_ok_dialog_->exec();
01997     return false;
01998   } else {
01999     //cancelled duplicated
02000     return false;
02001   }
02002 }
02003 
02004 void KinematicChainWizardPage::showTreeLink() {
02005   QTreeWidgetItem* item = link_tree_->currentItem();
02006   if(item != NULL)
02007   {
02008     std::string link_name = item->text(0).toStdString();
02009     if(!link_name.empty()) {
02010       parent_->setCurrentShowGroup("");
02011       parent_->setCurrentShowLink(link_name);
02012     }
02013   }
02014 }
02015 
02016 void KinematicChainWizardPage::baseLinkTreeClick() {
02017   QTreeWidgetItem* item = link_tree_->currentItem();
02018   if(item != NULL)
02019   {
02020     base_link_field_->setText(item->text(0));
02021   }
02022 }
02023 
02024 void KinematicChainWizardPage::tipLinkTreeClick()
02025 {
02026   QTreeWidgetItem* item = link_tree_->currentItem();
02027   if(item != NULL)
02028   {
02029     tip_link_field_->setText(item->text(0));
02030   }
02031 }
02032 
02033 void KinematicChainWizardPage::createLinkTree(const planning_models::KinematicModel* kmodel)
02034 {
02035   const KinematicModel::JointModel* rootJoint = kmodel->getRoot();
02036   addLinktoTreeRecursive(rootJoint->getChildLinkModel(), NULL);
02037 
02038   link_tree_->expandToDepth(0);
02039   link_tree_->header()->setResizeMode(0, QHeaderView::ResizeToContents);
02040   link_tree_->header()->setStretchLastSection(false);
02041 }
02042 
02043 
02044 
02045 void KinematicChainWizardPage::addLinktoTreeRecursive(const KinematicModel::LinkModel* link,
02046                                                       const KinematicModel::LinkModel* parent)
02047 {
02048   QTreeWidgetItem* toAdd = new QTreeWidgetItem(link_tree_);
02049   if(parent == NULL)
02050   {
02051     toAdd->setText(0, link->getName().c_str());
02052     link_tree_->addTopLevelItem(toAdd);
02053   }
02054   else
02055   {
02056     for(int i = 0; i < link_tree_->topLevelItemCount(); i++)
02057     {
02058       if(addLinkChildRecursive(link_tree_->topLevelItem(i), link, parent->getName()))
02059       {
02060         break;
02061       }
02062     }
02063   }
02064   for(size_t i = 0; i < link->getChildJointModels().size(); i++)
02065   {
02066     addLinktoTreeRecursive(link->getChildJointModels()[i]->getChildLinkModel(), link);
02067   }
02068 }
02069 
02070 bool KinematicChainWizardPage::addLinkChildRecursive(QTreeWidgetItem* parent,
02071                                                      const KinematicModel::LinkModel* link,
02072                                                      const string& parentName)
02073 {
02074   if(parent->text(0).toStdString() == parentName)
02075   {
02076     QTreeWidgetItem* toAdd = new QTreeWidgetItem(parent);
02077     toAdd->setText(0, link->getName().c_str());
02078     parent->addChild(toAdd);
02079     return true;
02080   }
02081   else
02082   {
02083     for(int i = 0; i < parent->childCount(); i++)
02084     {
02085       if(addLinkChildRecursive(parent->child(i), link, parentName))
02086       {
02087         return true;
02088       }
02089     }
02090   }
02091 
02092   return false;
02093 }
02094 
02095 JointCollectionWizardPage::JointCollectionWizardPage(PlanningDescriptionConfigurationWizard *parent) 
02096   : QWizardPage(parent), return_to_groups_(false)
02097 { 
02098   parent_ = parent;
02099   setTitle("Select Joint Collections");
02100 
02101   QGridLayout* layout = new QGridLayout(this);
02102 
02103   setSubTitle("Select an arbitrary group of joints to form a planning group.");
02104 
02105   QImage* image = new QImage();
02106   if(!image->load("./resources/groups.png"))
02107   {
02108     ROS_ERROR("FAILED TO LOAD ./resources/groups.png");
02109   }
02110 
02111   QLabel* imageLabel = new QLabel(this);
02112   imageLabel->setPixmap(QPixmap::fromImage(*image));
02113   imageLabel->setAlignment(Qt::AlignTop);
02114   imageLabel->setMinimumHeight(10);  
02115   imageLabel->setMinimumWidth(10);  
02116 
02117   layout->addWidget(imageLabel, 0, 1, 1, 1);
02118 
02119   QGroupBox* jointBox = new QGroupBox(this);
02120   jointBox->setTitle("Joints");
02121 
02122 
02123   QVBoxLayout* jointLayout = new QVBoxLayout(jointBox);
02124   joint_table_ = new QTableWidget(jointBox);
02125   jointLayout->addWidget(joint_table_);
02126   jointBox->setLayout(jointLayout);
02127   layout->addWidget(jointBox, 0, 0, 3, 1);
02128 
02129   QPushButton* selectButton = new QPushButton(jointBox);
02130   selectButton->setText("v Select v");
02131   jointLayout->addWidget(selectButton);
02132   selectButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02133 
02134   first_joint_field_ = new QLineEdit(this);
02135   registerField("first_joint*", first_joint_field_);
02136   first_joint_field_->hide();
02137 
02138   connect(selectButton, SIGNAL(clicked()), this, SLOT(selectJointButtonClicked()));
02139 
02140   QGroupBox* selectedBox = new QGroupBox(this);
02141   selectedBox->setTitle("Joint Group");
02142 
02143   QVBoxLayout* selectedLayout = new QVBoxLayout(selectedBox);
02144   selected_joint_table_ = new QTableWidget(jointBox);
02145   jointLayout->addWidget(selected_joint_table_);
02146 
02147   QPushButton* deselectButton = new QPushButton(jointBox);
02148   deselectButton->setText("^ Deselect ^");
02149   jointLayout->addWidget(deselectButton);
02150   deselectButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02151 
02152   connect(deselectButton, SIGNAL(clicked()), this, SLOT(deselectJointButtonClicked()));
02153 
02154   QLabel* jointGroupLabel = new QLabel(selectedBox);
02155   jointGroupLabel->setText(" Joint Group Name: ");
02156   joint_group_name_field_ = new QLineEdit(selectedBox);
02157   selectedLayout->addWidget(jointGroupLabel);
02158   selectedLayout->addWidget(joint_group_name_field_);
02159   selectedBox->setLayout(selectedLayout);
02160   layout->addWidget(selectedBox, 1, 1, 1, 1);
02161   setLayout(layout);
02162 
02163   createJointCollectionTables();
02164 
02165   good_group_dialog_ = new QDialog(this);
02166   QVBoxLayout* good_group_layout = new QVBoxLayout(good_group_dialog_);
02167   QLabel* good_dialog_text = new QLabel(good_group_dialog_);
02168   good_dialog_text->setText("Your group is valid and has been added to the planning groups.\nIt should be visualized in Rviz");
02169   good_group_layout->addWidget(good_dialog_text);
02170   QPushButton* return_to_groups_button = new QPushButton(tr("&Return To Groups"));
02171   QPushButton* add_another_kinematic_chain_button = new QPushButton(tr("&Add Another Joint Collection"));
02172   QPushButton* done_with_groups_button = new QPushButton(tr("&Done Adding Groups"));
02173   QDialogButtonBox* good_button_box = new QDialogButtonBox();
02174   good_group_layout->addWidget(good_button_box);
02175   good_button_box->addButton(return_to_groups_button, QDialogButtonBox::RejectRole);
02176   good_button_box->addButton(add_another_kinematic_chain_button, QDialogButtonBox::ResetRole);
02177   good_button_box->addButton(done_with_groups_button, QDialogButtonBox::AcceptRole);
02178   connect(good_button_box, SIGNAL(rejected()), good_group_dialog_, SLOT(reject()));
02179   connect(good_button_box, SIGNAL(rejected()), parent, SLOT(back()));
02180   connect(good_button_box, SIGNAL(accepted()), good_group_dialog_, SLOT(accept()));
02181   connect(add_another_kinematic_chain_button, SIGNAL(clicked()), this, SLOT(resetPage()));
02182   good_group_dialog_->setLayout(good_group_layout);
02183 
02184   not_ok_dialog_ = new QDialog(this);
02185   QVBoxLayout* notOkDialogLayout = new QVBoxLayout(not_ok_dialog_);
02186   QLabel* notOkText = new QLabel(not_ok_dialog_);
02187   notOkText->setText("Error! The planning group was invalid!");
02188   notOkDialogLayout->addWidget(notOkText);
02189   QDialogButtonBox* ok_button_box = new QDialogButtonBox(QDialogButtonBox::Ok);
02190   notOkDialogLayout->addWidget(ok_button_box);
02191   connect(ok_button_box, SIGNAL(accepted()), not_ok_dialog_, SLOT(accept()));
02192   not_ok_dialog_->setLayout(notOkDialogLayout);
02193 
02194   setButtonText(QWizard::NextButton, "Add Group");
02195   registerField("collection_name*", joint_group_name_field_);
02196 
02197 
02198 }
02199 
02200 bool JointCollectionWizardPage::validatePage() {
02201   
02202   string new_group_name = joint_group_name_field_->text().toStdString();
02203   vector<string> joints;
02204   for(int i = 0; i < selected_joint_table_->rowCount(); i++)
02205   {
02206     joints.push_back(selected_joint_table_->item(i, 0)->text().toStdString());
02207   }
02208   
02209   vector<string> subgroups;
02210   KinematicModel::GroupConfig gc(new_group_name, joints, subgroups);
02211 
02212   PlanningDescriptionConfigurationWizard::GroupAddStatus stat = 
02213     parent_->addGroup(gc);
02214 
02215   if(stat == PlanningDescriptionConfigurationWizard::GroupAddSuccess) {
02216     int val = good_group_dialog_->exec();
02217     parent_->setCurrentShowGroup("");
02218     if(val == QDialog::Accepted) {
02219       return true;
02220     } else if(val == QDialog::Rejected) {
02221       return false;
02222     } else {
02223       return false;
02224     }
02225   } else if(stat == PlanningDescriptionConfigurationWizard::GroupAddFailed) {
02226     not_ok_dialog_->exec();
02227     return false;
02228   } else {
02229     //cancelled duplicated
02230     return false;
02231   }
02232 }
02233 
02234 void JointCollectionWizardPage::selectJointButtonClicked()
02235 {
02236   QList<QTableWidgetItem*> selected = joint_table_->selectedItems();
02237 
02238   for(int i = 0; i < selected.size(); i++)
02239   {
02240     string name = selected[i]->text().toStdString();
02241     bool alreadyExists = false;
02242     int rowToAdd = 0;
02243     for(int r = 0; r < selected_joint_table_->rowCount(); r++)
02244     {
02245       QTableWidgetItem* item = selected_joint_table_->item(r, 0);
02246 
02247       if(item->text().toStdString() == name)
02248       {
02249         alreadyExists = true;
02250         break;
02251       }
02252       rowToAdd = r + 1;
02253     }
02254 
02255     if(!alreadyExists)
02256     {
02257       selected_joint_table_->setRowCount(selected_joint_table_->rowCount() + 1);
02258       QTableWidgetItem* newItem = new QTableWidgetItem(name.c_str());
02259       newItem->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02260       selected_joint_table_->setItem(rowToAdd, 0, newItem);
02261     }
02262   }
02263   if(selected_joint_table_->rowCount() > 0) {
02264     first_joint_field_->setText("has");
02265   }
02266 }
02267 
02268 void JointCollectionWizardPage::deselectJointButtonClicked()
02269 {
02270   QList<QTableWidgetItem*> deselected = selected_joint_table_->selectedItems();
02271 
02272   for(int i = 0; i < deselected.size(); i++)
02273   {
02274     selected_joint_table_->removeRow(deselected[i]->row());
02275   }
02276   if(selected_joint_table_->rowCount() == 0) {
02277     first_joint_field_->clear();
02278   }
02279 }
02280 
02281 void JointCollectionWizardPage::createJointCollectionTables()
02282 {
02283   const vector<KinematicModel::JointModel*>& jmv = parent_->getKinematicModel()->getJointModels();
02284 
02285   joint_table_->setRowCount((int)jmv.size());
02286   joint_table_->setColumnCount(1);
02287   joint_table_->setColumnWidth(0, 300);
02288   selected_joint_table_->setColumnCount(1);
02289   selected_joint_table_->setColumnWidth(0, 300);
02290   QStringList headerLabels;
02291   headerLabels.append("Joint");
02292   QStringList headerLabels2;
02293   headerLabels2.append("Selected Joints");
02294   joint_table_->setHorizontalHeaderLabels(headerLabels);
02295   selected_joint_table_->setHorizontalHeaderLabels(headerLabels2);
02296   for(size_t i = 1; i < jmv.size(); i++)
02297   {
02298     QTableWidgetItem* item = new QTableWidgetItem(jmv[i]->getName().c_str());
02299     item->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02300     joint_table_->setItem((int)i - 1, 0, item);
02301   }
02302 }
02303 
02304 CollisionsWizardPage::CollisionsWizardPage(PlanningDescriptionConfigurationWizard *parent,
02305                                            CollisionOperationsGenerator::DisableType disable_type)
02306   : QWizardPage(parent), disable_type_(disable_type), 
02307     allow_enable_(true), show_percentages_(false), is_clickable_(true), coll_default_enabled_(false)
02308 {
02309   parent_ = parent;
02310   std::string title = "Links ";
02311   std::string subtitle;
02312   if(disable_type_ == CollisionOperationsGenerator::ALWAYS) {
02313     title += "Always";
02314     subtitle = "The following links are always in collision over the sample space.\nClick on a link name to visualize the collision in Rviz";
02315     allow_enable_ = false;
02316   } else if(disable_type_ == CollisionOperationsGenerator::DEFAULT) {
02317     subtitle = "The following links are in collision in the default state, and will be disabled by default.\nClick on a link name to visualize the collision in Rviz, and check Enable to force self-collision checking for the indicated pair.";
02318     title += "Default";
02319   } else if(disable_type_ == CollisionOperationsGenerator::OFTEN) {
02320     title += "Often";
02321     subtitle = "The following links are in collision in the majority of sampled states.  Enabling them will substantially slow collision checking.\nClick on a link name to visualize the collision in Rviz, and check Enable to force self-collision checking for the indicated pair.";
02322     show_percentages_ = true;
02323   } else if(disable_type == CollisionOperationsGenerator::ADJACENT) { 
02324     is_clickable_ = false;
02325     title += "Adjacent";
02326     subtitle = "The following links are adjacent in the kinematic tree and will be disabled by default.\nCheck Enable to force self-collision checking for the indicated pair";
02327   } else {
02328     coll_default_enabled_ = true;
02329     show_percentages_ = true;
02330     subtitle = "The following links are in the indicated percentage of samples. By default, link pairs that were found to be in collision in one or more samples have collision checking enabled.\nLink pairs that were never found to be in collision have been disabled.\nClick on a link name to visualize the collision in Rviz.";
02331     title += "Occasionally and Never";
02332   }
02333   title += " In Collision";
02334   setTitle(title.c_str());
02335 
02336   QVBoxLayout* layout = new QVBoxLayout(this);
02337   setSubTitle(subtitle.c_str());
02338   QPushButton* generateButton = new QPushButton(this);
02339   generateButton->setText("Generate List (May take a minute)");
02340   layout->addWidget(generateButton);
02341   connect(generateButton, SIGNAL(clicked()), this, SLOT(generateCollisionTable()));
02342   generateButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02343 
02344   collision_table_ = new QTableWidget(this);
02345   layout->addWidget(collision_table_);
02346 
02347   if(allow_enable_) {
02348     QPushButton* toggle_selected = new QPushButton(this);
02349     toggle_selected->setText("Toggle Selected");
02350     layout->addWidget(toggle_selected);
02351     connect(toggle_selected, SIGNAL(clicked()), this, SLOT(toggleTable()));
02352     toggle_selected->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02353   }
02354   if(is_clickable_) {
02355     connect(collision_table_, SIGNAL(cellClicked(int, int)), this, SLOT(tableClicked()));
02356   }
02357   setLayout(layout);
02358 }
02359 
02360 void CollisionsWizardPage::generateCollisionTable() {
02361   vector<CollisionOperationsGenerator::StringPair> not_in_collision;
02362   vector<double> percentages;
02363   if(disable_type_ == CollisionOperationsGenerator::ALWAYS) {
02364     parent_->getOperationsGenerator()->generateAlwaysInCollisionPairs(collision_pairs_, in_collision_joint_values_);
02365   } else if(disable_type_ == CollisionOperationsGenerator::DEFAULT) {
02366     parent_->getOperationsGenerator()->generateDefaultInCollisionPairs(collision_pairs_, in_collision_joint_values_);
02367   } else if(disable_type_ == CollisionOperationsGenerator::OFTEN) {
02368     parent_->getOperationsGenerator()->generateOftenInCollisionPairs(collision_pairs_, percentages, in_collision_joint_values_);
02369   } else if(disable_type_ == CollisionOperationsGenerator::ADJACENT) { 
02370     parent_->getOperationsGenerator()->generateAdjacentInCollisionPairs(collision_pairs_);
02371   } else {
02372     parent_->getOperationsGenerator()->generateOccasionallyAndNeverInCollisionPairs(collision_pairs_, not_in_collision,
02373                                                                                     percentages, in_collision_joint_values_);
02374   }
02375 
02376   if(disable_type_ == CollisionOperationsGenerator::NEVER ||
02377      disable_type_ == CollisionOperationsGenerator::OCCASIONALLY) {
02378     disable_pairs_ = not_in_collision;
02379     enable_pairs_ = collision_pairs_;
02380   } else {
02381     disable_pairs_ = collision_pairs_;
02382   }
02383 
02384   collision_table_->clear();
02385   collision_table_->setRowCount((int)collision_pairs_.size()+(int)not_in_collision.size());
02386   collision_table_->setColumnCount(2);
02387   collision_table_->setColumnWidth(0, 500);
02388   collision_table_->setColumnWidth(1, 500);
02389   QStringList titleList;
02390   titleList.append("Link A");
02391   titleList.append("Link B");
02392   if(show_percentages_) {
02393     titleList.append("% Colliding");
02394   }
02395   if(allow_enable_) {
02396     titleList.append("Enable?");
02397   }
02398   if(show_percentages_ && allow_enable_) {
02399     collision_table_->setColumnCount(4);
02400     
02401     collision_table_->setColumnWidth(0, 300);
02402     collision_table_->setColumnWidth(1, 300);
02403     collision_table_->setColumnWidth(2, 200);
02404     collision_table_->setColumnWidth(3, 200);
02405   } else if(allow_enable_) {
02406     collision_table_->setColumnCount(3);
02407     
02408     collision_table_->setColumnWidth(0, 300);
02409     collision_table_->setColumnWidth(1, 300);
02410     collision_table_->setColumnWidth(2, 300);
02411   }
02412   //shouldn't ever have percentages without allowed enable
02413 
02414   collision_table_->setHorizontalHeaderLabels(titleList);
02415   if(collision_pairs_.size() + not_in_collision.size() == 0) {
02416     collision_table_->setRowCount(1);
02417     QTableWidgetItem* no_collide = new QTableWidgetItem("No Link Pairs Of This Kind");
02418     collision_table_->setItem(0, 0, no_collide);
02419   }
02420 
02421   bool bad_percentages = false;
02422   if(show_percentages_ && collision_pairs_.size() != percentages.size()) {
02423     ROS_WARN_STREAM("Percentages enabled but percentage size " << percentages.size() 
02424                     << " not equal to collision pairs size " << collision_pairs_.size());
02425     bad_percentages = true;
02426   }
02427 
02428   for(size_t i = 0; i < collision_pairs_.size(); i++)
02429   {
02430     QTableWidgetItem* linkA = new QTableWidgetItem(collision_pairs_[i].first.c_str());
02431     linkA->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02432     QTableWidgetItem* linkB = new QTableWidgetItem(collision_pairs_[i].second.c_str());
02433     linkB->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02434 
02435     collision_table_->setItem((int)i, 0, linkA);
02436     collision_table_->setItem((int)i, 1, linkB);
02437 
02438     if(show_percentages_) {
02439       stringstream percentageStream;
02440       if(bad_percentages) {
02441         percentageStream << 1.0;
02442       } else {
02443         percentageStream << percentages[i];
02444       }
02445       QTableWidgetItem* percentage = new QTableWidgetItem(percentageStream.str().c_str());
02446       percentage->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02447       collision_table_->setItem((int)i, 2, percentage);
02448     }
02449 
02450     if(allow_enable_) {
02451       QCheckBox* enable_box = new QCheckBox(collision_table_);
02452       if(coll_default_enabled_) {
02453         enable_box->setChecked(true);
02454       } else {
02455         enable_box->setChecked(false);
02456       }
02457       connect(enable_box, SIGNAL(toggled(bool)), this, SLOT(tableChanged()));
02458       if(show_percentages_) {
02459         collision_table_->setCellWidget((int)i, 3, enable_box); 
02460       } else {
02461         collision_table_->setCellWidget((int)i, 2, enable_box);
02462       }
02463     }
02464   }
02465   for(size_t i = 0; i < not_in_collision.size(); i++)
02466   {
02467     QTableWidgetItem* linkA = new QTableWidgetItem(not_in_collision[i].first.c_str());
02468     linkA->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02469     QTableWidgetItem* linkB = new QTableWidgetItem(not_in_collision[i].second.c_str());
02470     linkB->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02471 
02472     collision_table_->setItem((int)i+collision_pairs_.size(), 0, linkA);
02473     collision_table_->setItem((int)i+collision_pairs_.size(), 1, linkB);
02474 
02475     if(show_percentages_) {
02476       stringstream percentageStream;
02477       percentageStream << 0.0;
02478       QTableWidgetItem* percentage = new QTableWidgetItem(percentageStream.str().c_str());
02479       percentage->setFlags(Qt::ItemIsEnabled | Qt::ItemIsSelectable);
02480       collision_table_->setItem((int)i+collision_pairs_.size(), 2, percentage);
02481     }
02482 
02483     if(allow_enable_) {
02484       QCheckBox* enable_box = new QCheckBox(collision_table_);
02485       enable_box->setChecked(false);
02486       connect(enable_box, SIGNAL(toggled(bool)), this, SLOT(tableChanged()));
02487       if(show_percentages_) {
02488         collision_table_->setCellWidget((int)i+collision_pairs_.size(), 3, enable_box); 
02489       } else {
02490         collision_table_->setCellWidget((int)i+collision_pairs_.size(), 2, enable_box);
02491       }
02492     }
02493   }
02494   tableChanged();
02495 }
02496 
02497 void CollisionsWizardPage::toggleTable()
02498 {
02499   unsigned int column = 2;
02500   if(show_percentages_) {
02501     column = 3;
02502   }
02503   vector<int> rows = getSelectedRows(collision_table_);
02504   for(size_t i = 0; i < rows.size(); i++)
02505   {
02506     QCheckBox* box = dynamic_cast<QCheckBox*> (collision_table_->cellWidget(rows[i], column));
02507 
02508     if(box != NULL)
02509     {
02510       if(box->isChecked())
02511       {
02512         box->setChecked(false);
02513       }
02514       else
02515       {
02516         box->setChecked(true);
02517       }
02518     }
02519   }
02520   tableChanged();
02521 }
02522 
02523 void CollisionsWizardPage::tableClicked() {
02524   QList<QTableWidgetItem*> selected = collision_table_->selectedItems();
02525 
02526   ROS_DEBUG_STREAM("Table clicked");
02527 
02528   if(selected.size() == 0)
02529   {
02530     return;
02531   }
02532 
02533   int row = selected[0]->row();
02534   ROS_DEBUG_STREAM("Showing selection " << row);
02535 
02536   std_msgs::ColorRGBA color;
02537   color.r = 1.0;
02538   color.g = 1.0;
02539   color.b = 0.2;
02540   color.a = 1.0;
02541   parent_->visualizeCollision(in_collision_joint_values_, collision_pairs_, row, color);
02542 }
02543 
02544 void CollisionsWizardPage::tableChanged() {
02545   unsigned int column = 2;
02546   if(show_percentages_) {
02547     column = 3;
02548   }
02549   for(int i = 0; i < collision_table_->rowCount(); i++)
02550   {
02551     QCheckBox* box = dynamic_cast<QCheckBox*> (collision_table_->cellWidget(i, column));
02552     if(box != NULL)
02553     {
02554       pair<string, string> link_pair;
02555       link_pair.first = collision_table_->item(i, 0)->text().toStdString();
02556       link_pair.second = collision_table_->item(i, 1)->text().toStdString();
02557       bool in_disabled = false;
02558       vector<pair<string, string> >::iterator it = disable_pairs_.begin();
02559       while(it != disable_pairs_.end())
02560       {
02561         if((*it) == link_pair) {
02562           in_disabled = true;
02563           if(box->isChecked()) {
02564             disable_pairs_.erase(it);
02565           }
02566           break;
02567         }
02568         it++;
02569       }
02570       if(!in_disabled) {
02571         if(box->isChecked()) {
02572           it = extra_disable_pairs_.begin();
02573           while(it != extra_disable_pairs_.end()) {
02574             if((*it) == link_pair) {
02575               extra_disable_pairs_.erase(it);
02576               break;
02577             }
02578             it++;
02579           }
02580         }
02581         if(!box->isChecked()) {
02582           bool in_enable = false;
02583           it = enable_pairs_.begin();
02584           while(it != enable_pairs_.end()) {
02585             if((*it) == link_pair) {
02586               in_enable = true;
02587               break;
02588             }
02589             it++;
02590           }
02591           if(!in_enable) {
02592             disable_pairs_.push_back(link_pair);
02593           } else {
02594             bool found = false;
02595             it = extra_disable_pairs_.begin();
02596             while(it != extra_disable_pairs_.end()) {
02597               if((*it) == link_pair) {
02598                 found = true;
02599                 break;
02600               }
02601               it++;
02602             }
02603             if(!found) {
02604               extra_disable_pairs_.push_back(link_pair);
02605             }
02606           }
02607         }
02608       }
02609     }
02610   }
02611 }
02612 
02613 bool CollisionsWizardPage::validatePage() {
02614   parent_->getOperationsGenerator()->disablePairCollisionChecking(disable_pairs_);
02615   //for occasionally and never 
02616   if(!extra_disable_pairs_.empty()) {
02617     parent_->setDisableMap(CollisionOperationsGenerator::OCCASIONALLY, extra_disable_pairs_);
02618   }
02619   parent_->setDisableMap(disable_type_, disable_pairs_);
02620   return true;
02621 }
02622 
02623 OutputWizardPage::OutputWizardPage(PlanningDescriptionConfigurationWizard *parent) 
02624   : QWizardPage(parent), successful_generation_(false)
02625 {
02626   setTitle("Output Files");
02627   QVBoxLayout* layout = new QVBoxLayout(this);
02628 
02629   setSubTitle("Done! The wizard will auto-generate a stack called <your_robot_name>_arm_navigation "
02630               "in the selected folder when you click the generate button below.");
02631 
02632   package_path_field_ = new QLineEdit(this);
02633   package_path_field_->setText("<absolute directory path>");
02634   layout->addWidget(package_path_field_);
02635 
02636   QPushButton* selectFileButton = new QPushButton(this);
02637   selectFileButton->setText("Select Directory ...");
02638   selectFileButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02639   file_selector_ = new QFileDialog(this);
02640   file_selector_->setFileMode(QFileDialog::Directory);
02641   file_selector_->setOption(QFileDialog::ShowDirsOnly, true);
02642 
02643   connect(file_selector_, SIGNAL(fileSelected(const QString&)), this, SLOT(fileSelected(const QString&)));
02644   connect(selectFileButton, SIGNAL(clicked()), file_selector_, SLOT(open()));
02645   layout->addWidget(selectFileButton);
02646 
02647   QPushButton* generateButton = new QPushButton(this);
02648   generateButton->setText("Generate Config Files...");
02649   generateButton->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
02650 
02651   layout->addWidget(generateButton);
02652 
02653   progress_bar_ = new QProgressBar(this);
02654   progress_label_ = new QLabel(this);
02655   layout->addWidget(progress_bar_);
02656   layout->addWidget(progress_label_);
02657 
02658   really_exit_dialog_ = new QDialog(this);
02659   QVBoxLayout* really_exit_layout = new QVBoxLayout(really_exit_dialog_);
02660   QLabel* really_exit_text = new QLabel(really_exit_dialog_);
02661   really_exit_text->setText("You haven't generated an application - do you really want to exit?");
02662   really_exit_layout->addWidget(really_exit_text);
02663   QDialogButtonBox* exit_button_box = new QDialogButtonBox(QDialogButtonBox::Yes | QDialogButtonBox::No);
02664   connect(exit_button_box, SIGNAL(accepted()), really_exit_dialog_, SLOT(accept()));
02665   connect(exit_button_box, SIGNAL(rejected()), really_exit_dialog_, SLOT(reject()));
02666   really_exit_layout->addWidget(exit_button_box);
02667   really_exit_dialog_->setLayout(really_exit_layout);
02668 
02669   successful_creation_dialog_ = new QDialog(this);
02670   QVBoxLayout* successful_creation_layout = new QVBoxLayout(successful_creation_dialog_);
02671   QLabel* successful_creation_text = new QLabel(successful_creation_dialog_);
02672   successful_creation_text->setText("Your application was successfully created.");
02673   successful_creation_layout->addWidget(successful_creation_text);
02674   QPushButton* exit_now_button = new QPushButton(tr("&Exit Wizard"));
02675   QPushButton* stay_in_application_button = new QPushButton(tr("&Stay In Wizard"));
02676   QDialogButtonBox* success_button_box = new QDialogButtonBox();
02677   successful_creation_layout->addWidget(success_button_box);
02678   success_button_box->addButton(stay_in_application_button, QDialogButtonBox::RejectRole);
02679   success_button_box->addButton(exit_now_button, QDialogButtonBox::AcceptRole);
02680   connect(success_button_box, SIGNAL(rejected()), successful_creation_dialog_, SLOT(reject()));
02681   connect(success_button_box, SIGNAL(accepted()), successful_creation_dialog_, SLOT(accept()));
02682   connect(success_button_box, SIGNAL(accepted()), parent, SLOT(accept()));
02683   successful_creation_dialog_->setLayout(successful_creation_layout);
02684 
02685   connect(generateButton, SIGNAL(clicked()), parent, SLOT(writeFiles()));
02686   //addPage(output_files_page_);
02687   setLayout(layout);
02688 }
02689 
02690 PlanningDescriptionConfigurationWizard* pdcw;
02691 
02692 bool inited = false;
02693 
02694 void spin_function()
02695 {
02696   ros::WallRate r(100.0);
02697   unsigned int counter = 0;
02698   while(ros::ok())
02699   {
02700     if(inited)
02701     {
02702       pdcw->sendTransforms();
02703       if(counter % CONTROL_SPEED == 0)
02704       {
02705         counter = 1;
02706         pdcw->sendMarkers();
02707       }
02708       else
02709       {
02710         counter++;
02711       }
02712     }
02713     r.sleep();
02714     ros::spinOnce();
02715   }
02716 }
02717 
02718 void quit(int sig)
02719 {
02720   if(pdcw != NULL)
02721   {
02722     delete pdcw;
02723   }
02724   exit(0);
02725 }
02726 
02727 int main(int argc, char** argv)
02728 {
02729 
02730   QApplication qtApp(argc, argv);
02731 
02732   srand(time(NULL));
02733   ros::init(argc, argv, "planning_description_configuration_wizard", ros::init_options::NoSigintHandler);
02734 
02735   if(argc < 3)
02736   {
02737     ROS_INFO_STREAM("Must specify a package and relative urdf file");
02738     exit(0);
02739   }
02740 
02741   string urdf_package = argv[1];
02742   string urdf_path = argv[2];
02743   pdcw = new PlanningDescriptionConfigurationWizard(urdf_package, urdf_path, NULL);
02744   pdcw->setUpdatesEnabled(true);
02745 
02746   qtApp.setActiveWindow(pdcw);
02747 
02748   pdcw->show();
02749   if(!pdcw->isInited())
02750   {
02751     ROS_WARN_STREAM("Can't init. Exiting");
02752     exit(0);
02753   }
02754 
02755   inited = true;
02756 
02757   boost::thread spin_thread(boost::bind(&spin_function));
02758   return qtApp.exec();
02759 }
02760 


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43