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_WORLD_
00038 #define MOVEIT_COLLISION_DETECTION_WORLD_
00039
00040 #include <string>
00041 #include <vector>
00042 #include <map>
00043 #include <boost/shared_ptr.hpp>
00044 #include <boost/function.hpp>
00045 #include <Eigen/Geometry>
00046 #include <eigen_stl_containers/eigen_stl_vector_container.h>
00047 #include <geometric_shapes/shapes.h>
00048
00049 namespace collision_detection
00050 {
00051
00053 class World
00054 {
00055 public:
00056
00058 World();
00059
00063 World(const World &other);
00064
00065 ~World();
00066
00067
00068
00069
00070
00072 struct Object
00073 {
00074 Object(const std::string &id) : id_(id) {}
00075
00076 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00077
00079 std::string id_;
00080
00092 std::vector<shapes::ShapeConstPtr> shapes_;
00093
00097 EigenSTL::vector_Affine3d shape_poses_;
00098 };
00099
00100 typedef boost::shared_ptr<Object> ObjectPtr;
00101 typedef boost::shared_ptr<Object> ObjectConstPtr;
00102
00104 std::vector<std::string> getObjectIds() const;
00105
00107 ObjectConstPtr getObject(const std::string &id) const;
00108
00110 typedef std::map<std::string, ObjectConstPtr>::const_iterator const_iterator;
00112 const_iterator begin() const
00113 {
00114 return objects_.begin();
00115 }
00117 const_iterator end() const
00118 {
00119 return objects_.end();
00120 }
00122 std::size_t size() const
00123 {
00124 return objects_.size();
00125 }
00127 const_iterator find(const std::string& id) const
00128 {
00129 return objects_.find(id);
00130 }
00131
00132
00134 bool hasObject(const std::string &id) const;
00135
00141 void addToObject(const std::string &id,
00142 const std::vector<shapes::ShapeConstPtr> &shapes,
00143 const EigenSTL::vector_Affine3d &poses);
00144
00149 void addToObject(const std::string &id,
00150 const shapes::ShapeConstPtr &shape,
00151 const Eigen::Affine3d &pose);
00152
00155 bool moveShapeInObject(const std::string &id,
00156 const shapes::ShapeConstPtr &shape,
00157 const Eigen::Affine3d &pose);
00158
00165 bool removeShapeFromObject(const std::string &id,
00166 const shapes::ShapeConstPtr &shape);
00167
00172 bool removeObject(const std::string &id);
00173
00177 void clearObjects();
00178
00179 enum ActionBits
00180 {
00181 UNINITIALIZED = 0,
00182 CREATE = 1,
00183 DESTROY = 2,
00184 MOVE_SHAPE = 4,
00185 ADD_SHAPE = 8,
00186 REMOVE_SHAPE = 16,
00187 };
00188
00192 class Action
00193 {
00194 public:
00195 Action() : action_(UNINITIALIZED) {}
00196 Action(int v) : action_(v) {}
00197 operator ActionBits() const { return ActionBits(action_); }
00198 private:
00199 int action_;
00200 };
00201
00202 private:
00203 class Observer;
00204 public:
00205 class ObserverHandle
00206 {
00207 public:
00208 ObserverHandle() : observer_(NULL) {}
00209 private:
00210 ObserverHandle(const Observer* o) : observer_(o) {}
00211 const Observer *observer_;
00212 friend class World;
00213 };
00214
00215 typedef boost::function<void (const ObjectConstPtr&, Action)> ObserverCallbackFn;
00216
00221 ObserverHandle addObserver(const ObserverCallbackFn &callback);
00222
00224 void removeObserver(const ObserverHandle observer_handle);
00225
00228 void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const;
00229
00230 private:
00231
00233 void notify(const ObjectConstPtr&, Action);
00234
00236 void notifyAll(Action action);
00237
00238
00242 void ensureUnique(ObjectPtr &obj);
00243
00244
00245 virtual void addToObjectInternal(const ObjectPtr &obj,
00246 const shapes::ShapeConstPtr &shape,
00247 const Eigen::Affine3d &pose);
00248
00249
00251 std::map<std::string, ObjectPtr> objects_;
00252
00253
00254 class Observer
00255 {
00256 public:
00257 Observer(const ObserverCallbackFn &callback) :
00258 callback_(callback)
00259 {}
00260 ObserverCallbackFn callback_;
00261 };
00262 std::vector<Observer*> observers_;
00263
00264 };
00265
00266 typedef boost::shared_ptr<World> WorldPtr;
00267 typedef boost::shared_ptr<const World> WorldConstPtr;
00268
00269 }
00270
00271
00272 #endif