diff --git a/active_grasp/controller.py b/active_grasp/controller.py index 1f428ba..27f1065 100644 --- a/active_grasp/controller.py +++ b/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) diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 7f7260e..63ba005 100644 --- a/active_grasp/policy.py +++ b/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): diff --git a/active_grasp/timer.py b/active_grasp/timer.py index 7363167..34dc580 100644 --- a/active_grasp/timer.py +++ b/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