Accumulate times

active_grasp/controller.py

@@ -86,6 +86,7 @@ class GraspController:
         return self.collect_info(res)
 
     def reset(self):
+        Timer.reset()
         res = self.reset_env(ResetRequest())
         rospy.sleep(1.0)  # Wait for the TF tree to be updated.
         return from_bbox_msg(res.bbox)

active_grasp/policy.py

@@ -61,7 +61,7 @@ class Policy:
         self.T_base_task = Transform.translation(self.bbox.center - np.full(3, 0.15))
         self.T_task_base = self.T_base_task.inv()
         tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
-        rospy.sleep(0.5)  # Wait for tf tree to be updated
+        rospy.sleep(1.0)  # Wait for tf tree to be updated
         self.vis.workspace(self.task_frame, 0.3)
 
     def update(self, img, x, q):

active_grasp/timer.py

@@ -6,6 +6,7 @@ class Timer:
 
     def __init__(self, name):
         self.name = name
+        self.timers.setdefault(name, 0)
 
     def __enter__(self):
         self.start()
@@ -14,9 +15,13 @@ class Timer:
     def __exit__(self, *exc_info):
         self.stop()
 
+    @classmethod
+    def reset(cls):
+        cls.timers = dict()
+
     def start(self):
         self.tic = time.perf_counter()
 
     def stop(self):
         elapsed_time = time.perf_counter() - self.tic
-        self.timers[self.name] = elapsed_time
+        self.timers[self.name] += elapsed_time