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