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_fcl/collision_world_fcl.h>
00038 #include <fcl/shape/geometric_shape_to_BVH_model.h>
00039 #include <fcl/traversal/traversal_node_bvhs.h>
00040 #include <fcl/traversal/traversal_node_setup.h>
00041 #include <fcl/collision_node.h>
00042
00043 collision_detection::CollisionWorldFCL::CollisionWorldFCL() :
00044 CollisionWorld()
00045 {
00046 fcl::DynamicAABBTreeCollisionManager* m = new fcl::DynamicAABBTreeCollisionManager();
00047
00048 manager_.reset(m);
00049
00050
00051 observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
00052 }
00053
00054 collision_detection::CollisionWorldFCL::CollisionWorldFCL(const WorldPtr& world) :
00055 CollisionWorld(world)
00056 {
00057 fcl::DynamicAABBTreeCollisionManager* m = new fcl::DynamicAABBTreeCollisionManager();
00058
00059 manager_.reset(m);
00060
00061
00062 observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
00063 getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
00064 }
00065
00066 collision_detection::CollisionWorldFCL::CollisionWorldFCL(const CollisionWorldFCL &other, const WorldPtr& world) :
00067 CollisionWorld(other, world)
00068 {
00069 fcl::DynamicAABBTreeCollisionManager* m = new fcl::DynamicAABBTreeCollisionManager();
00070
00071 manager_.reset(m);
00072
00073 fcl_objs_ = other.fcl_objs_;
00074 for (std::map<std::string, FCLObject>::iterator it = fcl_objs_.begin() ; it != fcl_objs_.end() ; ++it)
00075 it->second.registerTo(manager_.get());
00076
00077
00078
00079 observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
00080 }
00081
00082 collision_detection::CollisionWorldFCL::~CollisionWorldFCL()
00083 {
00084 getWorld()->removeObserver(observer_handle_);
00085 }
00086
00087 void collision_detection::CollisionWorldFCL::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
00088 {
00089 checkRobotCollisionHelper(req, res, robot, state, NULL);
00090 }
00091
00092 void collision_detection::CollisionWorldFCL::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const
00093 {
00094 checkRobotCollisionHelper(req, res, robot, state, &acm);
00095 }
00096
00097 void collision_detection::CollisionWorldFCL::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
00098 {
00099 logError("FCL continuous collision checking not yet implemented");
00100 }
00101
00102 void collision_detection::CollisionWorldFCL::checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
00103 {
00104 logError("FCL continuous collision checking not yet implemented");
00105 }
00106
00107 void collision_detection::CollisionWorldFCL::checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const
00108 {
00109 const CollisionRobotFCL &robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
00110 FCLObject fcl_obj;
00111 robot_fcl.constructFCLObject(state, fcl_obj);
00112
00113 CollisionData cd(&req, &res, acm);
00114 cd.enableGroup(robot.getRobotModel());
00115 for (std::size_t i = 0 ; !cd.done_ && i < fcl_obj.collision_objects_.size() ; ++i)
00116 manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
00117
00118 if (req.distance)
00119 res.distance = distanceRobotHelper(robot, state, acm);
00120 }
00121
00122 void collision_detection::CollisionWorldFCL::checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const
00123 {
00124 checkWorldCollisionHelper(req, res, other_world, NULL);
00125 }
00126
00127 void collision_detection::CollisionWorldFCL::checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix &acm) const
00128 {
00129 checkWorldCollisionHelper(req, res, other_world, &acm);
00130 }
00131
00132 void collision_detection::CollisionWorldFCL::checkWorldCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix *acm) const
00133 {
00134 const CollisionWorldFCL &other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(other_world);
00135 CollisionData cd(&req, &res, acm);
00136 manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback);
00137
00138 if (req.distance)
00139 res.distance = distanceWorldHelper(other_world, acm);
00140 }
00141
00142 void collision_detection::CollisionWorldFCL::constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const
00143 {
00144 for (std::size_t i = 0 ; i < obj->shapes_.size() ; ++i)
00145 {
00146 FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
00147 if (g)
00148 {
00149 fcl::CollisionObject *co = new fcl::CollisionObject(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
00150 fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(co));
00151 fcl_obj.collision_geometry_.push_back(g);
00152 }
00153 }
00154 }
00155
00156 void collision_detection::CollisionWorldFCL::updateFCLObject(const std::string &id)
00157 {
00158
00159 std::map<std::string, FCLObject>::iterator jt = fcl_objs_.find(id);
00160 if (jt != fcl_objs_.end())
00161 {
00162 jt->second.unregisterFrom(manager_.get());
00163 jt->second.clear();
00164 }
00165
00166
00167 collision_detection::World::const_iterator it = getWorld()->find(id);
00168 if (it != getWorld()->end())
00169 {
00170
00171 if (jt != fcl_objs_.end())
00172 {
00173 constructFCLObject(it->second.get(), jt->second);
00174 jt->second.registerTo(manager_.get());
00175 }
00176 else
00177 {
00178 constructFCLObject(it->second.get(), fcl_objs_[id]);
00179 fcl_objs_[id].registerTo(manager_.get());
00180 }
00181 }
00182 else
00183 {
00184 if (jt != fcl_objs_.end())
00185 fcl_objs_.erase(jt);
00186 }
00187
00188
00189 }
00190
00191 void collision_detection::CollisionWorldFCL::setWorld(const WorldPtr& world)
00192 {
00193 if (world == getWorld())
00194 return;
00195
00196
00197 getWorld()->removeObserver(observer_handle_);
00198
00199
00200 manager_->clear();
00201 fcl_objs_.clear();
00202 cleanCollisionGeometryCache();
00203
00204 CollisionWorld::setWorld(world);
00205
00206
00207 observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
00208
00209
00210 getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
00211 }
00212
00213 void collision_detection::CollisionWorldFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action)
00214 {
00215 if (action == World::DESTROY)
00216 {
00217 std::map<std::string, FCLObject>::iterator it = fcl_objs_.find(obj->id_);
00218 if (it != fcl_objs_.end())
00219 {
00220 it->second.unregisterFrom(manager_.get());
00221 it->second.clear();
00222 fcl_objs_.erase(it);
00223 }
00224 cleanCollisionGeometryCache();
00225 }
00226 else
00227 {
00228 updateFCLObject(obj->id_);
00229 if (action & (World::DESTROY|World::REMOVE_SHAPE))
00230 cleanCollisionGeometryCache();
00231 }
00232 }
00233
00234 double collision_detection::CollisionWorldFCL::distanceRobotHelper(const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const
00235 {
00236 const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
00237 FCLObject fcl_obj;
00238 robot_fcl.constructFCLObject(state, fcl_obj);
00239
00240 CollisionRequest req;
00241 CollisionResult res;
00242 CollisionData cd(&req, &res, acm);
00243 cd.enableGroup(robot.getRobotModel());
00244
00245 for(std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
00246 manager_->distance(fcl_obj.collision_objects_[i].get(), &cd, &distanceCallback);
00247
00248
00249 return res.distance;
00250 }
00251
00252 double collision_detection::CollisionWorldFCL::distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state) const
00253 {
00254 return distanceRobotHelper(robot, state, NULL);
00255 }
00256
00257 double collision_detection::CollisionWorldFCL::distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const
00258 {
00259 return distanceRobotHelper(robot, state, &acm);
00260 }
00261
00262 double collision_detection::CollisionWorldFCL::distanceWorld(const CollisionWorld &world) const
00263 {
00264 return distanceWorldHelper(world, NULL);
00265 }
00266
00267 double collision_detection::CollisionWorldFCL::distanceWorld(const CollisionWorld &world, const AllowedCollisionMatrix &acm) const
00268 {
00269 return distanceWorldHelper(world, &acm);
00270 }
00271
00272 double collision_detection::CollisionWorldFCL::distanceWorldHelper(const CollisionWorld &other_world, const AllowedCollisionMatrix *acm) const
00273 {
00274 const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(other_world);
00275 CollisionRequest req;
00276 CollisionResult res;
00277 CollisionData cd(&req, &res, acm);
00278 manager_->distance(other_fcl_world.manager_.get(), &cd, &distanceCallback);
00279
00280 return res.distance;
00281 }
00282
00283 #include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
00284 const std::string collision_detection::CollisionDetectorAllocatorFCL::NAME_("FCL");