From 79d709d1ac9cfb6c4189cda8629fba7ce4a3dd42 Mon Sep 17 00:00:00 2001
From: 0nhc <thefoxfoxfox@outlook.com>
Date: Sun, 13 Oct 2024 06:40:54 -0500
Subject: [PATCH] ap-single-view baseline succeeded

---
 cfg/active_grasp.yaml          |  2 +-
 src/active_grasp/controller.py | 15 ++++++++++++---
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml
index dc4d91b..a77a9bb 100644
--- a/cfg/active_grasp.yaml
+++ b/cfg/active_grasp.yaml
@@ -12,7 +12,7 @@ grasp_controller:
   ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to panda_link8
   control_rate: 100
   linear_vel: 0.05
-  move_arm_time: 6
+  move_to_target_threshold: 0.05 # meter
   camera:
     frame_id: camera_depth_optical_frame
     info_topic: /camera/depth/camera_info
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index e59e3f3..263178d 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -36,7 +36,7 @@ class GraspController:
         self.min_z_dist = rospy.get_param("~camera/min_z_dist")
         self.control_rate = rospy.get_param("~control_rate")
         self.linear_vel = rospy.get_param("~linear_vel")
-        self.move_arm_time = rospy.get_param("~move_arm_time")
+        self.move_to_target_threshold = rospy.get_param("~move_to_target_threshold")
         self.policy_rate = rospy.get_param("policy/rate")
 
     def init_service_proxies(self):
@@ -113,8 +113,17 @@ class GraspController:
             target_seg_id = self.get_target_id(TargetIDRequest()).id
             self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
             # Wait for the robot to move to its desired camera pose
-            ticks = self.move_arm_time*self.policy_rate
-            for i in range(ticks):
+            moving_to_The_target = True
+            while(moving_to_The_target):
+                depth_img, seg_image, pose, q = self.get_state()
+                current_p = pose.as_matrix()[:3,3]
+                target_p = self.policy.x_d.as_matrix()[:3,3]
+                linear_d = np.sqrt((current_p[0]-target_p[0])**2+
+                                   (current_p[1]-target_p[1])**2+
+                                   (current_p[2]-target_p[2])**2)
+                if(linear_d < self.move_to_target_threshold):
+                    # Arrived
+                    moving_to_The_target = False
                 r.sleep()
 
         # Wait for a zero command to be sent to the robot.