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 #include <moveit/collision_detection/world.h>
00038 #include <console_bridge/console.h>
00039
00040 collision_detection::World::World()
00041 { }
00042
00043 collision_detection::World::World(const World &other)
00044 {
00045 objects_ = other.objects_;
00046 }
00047
00048 collision_detection::World::~World()
00049 {
00050 while (!observers_.empty())
00051 removeObserver(observers_.front());
00052 }
00053
00054 inline void collision_detection::World::addToObjectInternal(const ObjectPtr &obj,
00055 const shapes::ShapeConstPtr &shape,
00056 const Eigen::Affine3d &pose)
00057 {
00058 obj->shapes_.push_back(shape);
00059 obj->shape_poses_.push_back(pose);
00060 }
00061
00062 void collision_detection::World::addToObject(const std::string &id,
00063 const std::vector<shapes::ShapeConstPtr> &shapes,
00064 const EigenSTL::vector_Affine3d &poses)
00065 {
00066 if (shapes.size() != poses.size())
00067 {
00068 logError("Number of shapes and number of poses do not match. Not adding this object to collision world.");
00069 return;
00070 }
00071
00072 if (shapes.empty())
00073 return;
00074
00075 int action = ADD_SHAPE;
00076
00077 ObjectPtr& obj = objects_[id];
00078 if (!obj) {
00079 obj.reset(new Object(id));
00080 action |= CREATE;
00081 }
00082
00083 ensureUnique(obj);
00084
00085 for (std::size_t i = 0 ; i < shapes.size() ; ++i)
00086 addToObjectInternal(obj, shapes[i], poses[i]);
00087
00088 notify(obj, Action(action));
00089 }
00090
00091 void collision_detection::World::addToObject(const std::string &id,
00092 const shapes::ShapeConstPtr &shape,
00093 const Eigen::Affine3d &pose)
00094 {
00095 int action = ADD_SHAPE;
00096
00097 ObjectPtr& obj = objects_[id];
00098 if (!obj) {
00099 obj.reset(new Object(id));
00100 action |= CREATE;
00101 }
00102
00103 ensureUnique(obj);
00104 addToObjectInternal(obj, shape, pose);
00105
00106 notify(obj, Action(action));
00107 }
00108
00109 std::vector<std::string> collision_detection::World::getObjectIds() const
00110 {
00111 std::vector<std::string> id;
00112 for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
00113 id.push_back(it->first);
00114 return id;
00115 }
00116
00117 collision_detection::World::ObjectConstPtr collision_detection::World::getObject(const std::string &id) const
00118 {
00119 std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(id);
00120 if (it == objects_.end())
00121 return ObjectConstPtr();
00122 else
00123 return it->second;
00124 }
00125
00126 void collision_detection::World::ensureUnique(ObjectPtr &obj)
00127 {
00128 if (obj && !obj.unique())
00129 obj.reset(new Object(*obj));
00130 }
00131
00132 bool collision_detection::World::hasObject(const std::string &id) const
00133 {
00134 return objects_.find(id) != objects_.end();
00135 }
00136
00137 bool collision_detection::World::moveShapeInObject(const std::string &id,
00138 const shapes::ShapeConstPtr &shape,
00139 const Eigen::Affine3d &pose)
00140 {
00141 std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00142 if (it != objects_.end())
00143 {
00144 unsigned int n = it->second->shapes_.size();
00145 for (unsigned int i = 0 ; i < n ; ++i)
00146 if (it->second->shapes_[i] == shape)
00147 {
00148 ensureUnique(it->second);
00149 it->second->shape_poses_[i] = pose;
00150
00151 notify(it->second, MOVE_SHAPE);
00152 return true;
00153 }
00154 }
00155 return false;
00156 }
00157
00158 bool collision_detection::World::removeShapeFromObject(const std::string &id,
00159 const shapes::ShapeConstPtr &shape)
00160 {
00161 std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00162 if (it != objects_.end())
00163 {
00164 unsigned int n = it->second->shapes_.size();
00165 for (unsigned int i = 0 ; i < n ; ++i)
00166 if (it->second->shapes_[i] == shape)
00167 {
00168 ensureUnique(it->second);
00169 it->second->shapes_.erase(it->second->shapes_.begin() + i);
00170 it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
00171
00172 if (it->second->shapes_.empty())
00173 {
00174 notify(it->second, DESTROY);
00175 objects_.erase(it);
00176 }
00177 else
00178 {
00179 notify(it->second, REMOVE_SHAPE);
00180 }
00181 return true;
00182 }
00183 }
00184 return false;
00185 }
00186
00187 bool collision_detection::World::removeObject(const std::string &id)
00188 {
00189 std::map<std::string, ObjectPtr>::iterator it = objects_.find(id);
00190 if (it != objects_.end())
00191 {
00192 notify(it->second, DESTROY);
00193 objects_.erase(it);
00194 return true;
00195 }
00196 return false;
00197 }
00198
00199 void collision_detection::World::clearObjects()
00200 {
00201 notifyAll(DESTROY);
00202 objects_.clear();
00203 }
00204
00205 collision_detection::World::ObserverHandle collision_detection::World::addObserver(const ObserverCallbackFn &callback)
00206 {
00207 Observer *o = new Observer(callback);
00208 observers_.push_back(o);
00209 return ObserverHandle(o);
00210 }
00211
00212 void collision_detection::World::removeObserver(ObserverHandle observer_handle)
00213 {
00214 for (std::vector<Observer*>::iterator obs = observers_.begin() ; obs != observers_.end() ; ++obs)
00215 {
00216 if (*obs == observer_handle.observer_)
00217 {
00218 delete *obs;
00219 observers_.erase(obs);
00220 return;
00221 }
00222 }
00223 }
00224
00225 void collision_detection::World::notifyAll(Action action)
00226 {
00227 for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin() ; it != objects_.end() ; ++it)
00228 notify(it->second, action);
00229 }
00230
00231 void collision_detection::World::notify(const ObjectConstPtr& obj, Action action)
00232 {
00233 for (std::vector<Observer*>::const_iterator obs = observers_.begin() ; obs != observers_.end() ; ++obs)
00234 (*obs)->callback_(obj, action);
00235 }
00236
00237 void collision_detection::World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
00238 {
00239 for (std::vector<Observer*>::const_iterator obs = observers_.begin() ; obs != observers_.end() ; ++obs)
00240 {
00241 if (*obs == observer_handle.observer_)
00242 {
00243
00244 for (std::map<std::string, ObjectPtr>::const_iterator obj = objects_.begin() ; obj != objects_.end() ; ++obj)
00245 (*obs)->callback_(obj->second, action);
00246 break;
00247 }
00248 }
00249 }