update

src/active_grasp/controller.py

@@ -17,6 +17,9 @@ from robot_helpers.ros.moveit import MoveItClient, create_collision_object_from_
 from robot_helpers.spatial import Rotation, Transform
 from vgn.utils import look_at, cartesian_to_spherical, spherical_to_cartesian
 
+RED = '\033[91m'
+RESET = '\033[0m'
+
 
 class GraspController:
     def __init__(self, policy):
@@ -87,7 +90,9 @@ class GraspController:
         self.latest_seg_msg = msg
 
     def run(self):
+        print("before reset")
         bbox = self.reset()
+        print("after reset")
         self.switch_to_cartesian_velocity_control()
         with Timer("search_time"):
             grasp = self.search_grasp(bbox)
@@ -97,6 +102,7 @@ class GraspController:
                 res = self.execute_grasp(grasp)
         else:
             res = "aborted"
+            print(f"{RED}[{res}]No grasp found{RESET}")
         return self.collect_info(res)
 
     def reset(self):
@@ -125,7 +131,7 @@ class GraspController:
                     current_p = pose.as_matrix()[:3,3]
                     target_p = self.policy.x_d.as_matrix()[:3,3]
                     linear_d = np.linalg.norm(current_p - target_p)
-                    if(linear_d < self.move_to_target_threshold):
+                    if(linear_d < self.move_to_target_threshold*2):
                         # Arrived
                         moving_to_The_target = False
                 # sleep 3s to wait for the arrival of the robot
@@ -135,15 +141,19 @@ class GraspController:
         elif(self.policy.policy_type=="multi_view"):
             while not self.policy.done:
                 depth_img, seg_image, pose, q = self.get_state()
+                #print("before get target id in search_grasp()")
+                #import ipdb; ipdb.set_trace()
                 target_seg_id = self.get_target_id(TargetIDRequest()).id
                 support_seg_id = self.get_support_id(TargetIDRequest()).id
+                #print("after get target id in search_grasp()")
+
                 # print(target_seg_id, support_seg_id)
                 # import ipdb; ipdb.set_trace()
                 self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
                 r.sleep()
         else:
             print("Unsupported policy type: "+str(self.policy.policy_type))
-
+        import ipdb; ipdb.set_trace()
         # Wait for a zero command to be sent to the robot.
         rospy.sleep(0.2)
         self.policy.deactivate()