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 #ifndef MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_
00038 #define MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_
00039
00040 #include <moveit/collision_detection/collision_common.h>
00041 #include <moveit_msgs/AllowedCollisionMatrix.h>
00042 #include <boost/function.hpp>
00043 #include <iostream>
00044 #include <vector>
00045 #include <string>
00046 #include <map>
00047
00048 namespace collision_detection
00049 {
00050
00052 namespace AllowedCollision
00053 {
00054 enum Type
00055 {
00058 NEVER,
00059
00062 ALWAYS,
00063
00067 CONDITIONAL
00068 };
00069 }
00070
00072 typedef boost::function<bool(collision_detection::Contact&)> DecideContactFn;
00073
00077 class AllowedCollisionMatrix
00078 {
00079 public:
00080
00081 AllowedCollisionMatrix();
00082
00087 AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed = false);
00088
00090 AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix &msg);
00091
00093 AllowedCollisionMatrix(const AllowedCollisionMatrix& acm);
00094
00100 bool getEntry(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision_type) const;
00101
00107 bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
00108
00111 bool hasEntry(const std::string& name) const;
00112
00116 bool hasEntry(const std::string& name1, const std::string &name2) const;
00117
00121 void removeEntry(const std::string& name1, const std::string &name2);
00122
00125 void removeEntry(const std::string& name);
00126
00133 void setEntry(const std::string& name1, const std::string& name2, bool allowed);
00134
00139 void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn &fn);
00140
00147 void setEntry(const std::string& name, bool allowed);
00148
00156 void setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed);
00157
00164 void setEntry(const std::vector<std::string> &names1, const std::vector<std::string> &names2, bool allowed);
00165
00170 void setEntry(bool allowed);
00171
00173 void getAllEntryNames(std::vector<std::string>& names) const;
00174
00176 void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const;
00177
00179 void clear();
00180
00182 std::size_t getSize() const
00183 {
00184 return entries_.size();
00185 }
00186
00193 void setDefaultEntry(const std::string &name, bool allowed);
00194
00200 void setDefaultEntry(const std::string &name, const DecideContactFn &fn);
00201
00206 bool getDefaultEntry(const std::string &name, AllowedCollision::Type& allowed_collision) const;
00207
00212 bool getDefaultEntry(const std::string &name, DecideContactFn &fn) const;
00213
00222 bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn &fn) const;
00223
00231 bool getAllowedCollision(const std::string& name1, const std::string& name2, AllowedCollision::Type& allowed_collision) const;
00232
00234 void print(std::ostream& out) const;
00235
00236 private:
00237
00238 std::map<std::string, std::map<std::string, AllowedCollision::Type> > entries_;
00239 std::map<std::string, std::map<std::string, DecideContactFn> > allowed_contacts_;
00240
00241 std::map<std::string, AllowedCollision::Type> default_entries_;
00242 std::map<std::string, DecideContactFn> default_allowed_contacts_;
00243
00244 };
00245
00246 typedef boost::shared_ptr<AllowedCollisionMatrix> AllowedCollisionMatrixPtr;
00247 typedef boost::shared_ptr<const AllowedCollisionMatrix> AllowedCollisionMatrixConstPtr;
00248 }
00249
00250 #endif