fix #28

moveit · Jan 25, 2013 · 6e9c717 · 6e9c717
1 parent aad35e7
commit 6e9c717
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 3 deletions.
diff --git a/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
@@ -212,6 +212,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape,
                                             const kinematic_state::AttachedBody *ab);
 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, double scale, double padding,
                                             const CollisionWorld::Object *obj);
+void cleanCollisionGeometryCache(void);
 
 inline void transform2fcl(const Eigen::Affine3d &b, fcl::Transform3f &f)
 {

diff --git a/collision_detection_fcl/src/collision_common.cpp b/collision_detection_fcl/src/collision_common.cpp
@@ -320,12 +320,12 @@ struct FCLShapeCache
 {
   FCLShapeCache(void) : clean_count_(0) {}
 
-  void bumpUseCount(void) 
+  void bumpUseCount(bool force = false) 
   { 
     clean_count_++;
 
     // clean-up for cache (we don't want to keep infinitely large number of weak ptrs stored)
-    if (clean_count_ > MAX_CLEAN_COUNT)
+    if (clean_count_ > MAX_CLEAN_COUNT || force)
     {
       clean_count_ = 0;
       unsigned int from = map_.size();
@@ -702,6 +702,20 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape,
   return createCollisionGeometry<fcl::OBBRSS, CollisionWorld::Object>(shape, scale, padding, obj);
 }
 
+void cleanCollisionGeometryCache(void)
+{
+  FCLShapeCache &cache1 = GetShapeCache<fcl::OBBRSS, CollisionWorld::Object>();
+  {
+    boost::mutex::scoped_lock slock(cache1.lock_);
+    cache1.bumpUseCount(true);
+  }
+  FCLShapeCache &cache2 = GetShapeCache<fcl::OBBRSS, kinematic_state::AttachedBody>();
+  {   
+    boost::mutex::scoped_lock slock(cache2.lock_);
+    cache2.bumpUseCount(true);
+  }  
+}
+
 }
 
 void collision_detection::CollisionData::enableGroup(const kinematic_model::KinematicModelConstPtr &kmodel)

diff --git a/collision_detection_fcl/src/collision_world.cpp b/collision_detection_fcl/src/collision_world.cpp
@@ -195,6 +195,7 @@ bool collision_detection::CollisionWorldFCL::removeShapeFromObject(const std::st
   if (CollisionWorld::removeShapeFromObject(id, shape))
   {
     updateFCLObject(id);
+    cleanCollisionGeometryCache();
     return true;
   }
   else
@@ -212,13 +213,15 @@ void collision_detection::CollisionWorldFCL::removeObject(const std::string &id)
     fcl_objs_.erase(it);
     // manager_->update();
   }
+  cleanCollisionGeometryCache();
 }
 
 void collision_detection::CollisionWorldFCL::clearObjects(void)
 {
   CollisionWorld::clearObjects();
   manager_->clear();
-  fcl_objs_.clear();
+  fcl_objs_.clear();  
+  cleanCollisionGeometryCache();
 }
 
 double collision_detection::CollisionWorldFCL::distanceRobotHelper(const CollisionRobot &robot, const kinematic_state::KinematicState &state, const AllowedCollisionMatrix *acm) const