collision_world_fcl.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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   // m->tree_init_level = 2;
00048   manager_.reset(m);
00049 
00050   // request notifications about changes to new world
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   // m->tree_init_level = 2;
00058   manager_.reset(m);
00059 
00060   // request notifications about changes to new world
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   // m->tree_init_level = 2;
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   // manager_->update();
00076 
00077   // request notifications about changes to new world
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   // remove FCL objects that correspond to this object
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   // check to see if we have this object
00188   collision_detection::World::const_iterator it = getWorld()->find(id);
00189   if (it != getWorld()->end())
00190   {
00191     // construct FCL objects that correspond to this object
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   // manager_->update();
00210 }
00211 
00212 void collision_detection::CollisionWorldFCL::setWorld(const WorldPtr& world)
00213 {
00214   if (world == getWorld())
00215     return;
00216 
00217   // turn off notifications about old world
00218   getWorld()->removeObserver(observer_handle_);
00219 
00220   // clear out objects from old world
00221   manager_->clear();
00222   fcl_objs_.clear();
00223   cleanCollisionGeometryCache();
00224 
00225   CollisionWorld::setWorld(world);
00226 
00227   // request notifications about changes to new world
00228   observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
00229 
00230   // get notifications any objects already in the new world
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");


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:35