planning_environment: planning_environment: collision_operations_generator

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author E. Gil Jones
00036  *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <map>
00041 #include <string>
00042 #include <vector>
00043 #include <fstream>
00044 
00045 #include <planning_models/kinematic_model.h>
00046 #include <planning_models/kinematic_state.h>
00047 #include <collision_space/environmentODE.h>
00048 #include <yaml-cpp/yaml.h>
00049 
00050 double gen_rand(double min, double max)
00051 {
00052   int rand_num = rand()%100+1;
00053   double result = min + (double)((max-min)*rand_num)/101.0;
00054   return result;
00055 }
00056 
00057 static const unsigned int ESTABLISH_ALWAYS_NUM = 100;
00058 static const unsigned int ESTABLISH_OFTEN_NUM = 500;
00059 static const double ESTABLISH_OFTEN_PERCENTAGE = .5;
00060 static const unsigned int PERFORMANCE_TESTING_NUM = 500;
00061 
00062 class CollisionOperationsGenerator {
00063 
00064 public:
00065 
00066   CollisionOperationsGenerator(const std::string& full_path_name) {
00067     
00068     ok_ = false;
00069 
00070     bool urdf_ok = urdf_model_.initFile(full_path_name);
00071 
00072     if(!urdf_ok) {
00073       ROS_WARN_STREAM("Urdf file " << full_path_name << " not ok");
00074       return;
00075     }
00076     std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00077     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00078     const urdf::Link *root = urdf_model_.getRoot().get();
00079 
00080     //now this should work with an non-identity transform
00081     planning_models::KinematicModel::MultiDofConfig config("world_joint");
00082     config.type = "Floating";
00083     config.parent_frame_id = root->name;
00084     config.child_frame_id = root->name;
00085     multi_dof_configs.push_back(config);
00086     
00087     kmodel_ = new planning_models::KinematicModel(urdf_model_,gcs, multi_dof_configs);    
00088     if(kmodel_->getRoot() == NULL) {
00089       ROS_INFO_STREAM("Kinematic root is NULL");
00090       return;
00091     }
00092 
00093     ode_collision_model_ = new collision_space::EnvironmentModelODE();
00094 
00095     const std::vector<planning_models::KinematicModel::LinkModel*>& coll_links = kmodel_->getLinkModelsWithCollisionGeometry();
00096   
00097     std::vector<std::string> coll_names;
00098     for(unsigned int i = 0; i < coll_links.size(); i++) {
00099       coll_names.push_back(coll_links[i]->getName());
00100     }
00101     collision_space::EnvironmentModel::AllowedCollisionMatrix default_collision_matrix(coll_names,false);
00102     std::map<std::string, double> default_link_padding_map;
00103     ode_collision_model_->setRobotModel(kmodel_, default_collision_matrix, 
00104                                         default_link_padding_map, 0.0, 1.0);
00105     generateSamplingStructures();
00106     ok_ = true;
00107   }
00108 
00109   ~CollisionOperationsGenerator() {
00110     delete ode_collision_model_;
00111     delete kmodel_;
00112   }
00113 
00114   void generateSamplingStructures() {
00115     const std::vector<planning_models::KinematicModel::JointModel*>& jmv = kmodel_->getJointModels();
00116     //assuming that 0th is world joint, which we don't want to include
00117     for(unsigned int i = 1; i < jmv.size(); i++) {
00118       const std::map<std::string, std::pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
00119       for(std::map<std::string, std::pair<double, double> >::const_iterator it = joint_bounds.begin();
00120           it != joint_bounds.end();
00121           it++) {
00122         if(joint_bounds_map_.find(it->first) != joint_bounds_map_.end()) {
00123           ROS_WARN_STREAM("Have repeat DOF names for " << it->first);
00124           continue;
00125         }
00126         if(it->second.first > it->second.second) {
00127           ROS_WARN_STREAM("Lower bound for DOF " << it->first << " is greater than upper bound");
00128         } else if(it->second.first == -DBL_MAX) {
00129           ROS_WARN_STREAM("Some non-root DOF " << it->first << " has negative inf lower bound");
00130         } 
00131         if(it->second.second == DBL_MAX) {
00132           ROS_WARN_STREAM("Some non-root DOF " << it->first << " has inf upper[ bound");
00133         } 
00134         joint_bounds_map_[it->first] = it->second;        
00135       }
00136     }
00137   }
00138 
00139   void generateOutputCollisionOperations(unsigned int num, const std::string& output_file) {
00140     sampleAndCountCollisions(ESTABLISH_ALWAYS_NUM);
00141     
00142     ROS_INFO_STREAM("Established always in collision pairs");
00143 
00144     planning_models::KinematicState state(kmodel_);
00145     collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = ode_collision_model_->getCurrentAllowedCollisionMatrix();
00146 
00147     altered_acm.changeEntry(false);
00148     for(unsigned int i = 0; i < always_in_collision_.size(); i++) {
00149       altered_acm.changeEntry(always_in_collision_[i].first,
00150                                always_in_collision_[i].second,
00151                                true);
00152     }    
00153 
00154     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00155 
00156     saved_always_in_collision_ = always_in_collision_;
00157 
00158     sampleAndCountCollisions(ESTABLISH_OFTEN_NUM);
00159     for(unsigned int i = 0; i < often_in_collision_.size(); i++) {
00160       altered_acm.changeEntry(often_in_collision_[i].first,
00161                               often_in_collision_[i].second,
00162                               true);
00163     }    
00164     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00165     saved_often_in_collision_ = often_in_collision_;
00166     often_percentage_map_ = percentage_map_;
00167     ROS_INFO_STREAM("Established often in collision pairs");
00168     
00169     sampleAndCountCollisions(num);
00170     emitYamlOutputStructures(output_file);
00171     printOutputStructures(output_file);
00172   }
00173 
00174   void sampleAndCountCollisions(unsigned int num) {
00175     resetCountingMap();
00176 
00177     planning_models::KinematicState state(kmodel_);
00178 
00179     ros::WallTime n1 = ros::WallTime::now();
00180     for(unsigned int i = 0; i < num; i++) {
00181       generateRandomState(state);
00182       ode_collision_model_->updateRobotModel(&state);
00183       std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
00184       std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
00185       ode_collision_model_->getAllCollisionContacts(allowed_contacts,
00186                                                     coll_space_contacts,
00187                                                     1);
00188       if(i != 0 && i % 1000 == 0) {
00189         ROS_INFO_STREAM("Num " << i << " getting all contacts takes " << (ros::WallTime::now()-n1).toSec());
00190         n1 = ros::WallTime::now();
00191       }
00192       for(unsigned int i = 0; i < coll_space_contacts.size(); i++) {
00193         collision_space::EnvironmentModel::Contact& contact = coll_space_contacts[i];
00194         if(collision_count_map_.find(contact.body_name_1) == collision_count_map_.end()) {
00195           ROS_WARN_STREAM("Problem - have no count for collision body " << contact.body_name_1);
00196         }
00197         if(collision_count_map_.find(contact.body_name_2) == collision_count_map_.end()) {
00198           ROS_WARN_STREAM("Problem - have no count for collision body " << contact.body_name_2);
00199         }
00200         collision_count_map_[contact.body_name_1][contact.body_name_2]++;
00201         collision_count_map_[contact.body_name_2][contact.body_name_1]++;
00202       }
00203     }
00204     buildOutputStructures(num);
00205   }
00206 
00207   void generateAndCompareOutputStructures(unsigned int num) {
00208     sampleAndCountCollisions(ESTABLISH_ALWAYS_NUM);
00209     
00210     ROS_INFO_STREAM("Established always in collision pairs");
00211 
00212     planning_models::KinematicState state(kmodel_);
00213     collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = ode_collision_model_->getCurrentAllowedCollisionMatrix();
00214 
00215     altered_acm.changeEntry(false);
00216     for(unsigned int i = 0; i < always_in_collision_.size(); i++) {
00217       altered_acm.changeEntry(always_in_collision_[i].first,
00218                               always_in_collision_[i].second,
00219                               true);
00220     }    
00221     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00222     saved_always_in_collision_ = always_in_collision_;
00223 
00224     sampleAndCountCollisions(ESTABLISH_OFTEN_NUM);
00225     for(unsigned int i = 0; i < often_in_collision_.size(); i++) {
00226       altered_acm.changeEntry(often_in_collision_[i].first,
00227                               often_in_collision_[i].second,
00228                               true);
00229     }    
00230     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00231     saved_often_in_collision_ = often_in_collision_;
00232     ROS_INFO_STREAM("Established often in collision pairs");
00233 
00234     std::map<std::string, std::map<std::string, double> > first_percentage_map;
00235     sampleAndCountCollisions(num);
00236     first_percentage_map = percentage_map_;
00237     
00238     ROS_INFO_STREAM("Done with first");
00239 
00240     sampleAndCountCollisions(num);
00241 
00242     ROS_INFO_STREAM("Done with second");
00243 
00244     for(std::map<std::string, std::map<std::string, double> >::iterator it = first_percentage_map.begin();
00245         it != first_percentage_map.end();
00246         it++) {
00247       for(std::map<std::string, double>::iterator it2 = it->second.begin();
00248           it2 != it->second.end();
00249           it2++) {
00250         bool first_all = (it2->second == num);
00251         bool second_all = (percentage_map_[it->first][it2->first] == num);
00252         
00253         if(first_all != second_all) {
00254           ROS_INFO_STREAM("Links " << it->first << " and " << it2->first << " different all");
00255         }
00256         
00257         bool first_never = (it2->second == 0);
00258         bool second_never = (percentage_map_[it->first][it2->first] == 0);
00259         
00260         if(first_never != second_never) {
00261           ROS_INFO_STREAM("Links " << it->first << " and " << it2->first << " different never " << it2->second << " " << percentage_map_[it->first][it2->first]);
00262         }
00263       }
00264     }
00265   }
00266 
00267   void buildOutputStructures(unsigned int num) {
00268     always_in_collision_.clear();
00269     never_in_collision_.clear();    
00270     often_in_collision_.clear();
00271     occasionally_in_collision_.clear();
00272     percentage_map_.clear();
00273     for(std::map<std::string, std::map<std::string, unsigned int> >::iterator it = collision_count_map_.begin();
00274         it != collision_count_map_.end();
00275         it++) {
00276       for(std::map<std::string, unsigned int>::iterator it2 = it->second.begin();
00277           it2 != it->second.end();
00278           it2++) {
00279         if(it->first == it2->first) {
00280           continue;
00281         }
00282         bool already_in_lists = false;
00283         //if we've already registered this pair continue
00284         if(percentage_map_.find(it->first) != percentage_map_.end()) {
00285           if(percentage_map_.find(it->first)->second.find(it2->first) != 
00286              percentage_map_.find(it->first)->second.end()) {
00287             already_in_lists = true;
00288           }
00289         }
00290         if(percentage_map_.find(it2->first) != percentage_map_.end()) {
00291           if(percentage_map_.find(it2->first)->second.find(it->first) != 
00292              percentage_map_.find(it2->first)->second.end()) {
00293             already_in_lists = true;
00294           }
00295         }
00296         if(it2->second == num) {
00297           if(!already_in_lists) {
00298             always_in_collision_.push_back(std::pair<std::string, std::string>(it->first, it2->first));
00299           }
00300           percentage_map_[it->first][it2->first] = 1.0;
00301           percentage_map_[it2->first][it->first] = 1.0;
00302         } else if(it2->second == 0) {
00303           if(!already_in_lists) {
00304             never_in_collision_.push_back(std::pair<std::string, std::string>(it->first, it2->first));
00305           }
00306           percentage_map_[it->first][it2->first] = 0.0;
00307           percentage_map_[it2->first][it->first] = 0.0;          
00308         } else {
00309           double per = (it2->second*1.0)/(num*1.0);
00310           if(!already_in_lists) {
00311             if(per > ESTABLISH_OFTEN_PERCENTAGE) {
00312               often_in_collision_.push_back(std::pair<std::string, std::string>(it->first, it2->first));
00313             } else {
00314               occasionally_in_collision_.push_back(std::pair<std::string, std::string>(it->first, it2->first));
00315             }
00316           }
00317           percentage_map_[it->first][it2->first] = per;
00318           percentage_map_[it2->first][it->first] = per;                    
00319         }
00320       }
00321     }
00322   }
00323 
00324   void printOutputStructures(const std::string& filename) {
00325     std::ofstream outfile((filename+".txt").c_str());
00326 
00327     outfile << "Always in collision pairs: " << std::endl;
00328     for(unsigned int i = 0; i < saved_always_in_collision_.size(); i++) {
00329       outfile << saved_always_in_collision_[i].first << " " << saved_always_in_collision_[i].second << std::endl;
00330     }
00331     outfile << std::endl;
00332 
00333     outfile << "Often in collision pairs: " << std::endl;
00334     for(unsigned int i = 0; i < saved_often_in_collision_.size(); i++) {
00335       outfile << saved_often_in_collision_[i].first << " " << saved_often_in_collision_[i].second;
00336       outfile << " " << often_percentage_map_[saved_often_in_collision_[i].first][saved_often_in_collision_[i].second] << std::endl;
00337     }
00338     outfile << std::endl;
00339 
00340     outfile << "Never in collision pairs: " << std::endl;
00341     for(unsigned int i = 0; i < never_in_collision_.size(); i++) {
00342       outfile << never_in_collision_[i].first << " " << never_in_collision_[i].second << std::endl;
00343     }
00344     outfile << std::endl;
00345 
00346     outfile << "Occasionally in collision pairs: " << std::endl;
00347     for(unsigned int i = 0; i < occasionally_in_collision_.size(); i++) {
00348       outfile << occasionally_in_collision_[i].first << " " << occasionally_in_collision_[i].second;
00349       outfile << " " << percentage_map_[occasionally_in_collision_[i].first][occasionally_in_collision_[i].second] << std::endl;
00350     }
00351   }
00352 
00353   void emitYamlOutputStructures(const std::string& filename) {
00354     std::ofstream outfile((filename+".yaml").c_str());
00355 
00356     YAML::Emitter outy;
00357     outy << YAML::BeginMap;
00358     outy << YAML::Key << "default_collision_operations";
00359     outy << YAML::Value << YAML::BeginSeq; 
00360     for(unsigned int i = 0; i < saved_often_in_collision_.size(); i++) {
00361       outy << YAML::BeginMap; 
00362       outy << YAML::Key << "object1" << YAML::Value << saved_often_in_collision_[i].first;
00363       outy << YAML::Key << "object2" << YAML::Value << saved_often_in_collision_[i].second;
00364       outy << YAML::Key << "operation" << YAML::Value << "disable";
00365       outy << YAML::Comment("Often in collision");
00366       outy << YAML::EndMap;
00367     }
00368     for(unsigned int i = 0; i < saved_always_in_collision_.size(); i++) {
00369       outy << YAML::BeginMap; 
00370       outy << YAML::Key << "object1" << YAML::Value << saved_always_in_collision_[i].first;
00371       outy << YAML::Key << "object2" << YAML::Value << saved_always_in_collision_[i].second;
00372       outy << YAML::Key << "operation" << YAML::Value << "disable";
00373       outy << YAML::Comment("Always in collision");
00374       outy << YAML::EndMap;
00375     }
00376     for(unsigned int i = 0; i < never_in_collision_.size(); i++) {
00377       outy << YAML::BeginMap; 
00378       outy << YAML::Key << "object1" << YAML::Value << never_in_collision_[i].first;
00379       outy << YAML::Key << "object2" << YAML::Value << never_in_collision_[i].second;
00380       outy << YAML::Key << "operation" << YAML::Value << "disable";
00381       outy << YAML::Comment("Never in collision");
00382       outy << YAML::EndMap;
00383     }
00384     outy << YAML::EndMap;
00385     outfile << outy.c_str();
00386   }
00387 
00388   void performanceTestFromOutputStructures() {
00389     unsigned int num = PERFORMANCE_TESTING_NUM;
00390     planning_models::KinematicState state(kmodel_);
00391     collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = ode_collision_model_->getCurrentAllowedCollisionMatrix();
00392 
00393     altered_acm.changeEntry(false);
00394     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00395     ros::WallTime n1 = ros::WallTime::now();
00396     for(unsigned int i = 0; i < num; i++) {
00397       generateRandomState(state);
00398       ode_collision_model_->updateRobotModel(&state);
00399       std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
00400       std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
00401       ode_collision_model_->getAllCollisionContacts(allowed_contacts,
00402                                                     coll_space_contacts,
00403                                                     1);
00404     }
00405     ROS_INFO_STREAM("All enabled collision check average " <<
00406                     (ros::WallTime::now()-n1).toSec()/(num*1.0));
00407 
00408     for(unsigned int i = 0; i < saved_always_in_collision_.size(); i++) {
00409       altered_acm.changeEntry(saved_always_in_collision_[i].first,
00410                                saved_always_in_collision_[i].second,
00411                                true);
00412     }    
00413 
00414     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00415     n1 = ros::WallTime::now();
00416     for(unsigned int i = 0; i < num; i++) {
00417       generateRandomState(state);
00418       ode_collision_model_->updateRobotModel(&state);
00419       std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
00420       std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
00421       ode_collision_model_->getAllCollisionContacts(allowed_contacts,
00422                                                     coll_space_contacts,
00423                                                     1);
00424     }
00425     ROS_INFO_STREAM("Always disabled collision check average " <<
00426                     (ros::WallTime::now()-n1).toSec()/(num*1.0));
00427 
00428     for(unsigned int i = 0; i < saved_often_in_collision_.size(); i++) {
00429       altered_acm.changeEntry(saved_often_in_collision_[i].first,
00430                                saved_often_in_collision_[i].second,
00431                                true);
00432     }    
00433 
00434     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00435     n1 = ros::WallTime::now();
00436     for(unsigned int i = 0; i < num; i++) {
00437       generateRandomState(state);
00438       ode_collision_model_->updateRobotModel(&state);
00439       std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
00440       std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
00441       ode_collision_model_->getAllCollisionContacts(allowed_contacts,
00442                                                     coll_space_contacts,
00443                                                     1);
00444     }
00445     ROS_INFO_STREAM("Often disabled collision check average " <<
00446                     (ros::WallTime::now()-n1).toSec()/(num*1.0));
00447 
00448     for(unsigned int i = 0; i < never_in_collision_.size(); i++) {
00449       altered_acm.changeEntry(never_in_collision_[i].first,
00450                                never_in_collision_[i].second,
00451                                true);
00452     }    
00453     ode_collision_model_->setAlteredCollisionMatrix(altered_acm);
00454     n1 = ros::WallTime::now();
00455     for(unsigned int i = 0; i < num; i++) {
00456       generateRandomState(state);
00457       ode_collision_model_->updateRobotModel(&state);
00458       std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts;
00459       std::vector<collision_space::EnvironmentModel::Contact> coll_space_contacts;
00460       ode_collision_model_->getAllCollisionContacts(allowed_contacts,
00461                                                     coll_space_contacts,
00462                                                     1);
00463     }
00464     ROS_INFO_STREAM("Never disabled collision check average " <<
00465                     (ros::WallTime::now()-n1).toSec()/(num*1.0));
00466     
00467   }
00468 
00469 
00470   bool isOk() const {
00471     return ok_;
00472   }
00473 
00474 protected:
00475 
00476   void resetCountingMap() {
00477     const std::vector<planning_models::KinematicModel::LinkModel*>& lmv = kmodel_->getLinkModelsWithCollisionGeometry();
00478 
00479     collision_count_map_.clear();
00480 
00481     //assuming that 0th is world joint, which we don't want to include
00482     std::map<std::string, unsigned int> all_link_zero;
00483     for(unsigned int i = 0; i < lmv.size(); i++) {
00484       all_link_zero[lmv[i]->getName()] = 0;
00485     }
00486     for(unsigned int i = 0; i < lmv.size(); i++) {
00487       collision_count_map_[lmv[i]->getName()] = all_link_zero;
00488     }
00489   }
00490 
00491   void generateRandomState(planning_models::KinematicState& state) {
00492     std::map<std::string, double> values;
00493     for(std::map<std::string, std::pair<double, double> >::iterator it = joint_bounds_map_.begin();
00494         it != joint_bounds_map_.end();
00495         it++) {
00496       values[it->first] = gen_rand(it->second.first, it->second.second);
00497       //ROS_INFO_STREAM("Value for " << it->first << " is " << values[it->first] << " bounds " << 
00498       //                it->second.first << " " << it->second.second);
00499     }
00500     state.setKinematicState(values);
00501   }
00502 
00503   bool ok_;
00504 
00505   collision_space::EnvironmentModel* ode_collision_model_;
00506   planning_models::KinematicModel* kmodel_;
00507   urdf::Model urdf_model_;
00508 
00509   std::map<std::string, std::pair<double, double> > joint_bounds_map_;
00510   std::map<std::string, std::map<std::string, unsigned int> > collision_count_map_;
00511 
00512   std::vector<std::pair<std::string, std::string> > always_in_collision_;
00513   std::vector<std::pair<std::string, std::string> > saved_always_in_collision_;
00514   std::vector<std::pair<std::string, std::string> > never_in_collision_;
00515   std::vector<std::pair<std::string, std::string> > often_in_collision_;
00516   std::vector<std::pair<std::string, std::string> > saved_often_in_collision_;
00517   std::vector<std::pair<std::string, std::string> > occasionally_in_collision_;
00518   std::map<std::string, std::map<std::string, double> > percentage_map_;
00519   std::map<std::string, std::map<std::string, double> > often_percentage_map_;
00520 
00521 };
00522 
00523 static const unsigned int TIMES = 50000;
00524 
00525 int main(int argc, char** argv) {
00526 
00527   ros::init(argc, argv, "collision_operations_generator");
00528 
00529   srand(time(NULL));
00530   
00531   if(argc < 2) {
00532     ROS_INFO_STREAM("Must specify a urdf file");
00533     exit(0);
00534   }
00535 
00536   std::string urdf_file = argv[1];
00537   std::string output_file;
00538   if(argc == 3) {
00539     output_file = argv[2];
00540   }
00541 
00542   CollisionOperationsGenerator cog(urdf_file);
00543 
00544   if(!cog.isOk()) {
00545     ROS_INFO_STREAM("Something wrong with urdf");
00546     exit(0);
00547   }
00548 
00549   if(argc == 3) {
00550     cog.generateOutputCollisionOperations(TIMES, output_file);
00551     cog.performanceTestFromOutputStructures();
00552   } else {
00553     cog.generateAndCompareOutputStructures(TIMES);
00554   }
00555 
00556   ros::shutdown();
00557   exit(0);
00558 }
00559
collision_operations_generator_test.cpp
