Merge pull request #9 from personalrobotics/feature/boostMemoryLeakFixes

Fix memory leaks in environment cloning

src/prpy/base/wamrobot.py

@@ -51,9 +51,22 @@ def CloneBindings(self, parent):
         Robot.CloneBindings(self, parent)
 
         self.mac_retimer = None
-        self.trajectory_module = prpy.rave.load_module(self.GetEnv(), 'Trajectory', self.GetName())
+        trajectory_module = prpy.rave.load_module(self.GetEnv(), 'Trajectory', self.GetName())
         import manipulation2.trajectory
-        manipulation2.trajectory.bind(self.trajectory_module)
+        #manipulation2.trajectory.bind(self.trajectory_module)
+
+        def myFun(x):
+            pass
+        import types
+
+        class Object():
+            pass
+
+        self.trajectory_module = Object()
+        self.trajectory_module.module = trajectory_module
+        self.trajectory_module.blendtrajectory = types.MethodType(manipulation2.trajectory.blendtrajectory, trajectory_module)
+        self.trajectory_module.executeblendedtrajectory = types.MethodType(manipulation2.trajectory.executeblendedtrajectory, trajectory_module)
+
 
     def RetimeTrajectory(self, traj, max_jerk=30.0, synchronize=False,
                          stop_on_stall=True, stop_on_ft=False, force_direction=None,

src/prpy/bind.py

@@ -31,6 +31,8 @@
 import logging
 from clone import CloneException
 
+logger = logging.getLogger('bind')
+
 class NotCloneableException(CloneException):
     pass
 
@@ -41,6 +43,7 @@ class InstanceDeduplicator(object):
     USERDATA_DESTRUCTOR = '__destructor__'
     USERDATA_CANONICAL = 'canonical_instance'
     ATTRIBUTE_CANONICAL = '_canonical_instance'
+    ATTRIBUTE_IS_CANONICAL = '_is_canonical_instance'
     KINBODY_TYPE, LINK_TYPE, JOINT_TYPE, MANIPULATOR_TYPE = range(4)
 
     @staticmethod
@@ -119,14 +122,21 @@ def get_canonical(cls, instance):
         # If it's not available, fall back on doing the full lookup not
         # available, fall back on doing the full lookup
         except AttributeError:
-            userdata_getter, userdata_setter = cls.get_storage_methods(instance)
             try:
-                canonical_instance = userdata_getter(cls.USERDATA_CANONICAL)
+                is_canonical_instance = object.__getattribute__(instance, cls.ATTRIBUTE_IS_CANONICAL)
+                canonical_instance = instance
+            except AttributeError:
+                userdata_getter, userdata_setter = cls.get_storage_methods(instance)
+                try:
+                    canonical_instance = userdata_getter(cls.USERDATA_CANONICAL)
 
-                # ...and cache the value for future queries.
-                object.__setattr__(instance, cls.ATTRIBUTE_CANONICAL, canonical_instance)
-            except KeyError:
-                canonical_instance = None
+                    # ...and cache the value for future queries.
+                    if canonical_instance is instance:
+                        object.__setattr__(instance, cls.ATTRIBUTE_IS_CANONICAL, True)
+                    else:
+                        object.__setattr__(instance, cls.ATTRIBUTE_CANONICAL, canonical_instance)
+                except KeyError:
+                    canonical_instance = None
 
         return canonical_instance
 
@@ -175,6 +185,38 @@ def call(self, name, *args, **kw_args):
 
         return env, owner, key
 
+    # Cleanup the UserData owned by a KinBody when it is removed from
+    # the environment.
+    @staticmethod
+    def cleanup_callback(owner, flag):
+        if flag == 0: # removed
+            # Clear any attributes that the user might have bound to
+            # the object. This is necessary to clear cycles.
+            canonical_instance = InstanceDeduplicator.get_canonical(owner)
+            if canonical_instance is not None:
+                canonical_dict = object.__getattribute__(canonical_instance, '__dict__')
+                canonical_dict.clear()
+
+
+            # Remove any storage (e.g. canonical_instance) bound to
+            # this object.
+            children, canonical_instance = InstanceDeduplicator.get_bound_children(owner)
+            InstanceDeduplicator.remove_storage(owner)
+
+            # Remove circular references that pass through Boost.Python.
+            # A normal iterator can't be used here because clear_referrers
+            # will delete the child from children causing elements to be
+            # skipped
+            while children:
+                child = children.pop()
+                InstanceDeduplicator.logger.debug(child)
+                clear_referrers(child)
+                # NOTE: this is also an acceptable body for the loop :)
+                # clear_referrers(children[0])
+            InstanceDeduplicator.logger.debug(owner)
+            if canonical_instance != None:
+                clear_referrers(canonical_instance)
+
     @classmethod
     def get_storage_methods(cls, target):
         env, owner, target_key = cls.get_environment_id(target)
@@ -187,14 +229,8 @@ def get_storage_methods(cls, target):
             user_data = dict()
             env.SetUserData(user_data)
 
-            # Cleanup the UserData owned by a KinBody when it is removed from
-            # the environment.
-            def cleanup_callback(owner, flag):
-                if flag == 0: # removed
-                    cls.remove_storage(owner)
-
             if hasattr(env, 'RegisterBodyCallback'):
-                handle = env.RegisterBodyCallback(cleanup_callback)
+                handle = env.RegisterBodyCallback(InstanceDeduplicator.cleanup_callback)
                 user_data[cls.USERDATA_DESTRUCTOR] = handle
             else:
                 cls.logger.warning(
@@ -231,6 +267,28 @@ def setter(key, value):
 
         return getter, setter
 
+    @classmethod
+    def get_bound_children(cls, parent):
+        # Lookup the key for all bound children.
+        env, _, parent_key = cls.get_environment_id(parent)
+        user_data = env.GetUserData()
+        try:
+            child_keys = user_data[parent_key][cls.USERDATA_CHILDREN]
+        except KeyError:
+            # There are no bound children.
+            return [], None
+
+        # Resolve the key to an instance.
+        children = []
+        canonical_instance = None
+        for child_key in child_keys:
+            canonical_child = user_data[child_key].get(cls.USERDATA_CANONICAL, None)
+            if canonical_child != parent:
+                children.append(canonical_child)
+            else:
+                canonical_instance = canonical_child
+        return children, canonical_instance
+
     @classmethod
     def remove_storage(cls, kinbody):
         env, owner, owner_key = cls.get_environment_id(kinbody)
@@ -245,6 +303,59 @@ def remove_storage(cls, kinbody):
             for child_key in children_set:
                 user_data.pop(child_key)
 
+
+def clear_referrers(obj, debug=False):
+    import gc
+
+    # TODO: We should do a topographical sort on these references.
+
+    for referrer in gc.get_referrers(obj):
+        #TODO print if deleting from user defined referrer
+        logger.debug('Clearing referrer "%s" to object "%s".', referrer, obj)
+
+        # Handle standard Python objects.
+        if hasattr(referrer, '__dict__'):
+            for field,value in referrer.__dict__.items():
+                if value is obj:
+                    del referrer.__dict__[field]
+
+        # Remove references from built-in collections.
+        if isinstance(referrer, dict):
+            for field, value in referrer.items():
+                if value is obj:
+                    del referrer[field]
+        elif isinstance(referrer, list) or isinstance(referrer, set):
+            referrer.remove(obj)
+        # tuple and frozenset are immutable, so we remove the whole object.
+        elif isinstance(referrer, tuple) or isinstance(referrer, frozenset):
+            clear_referrers(referrer, debug=debug)
+
+        if debug:
+            import pprint
+            pp = pprint.PrettyPrinter(indent=4)
+            pp.pprint(referrer)
+
+def print_referrers(obj, dbg=False):
+    import gc
+    for referrer in gc.get_referrers(obj):
+        if isinstance(referrer, dict):
+            for field, value in referrer.items():
+                if value is obj:
+                    print referrer, field
+        elif hasattr(referrer, '__dict__'):
+            for field,value in referrer.__dict__.items():
+                if value is obj:
+                    print referrer, field
+        elif hasattr(referrer, 'remove'):
+            print referrer
+        elif type(referrer) == tuple:
+            clear_referrers(referrer, dbg=True)
+        else:
+            #import pprint
+            #pp = pprint.PrettyPrinter(indent=4)
+            #pp.pprint(referrer)
+            pass
+
 def bind_subclass(instance, subclass, *args, **kw_args):
     # Deduplicate the classes associated with any of the objects that we're
     # modifying at runtime. This is necessary for the modifications to be

src/prpy/clone.py

@@ -45,15 +45,27 @@ def __enter__(self):
         pass
 
     def __exit__(self, *args):
+        from prpy.bind import clear_referrers
         if self.destroy_on_exit:
             self.Destroy()
-            self.SetUserData(None)
         else:
             self.__class__.get_envs().pop()
 
     def Destroy(self):
         self.__class__.get_envs().pop()
+
+        # Manually Remove() all objects from the environment. This forces
+        # OpenRAVE to call functions registered to RegisterBodyCallback.
+        # Otherwise, these functions are only called when the environment is
+        # destructed. This is too late for prpy.bind to cleanup circular
+        # references.
+        # TODO: Make this the default behavior in OpenRAVE.
+        for body in self.GetBodies():
+            import prpy.bind
+            prpy.bind.InstanceDeduplicator.cleanup_callback(body, flag=0)
+
         openravepy.Environment.Destroy(self)
+        self.SetUserData(None)
 
     @classmethod
     def get_env(cls):

src/prpy/planning/base.py

@@ -55,10 +55,18 @@ def __init__(self, func):
     def __call__(self, instance, robot, *args, **kw_args):
         env = robot.GetEnv()
 
-        with Clone(env, clone_env=instance.env):
-            planning_traj = self.func(instance, Cloned(robot), *args, **kw_args)
+        cenv = Clone(env)
+        try:#, clone_env=instance.env):
+            instance.setupEnv(cenv)
+            crobot = Cloned(robot)
+
+            planning_traj = self.func(instance, crobot, *args, **kw_args)
             traj = openravepy.RaveCreateTrajectory(env, planning_traj.GetXMLId())
             traj.Clone(planning_traj, 0)
+        except:
+            raise
+        finally:
+            cenv.Destroy()
 
         return traj 
 

src/prpy/planning/cbirrt.py

@@ -39,6 +39,13 @@ def __init__(self):
             self.problem = openravepy.RaveCreateProblem(self.env, 'CBiRRT')
         except openravepy.openrave_exception:
             raise UnsupportedPlanningError('Unable to create CBiRRT module.')
+
+    def setupEnv(self, env):
+        self.env = env
+        try:
+            self.problem = openravepy.RaveCreateProblem(self.env, 'CBiRRT')
+        except openravepy.openrave_exception:
+            raise UnsupportedPlanningError('Unable to create CBiRRT module.')
 
     def __str__(self):
         return 'CBiRRT'

src/prpy/planning/chomp.py

@@ -136,19 +136,41 @@ def get_affected_links(body, dof_indices):
 class CHOMPPlanner(BasePlanner):
     def __init__(self):
         super(CHOMPPlanner, self).__init__()
+        self.setupEnv(self.env)
+
+    def setupEnv(self, env):
+        self.env = env
         try:
             from orcdchomp import orcdchomp
-            self.module = openravepy.RaveCreateModule(self.env, 'orcdchomp')
+            module = openravepy.RaveCreateModule(self.env, 'orcdchomp')
         except ImportError:
             raise UnsupportedPlanningError('Unable to import orcdchomp.')
         except openravepy.openrave_exception as e:
             raise UnsupportedPlanningError('Unable to create orcdchomp module: %s' % e)
 
-        if self.module is None:
+        if module is None:
             raise UnsupportedPlanningError('Failed loading module.')
-
         self.initialized = False
-        orcdchomp.bind(self.module)
+        #orcdchomp.bind(self.module)
+
+        class Object():
+            pass
+
+        import types
+
+        self.module = Object()
+        self.module.module = module
+        self.module.viewspheres = types.MethodType(orcdchomp.viewspheres,module)
+        self.module.computedistancefield = types.MethodType(orcdchomp.computedistancefield,module)
+        self.module.addfield_fromobsarray = types.MethodType(orcdchomp.addfield_fromobsarray,module)
+        self.module.removefield = types.MethodType(orcdchomp.removefield,module)
+        self.module.create = types.MethodType(orcdchomp.create,module)
+        self.module.iterate = types.MethodType(orcdchomp.iterate,module)
+        self.module.gettraj = types.MethodType(orcdchomp.gettraj,module)
+        self.module.destroy = types.MethodType(orcdchomp.destroy,module)
+        self.module.runchomp = types.MethodType(orcdchomp.runchomp,module)
+        self.module.GetEnv = self.module.module.GetEnv
+
         self.distance_fields = DistanceFieldManager(self.module)
 
     def __str__(self):

src/prpy/planning/sbpl.py

@@ -40,6 +40,13 @@ def __init__(self):
         except openravepy.openrave_exception:
             raise UnsupportedPlanningError('Unable to create SBPL module')
 
+    def setupEnv(self, env):
+        self.env = env
+        try:
+            self.problem = openravepy.RaveCreateProblem(self.env, 'SBPL')
+        except openravepy.openrave_exception:
+            raise UnsupportedPlanningError('Unable to create SBPL module.')
+
     def __str__(self):
         return 'SBPL'
 

src/prpy/simulation/servo.py

@@ -33,8 +33,11 @@
 class ServoSimulator(object):
     def __init__(self, manip, rate, watchdog_timeout):
         self.manip = manip
-        self.robot = self.manip.GetRobot()
-        self.env = self.robot.GetEnv()
+        # enabling threading and the following lines causes robot (and hence 
+        # the environment) to not be garbage collected. It seems that threading
+        # somehow blocks self.robot from being garbage collected
+        #self.robot = self.manip.GetRobot()
+        #self.env = self.robot.GetEnv()
 
         self.indices = self.manip.GetArmIndices()
         self.num_dofs = len(self.indices)