From 6ea4ef4d347f700584f4f050cbf01f453394ca02 Mon Sep 17 00:00:00 2001 From: Michel Breyer <10465414+mbreyer@users.noreply.github.com> Date: Mon, 6 Dec 2021 10:49:18 +0100 Subject: [PATCH] Fix timing issue --- scripts/run.py | 2 +- src/active_grasp/controller.py | 1 + src/active_grasp/nbv.py | 3 --- src/active_grasp/policy.py | 3 +++ 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/scripts/run.py b/scripts/run.py index 57bb81a..b9da424 100755 --- a/scripts/run.py +++ b/scripts/run.py @@ -27,7 +27,7 @@ def main(): seed_simulation(args.seed) rospy.sleep(1.0) # Prevents a rare race condiion - for _ in tqdm(range(args.runs)): + for _ in tqdm(range(args.runs), disable=args.wait_for_input): if args.wait_for_input: controller.gripper.move(0.08) controller.switch_to_joint_trajectory_control() diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index 4a07829..6129469 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -107,6 +107,7 @@ class GraspController: r.sleep() rospy.sleep(0.1) # Wait for a zero command to be sent to the robot. timer.shutdown() + self.policy.deactivate() return self.policy.best_grasp def get_state(self): diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py index ea15421..443db27 100644 --- a/src/active_grasp/nbv.py +++ b/src/active_grasp/nbv.py @@ -104,9 +104,6 @@ class NextBestView(MultiViewPolicy): if gain < self.min_gain and len(self.views) > self.T: self.done = True - if self.done: - self.vis.clear_ig_views() - self.x_d = nbv def best_grasp_prediction_is_stable(self): diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py index a4b4b77..c8e314a 100644 --- a/src/active_grasp/policy.py +++ b/src/active_grasp/policy.py @@ -96,6 +96,9 @@ class Policy: filtered_qualities.append(quality) return filtered_grasps, filtered_qualities + def deactivate(self): + self.vis.clear_ig_views() + def select_best_grasp(grasps, qualities): i = np.argmax(qualities)