Merge pull request #136 from personalrobotics/feature/timing_tags

Adding tags for capturing trajectory timing data

Author: mkoval

src/prpy/base/robot.py

@@ -33,10 +33,11 @@
 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 SetTrajectoryTags
 
 logger = logging.getLogger('robot')
 
@@ -377,6 +378,7 @@ def do_execute():
 
             with Timer() as timer:
                 traj = self.PostProcessPath(path, defer=False, **kwargs)
+            SetTrajectoryTags(traj, {Tags.POSTPROCESS_TIME: timer.get_duration()}, append=True)
 
             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)
+            SetTrajectoryTags(exec_traj, {Tags.EXECUTION_TIME: timer.get_duration()}, append=True)
+            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)
+        SetTrajectoryTags(result, {Tags.PLAN_TIME: timer.get_duration()}, append=True)
 
         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