Go to the documentation of this file.00001 import roslib; roslib.load_manifest('iri_common_smach')
00002 import rospy
00003 import smach
00004 import smach_ros
00005 import time
00006
00007 """
00008 Timed State class to measure execution time and number of executions
00009
00010 USAGE:
00011 Init method should call timed_state.TimedState.__init__(...) instead of smach.State.__init__(...)
00012 Use timed_execute instead of execute to measure times
00013 """
00014
00015 class TimedState(smach.State):
00016 """
00017 State class that measures times and executions
00018 """
00019 def __init__(self, **args):
00020 """
00021 Init method also initializes timer and executions counter
00022 WARNING user state should call the init of this class : timed_state.TimedState.__init__(...)
00023 """
00024 smach.State.__init__(self, **args)
00025 self.total_time = 0
00026 self.num_executions = 0
00027
00028 def execute(self, userdata):
00029 """
00030 Calls timed_execute and measures its execution time
00031 WARNING Don't redefine this method
00032 """
00033 init_time = time.time()
00034 self.num_executions += 1
00035 result = self.timed_execute(userdata)
00036 self.total_time += (time.time() - init_time)
00037 return result
00038
00039 def timed_execute(self, userdata):
00040 """
00041 Use this method instead of execute
00042 Code executed here is timed
00043 """
00044 raise NotImplementedError()
00045
00046 def get_execution_time(self):
00047 return self.total_time
00048
00049 def get_number_executions(self):
00050 return self.num_executions