Merge pull request #286 from personalrobotics/bugfix/postprocess_reuse

Reuse postprocessing environments in a safer way.

Author: mkoval

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
@@ -165,6 +171,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,13 +293,15 @@ 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()
+        # 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.
+        postprocess_env = Robot._postprocess_envs[
+            openravepy.RaveGetEnvironmentId(self.GetEnv())]
 
         with Clone(self.GetEnv(),
-                   clone_env=self._postprocess_env) as cloned_env:
+                   clone_env=postprocess_env) as cloned_env:
             cloned_robot = cloned_env.Cloned(self)
 
             # Planners only operate on the active DOFs. We'll set any DOFs