Adding tags for capturing trajectory timing data

Author: jeking04

src/prpy/base/robot.py

@@ -28,15 +28,16 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import functools, logging, openravepy, numpy
+import functools, logging, openravepy, numpy, time
 import prpy.util
 from .. import bind, named_config, planning, util
 from ..clone import Clone, Cloned
 from ..tsr.tsrlibrary import TSRLibrary
-from ..planning.base import Sequence 
+from ..planning.base import Sequence, Tags
 from ..planning.ompl import OMPLSimplifier
 from ..planning.retimer import HauserParabolicSmoother, OpenRAVEAffineRetimer, ParabolicRetimer
 from ..planning.mac_smoother import MacSmoother
+from ..util import CopyTrajectory, GetTrajectoryTags, SetTrajectoryTags, TagTrajectoryTiming
 
 logger = logging.getLogger('robot')
 
@@ -377,6 +378,7 @@ def do_execute():
 
             with Timer() as timer:
                 traj = self.PostProcessPath(path, defer=False, **kwargs)
+            TagTrajectoryTiming(traj, Tags.POSTPROCESS_TIME, timer.get_duration())
 
             logger.info('Post-processing took %.3f seconds and produced a path'
                         ' with %d waypoints and a duration of %.3f seconds.',
@@ -385,7 +387,10 @@ def do_execute():
                 traj.GetDuration()
             )
 
-            return self.ExecuteTrajectory(traj, defer=False, **kwargs)
+            with Timer() as timer:
+                exec_traj = self.ExecuteTrajectory(traj, defer=False, **kwargs)
+            TagTrajectoryTiming(exec_traj, Tags.EXECUTION_TIME, timer.get_duration())
+            return exec_traj
 
         if defer is True:
             from trollius.executor import get_default_executor
@@ -541,7 +546,10 @@ def _PlanWrapper(self, planning_method, args, kw_args):
         config_spec = self.GetActiveConfigurationSpecification('linear')
 
         # Call the planner.
-        result = planning_method(self, *args, **kw_args)
+        from ..util import Timer
+        with Timer() as timer:
+            result = planning_method(self, *args, **kw_args)
+        TagTrajectoryTiming(result, Tags.PLAN_TIME, timer.get_duration())
 
         def postprocess_trajectory(traj):
             # Strip inactive DOFs from the trajectory.

src/prpy/planning/base.py

@@ -43,6 +43,9 @@ class Tags(object):
     CONSTRAINED = 'constrained'
     PLANNER = 'planner'
     METHOD = 'planning_method'
+    PLAN_TIME = 'planning_time'
+    POSTPROCESS_TIME = 'postprocess_time'
+    EXECUTION_TIME = 'execution_time'
 
 class PlanningError(Exception):
     pass

src/prpy/util.py

@@ -693,3 +693,14 @@ def FindCatkinResource(package, relative_path):
     else:
         raise IOError('Loading resource "{:s}" failed.'.format(
                       relative_path))
+
+def TagTrajectoryTiming(traj, tag, timedata):
+    """
+    Tag a trajectory with timing metadata
+    @param traj The trajectory to tag
+    @param tag The name of the tag to add
+    @param timedata The timing data to put in the tag (usually time in seconds)
+    """
+    tags = GetTrajectoryTags(traj)
+    tags.setdefault(tag, timedata)
+    SetTrajectoryTags(traj, tags, append=False)