Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039
00040 #include <map>
00041 #include <string>
00042 #include <vector>
00043 #include <fstream>
00044
00045 #include <planning_environment/models/collision_models.h>
00046 #include <yaml-cpp/yaml.h>
00047
00048 namespace planning_environment
00049 {
00050
00051 class CollisionOperationsGenerator {
00052
00053 public:
00054
00055 enum DisableType {
00056 ADJACENT,
00057 ALWAYS,
00058 DEFAULT,
00059 OFTEN,
00060 OCCASIONALLY,
00061 NEVER
00062 };
00063
00064 enum SamplingSafety
00065 {
00066 VerySafe,
00067 Safe,
00068 Normal,
00069 Fast,
00070 VeryFast
00071 };
00072
00073 CollisionOperationsGenerator(CollisionModels* cm);
00074
00075 typedef std::pair<std::string, std::string> StringPair;
00076 typedef std::map<std::string, double> CollidingJointValues;
00077
00078 void generateAdjacentInCollisionPairs(std::vector<StringPair>& adjacent_in_collision_pairs);
00079
00080 void generateAlwaysInCollisionPairs(std::vector<StringPair>& always_in_collision_pairs,
00081 std::vector<CollidingJointValues>& in_collision_joint_values);
00082
00083 void generateDefaultInCollisionPairs(std::vector<StringPair>& default_in_collision_pairs,
00084 std::vector<CollidingJointValues>& in_collision_joint_values);
00085
00086 void generateOftenInCollisionPairs(std::vector<StringPair>& often_in_collision_pairs,
00087 std::vector<double>& collision_percentage,
00088 std::vector<CollidingJointValues>& in_collision_joint_values);
00089
00090 void generateOccasionallyAndNeverInCollisionPairs(std::vector<StringPair>& occasionally_in_collision_pairs,
00091 std::vector<StringPair>& never_in_collision_pairs,
00092 std::vector<double>& collision_percentage,
00093 std::vector<CollidingJointValues>& in_collision_joint_values);
00094
00095 void enablePairCollisionChecking(const StringPair& pair);
00096 void disablePairCollisionChecking(const StringPair& pair);
00097 void disablePairCollisionChecking(const std::vector<StringPair>& pair_vec);
00098
00099 void enableAllCollisions();
00100 void outputYamlStringOfSavedResults(YAML::Emitter& outy, const std::map<DisableType, std::vector<StringPair> >& disable_types);
00101
00102
00103 void performanceTestSavedResults(std::map<CollisionOperationsGenerator::DisableType, std::vector<CollisionOperationsGenerator::StringPair> >& disable_types);
00104
00105 void generateSamplingStructures(const std::map<std::string, bool>& add_map);
00106
00107 inline void setSafety(SamplingSafety safety)
00108 {
00109 switch(safety)
00110 {
00111 case VerySafe:
00112 establish_always_num_ = 5000;
00113 establish_often_num_ = 15000;
00114 establish_often_percentage_ = 0.5;
00115 establish_occasional_num_ = 1000000;
00116 performance_testing_num_ = 5000;
00117 break;
00118
00119 case Safe:
00120 establish_always_num_ = 10000;
00121 establish_often_num_ = 5000;
00122 establish_often_percentage_ = 0.5;
00123 establish_occasional_num_ = 100000;
00124 performance_testing_num_ = 1000;
00125 break;
00126
00127 case Normal:
00128 establish_always_num_ = 1000;
00129 establish_often_num_ = 1000;
00130 establish_often_percentage_ = 0.5;
00131 establish_occasional_num_ = 20000;
00132 performance_testing_num_ = 1000;
00133 break;
00134
00135 case Fast:
00136 establish_always_num_ = 100;
00137 establish_often_num_ = 500;
00138 establish_often_percentage_ = 0.5;
00139 establish_occasional_num_ = 1000;
00140 performance_testing_num_ = 100;
00141 break;
00142
00143 case VeryFast:
00144 establish_always_num_ = 100;
00145 establish_often_num_ = 100;
00146 establish_often_percentage_ = 0.5;
00147 establish_occasional_num_ = 500;
00148 performance_testing_num_ = 10;
00149 break;
00150 }
00151 }
00152
00153 unsigned int establish_always_num_;
00154 unsigned int establish_often_num_;
00155 double establish_often_percentage_;
00156 unsigned int establish_occasional_num_;
00157 unsigned int performance_testing_num_;
00158
00159 protected:
00160
00161 void accumulateAdjacentLinksRecursive(const planning_models::KinematicModel::LinkModel* parent,
00162 std::vector<StringPair>& adjacencies);
00163
00164 void sampleAndCountCollisions(unsigned int num);
00165
00166 void buildOutputStructures(unsigned int num, double low_value, double high_value,
00167 std::vector<StringPair>& meets_threshold_collision,
00168 std::vector<double>& collision_percentages,
00169 std::vector<CollidingJointValues>& joint_values,
00170 std::map<std::string, std::map<std::string, double> >& percentage_num);
00171
00172 void resetCountingMap();
00173
00174 void generateRandomState(planning_models::KinematicState& state);
00175
00176 std::map<std::string, std::pair<double, double> > joint_bounds_map_;
00177 std::map<std::string, std::map<std::string, unsigned int> > collision_count_map_;
00178 std::map<std::string, std::map<std::string, CollidingJointValues> > collision_joint_values_;
00179
00180 planning_environment::CollisionModels* cm_;
00181
00182 };
00183 }