Reuses postprocessing environments in a safer way.

The previous implementation of PostProcessPath could potentially
generate unbounded numbers of environments, as it could generate
a new robot instance every time we clone into the same environment.

Here, we keep a module-level mapping from environments to
postprocessing environments and create them on demand, which
guarantees that each cloned environment will have at most one
postprocessing environment.

Author: psigen

src/prpy/base/robot.py

@@ -27,7 +27,7 @@
 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
-
+import collections
 import functools, logging, openravepy, numpy
 from .. import bind, named_config, exceptions, util
 from ..clone import Clone, Cloned
@@ -42,6 +42,12 @@
 
 
 class Robot(openravepy.Robot):
+
+    _postprocess_envs = collections.defaultdict(openravepy.Environment)
+    """
+    Mapping from robot environments to plan postprocessing environments.
+    """
+
     def __init__(self, robot_name=None):
         self.actions = None
         self.planner = None
@@ -74,6 +80,13 @@ def __init__(self, robot_name=None):
         self.smoother = self.retimer
         self.affine_retimer = OpenRAVEAffineRetimer()
 
+        # Since we don't want to endlessly create postprocessing environments,
+        # we maintain a map that uniquely associates each OpenRAVE environment
+        # with a given postprocessing environment.  This way, if we re-clone
+        # into a previously used environment, we will not create a new one.
+        self._postprocess_env = Robot._postprocess_envs[
+            openravepy.RaveGetEnvironmentId(self.GetEnv())]
+
     def __dir__(self):
         # We have to manually perform a lookup in InstanceDeduplicator because
         # __methods__ bypass __getattribute__.
@@ -165,6 +178,7 @@ def CloneBindings(self, parent):
         self.base_manipulation = openravepy.interfaces.BaseManipulation(self)
         self.task_manipulation = openravepy.interfaces.TaskManipulation(self)
 
+
     def AttachController(self, name, args, dof_indices, affine_dofs, simulated):
         """
         Create and attach a controller to a subset of this robot's DOFs. If
@@ -286,11 +300,6 @@ def PostProcessPath(self, path,
             logger.debug('Detected "%s" tag on trajectory: Setting smooth'
                          ' = True', Tags.SMOOTH)
 
-        # Lazily create an environment for post-processing.
-        # This is faster than creating a new environment for every operation.
-        if not hasattr(self, '_postprocess_env'):
-            self._postprocess_env = openravepy.Environment()
-
         with Clone(self.GetEnv(),
                    clone_env=self._postprocess_env) as cloned_env:
             cloned_robot = cloned_env.Cloned(self)