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
00082 std::vector<shapes::ShapeConstPtr> shapes_;
00083
00085 EigenSTL::vector_Affine3d shape_poses_;
00086 };
00087
00088 typedef boost::shared_ptr<Object> ObjectPtr;
00089 typedef boost::shared_ptr<Object> ObjectConstPtr;
00090
00092 std::vector<std::string> getObjectIds() const;
00093
00095 ObjectConstPtr getObject(const std::string &id) const;
00096
00098 typedef std::map<std::string, ObjectConstPtr>::const_iterator const_iterator;
00100 const_iterator begin() const
00101 {
00102 return objects_.begin();
00103 }
00105 const_iterator end() const
00106 {
00107 return objects_.end();
00108 }
00110 std::size_t size() const
00111 {
00112 return objects_.size();
00113 }
00115 const_iterator find(const std::string& id) const
00116 {
00117 return objects_.find(id);
00118 }
00119
00120
00122 bool hasObject(const std::string &id) const;
00123
00129 void addToObject(const std::string &id,
00130 const std::vector<shapes::ShapeConstPtr> &shapes,
00131 const EigenSTL::vector_Affine3d &poses);
00132
00137 void addToObject(const std::string &id,
00138 const shapes::ShapeConstPtr &shape,
00139 const Eigen::Affine3d &pose);
00140
00143 bool moveShapeInObject(const std::string &id,
00144 const shapes::ShapeConstPtr &shape,
00145 const Eigen::Affine3d &pose);
00146
00153 bool removeShapeFromObject(const std::string &id,
00154 const shapes::ShapeConstPtr &shape);
00155
00160 bool removeObject(const std::string &id);
00161
00165 void clearObjects();
00166
00167 enum ActionBits
00168 {
00169 UNINITIALIZED = 0,
00170 CREATE = 1,
00171 DESTROY = 2,
00172 MOVE_SHAPE = 4,
00173 ADD_SHAPE = 8,
00174 REMOVE_SHAPE = 16,
00175 };
00176
00180 class Action
00181 {
00182 public:
00183 Action() : action_(UNINITIALIZED) {}
00184 Action(int v) : action_(v) {}
00185 operator ActionBits() const { return ActionBits(action_); }
00186 private:
00187 int action_;
00188 };
00189
00190 private:
00191 class Observer;
00192 public:
00193 class ObserverHandle
00194 {
00195 public:
00196 ObserverHandle() : observer_(NULL) {}
00197 private:
00198 ObserverHandle(const Observer* o) : observer_(o) {}
00199 const Observer *observer_;
00200 friend class World;
00201 };
00202
00203 typedef boost::function<void (const ObjectConstPtr&, Action)> ObserverCallbackFn;
00204
00209 ObserverHandle addObserver(const ObserverCallbackFn &callback);
00210
00212 void removeObserver(const ObserverHandle observer_handle);
00213
00216 void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const;
00217
00218 private:
00219
00221 void notify(const ObjectConstPtr&, Action);
00222
00224 void notifyAll(Action action);
00225
00226
00230 void ensureUnique(ObjectPtr &obj);
00231
00232
00233 virtual void addToObjectInternal(const ObjectPtr &obj,
00234 const shapes::ShapeConstPtr &shape,
00235 const Eigen::Affine3d &pose);
00236
00237
00239 std::map<std::string, ObjectPtr> objects_;
00240
00241
00242 class Observer
00243 {
00244 public:
00245 Observer(const ObserverCallbackFn &callback) :
00246 callback_(callback)
00247 {}
00248 ObserverCallbackFn callback_;
00249 };
00250 std::vector<Observer*> observers_;
00251
00252 };
00253
00254 typedef boost::shared_ptr<World> WorldPtr;
00255 typedef boost::shared_ptr<const World> WorldConstPtr;
00256
00257 }
00258
00259
00260 #endif