solved the fucking mysterous bug

cfg/active_grasp.rviz

@@ -6,7 +6,8 @@ Panels:
       Expanded:
         - /TF1/Frames1
         - /Markers1/Namespaces1
-      Splitter Ratio: 0.6881287693977356
+        - /Marker1
+      Splitter Ratio: 0.6852940917015076
     Tree Height: 226
   - Class: rviz/Selection
     Name: Selection
@@ -362,6 +363,42 @@ Visualization Manager:
       Transport Hint: raw
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 239; 41; 41
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /debug_pcd
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /grasp_markers
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -390,7 +427,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.7134475111961365
+      Distance: 0.8019989132881165
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -398,17 +435,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.3979946970939636
+        X: 0.5695413947105408
-        Y: -0.1718180924654007
+        Y: -0.03970015048980713
-        Z: 0.29551374912261963
+        Z: 0.45675671100616455
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4653984010219574
+      Pitch: 0.295397013425827
       Target Frame: <Fixed Frame>
-      Yaw: 1.0903979539871216
+      Yaw: 5.118584632873535
     Saved:
       - Class: rviz/Orbit
         Distance: 1.2000000476837158
@@ -448,5 +485,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1095
-  X: 1270
+  X: 1260
-  Y: 138
+  Y: 123

cfg/active_grasp.yaml

@@ -2,7 +2,7 @@ bt_sim:
   gui: True
   gripper_force: 10
   # scene: random
-  scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml
+  scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml
 
 hw:
   roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt

scripts/run.py

@@ -46,7 +46,7 @@ def main():
 def create_parser():
     parser = argparse.ArgumentParser()
     parser.add_argument("policy", type=str, choices=registry.keys())
-    parser.add_argument("--runs", type=int, default=10)
+    parser.add_argument("--runs", type=int, default=1)
     parser.add_argument("--wait-for-input", action="store_true")
     parser.add_argument("--logdir", type=Path, default="logs")
     parser.add_argument("--seed", type=int, default=1)

src/active_grasp/active_perception_policy.py

@@ -12,6 +12,15 @@ import requests
 import matplotlib.pyplot as plt
 from vgn.grasp import ParallelJawGrasp
 import time
+from visualization_msgs.msg import Marker, MarkerArray
+from geometry_msgs.msg import Pose
+import tf
+
+import sensor_msgs.point_cloud2 as pc2
+from sensor_msgs.msg import PointCloud2, PointField
+import std_msgs.msg
+import ros_numpy
+
 
 
 class RealTime3DVisualizer:
@@ -192,6 +201,10 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
         self.updated = False
         self._base_url = flask_base_url
 
+        # For debugging
+        self.pcd_publisher = rospy.Publisher('/debug_pcd', PointCloud2, queue_size=10)
+        self.grasp_publisher = rospy.Publisher("/grasp_markers", MarkerArray, queue_size=10)
+
 
     def request_grasping_pose(self, data):
         response = requests.post(f"{self._base_url}/get_gsnet_grasp", json=data)
@@ -247,17 +260,28 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
             self.target_points, self.scene_points = self.depth_image_to_ap_input(img, seg, target_id)
             target_points_list = np.asarray(self.target_points.cpu().numpy())[0].tolist()
             central_point_of_target = np.mean(target_points_list, axis=0)
+            target_points_radius = np.max(np.linalg.norm(target_points_list - central_point_of_target, axis=1))
             scene_points_list = np.asarray(self.scene_points.cpu().numpy())[0].tolist()
             merged_points_list = target_points_list + scene_points_list
-            # gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), central_point_of_target)
+            gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), 
-            gsnet_input_points = target_points_list
+                                                      central_point_of_target, 
+                                                      radius=target_points_radius)
+            # gsnet_input_points = target_points_list
+            # gsnet_input_points = merged_points_list
+            self.publish_pointcloud(gsnet_input_points)
             gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points))
 
+            # DEBUG: publish grasps
+            # self.publish_grasps(gsnet_grasping_poses)
+
             # Convert all grasping poses' reference frame to arm frame
             current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0")
             for gg in gsnet_grasping_poses:
-                T = np.asarray(gg['T'])
+                gg['T'] = current_cam_pose.cpu().numpy().dot(np.asarray(gg['T']))
-                gg['T'] = current_cam_pose.cpu().numpy() @ T
+                # Now here is a mysterous bug, the grasping poses have to be rotated 
+                # 90 degrees around y-axis to be in the correct reference frame
+                R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
+                gg['T'][:3, :3] = gg['T'][:3, :3].dot(R)
             
             # Convert grasping poses to ParallelJawGrasp objects
             grasps = []
@@ -271,7 +295,6 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
             
             # Visualize grasps
             self.vis.grasps(self.base_frame, grasps, qualities)
-            time.sleep(1000000)
             
             # Filter grasps
             filtered_grasps = []
@@ -280,8 +303,8 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
                 pose = grasp.pose
                 # tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
                 tip = pose.translation
-                # if self.bbox.is_inside(tip):
+                if self.bbox.is_inside(tip):
-                if(True):
+                # if(True):
                     q_grasp = self.solve_ee_ik(q, pose * self.T_grasp_ee)
                     if q_grasp is not None:
                         filtered_grasps.append(grasp)
@@ -294,6 +317,59 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
                 self.vis.clear_grasp()
             self.done = True
     
+    def publish_grasps(self, gg):
+        marker_array = MarkerArray()
+        marker_array.markers = []
+        for idx, g in enumerate(gg):
+            g['T'] = np.asarray(g['T'])
+            marker = Marker()
+            marker.header.frame_id = "camera_depth_optical_frame"
+            marker.header.stamp = rospy.Time.now()
+            marker.ns = "grasps"
+            marker.id = idx
+            marker.type = Marker.ARROW
+            marker.action = Marker.ADD
+            marker.pose.position.x = g['T'][0, 3]
+            marker.pose.position.y = g['T'][1, 3]
+            marker.pose.position.z = g['T'][2, 3]
+            q = tf.transformations.quaternion_from_matrix(g['T'])
+            marker.pose.orientation.x = q[0]
+            marker.pose.orientation.y = q[1]
+            marker.pose.orientation.z = q[2]
+            marker.pose.orientation.w = q[3]
+            marker.scale.x = 0.1
+            marker.scale.y = 0.01
+            marker.scale.z = 0.01
+            marker.color.a = 1.0
+            marker.color.r = 0.0
+            marker.color.g = 1.0
+            marker.color.b = 0.0
+            marker_array.markers.append(marker)
+        self.grasp_publisher.publish(marker_array)
+
+    def publish_pointcloud(self, point_cloud):
+        point_cloud = np.asarray(point_cloud)
+        cloud_msg = self.create_pointcloud_msg(point_cloud)
+        self.pcd_publisher.publish(cloud_msg)
+
+    def create_pointcloud_msg(self, point_cloud):
+        # Define the header
+        header = std_msgs.msg.Header()
+        header.stamp = rospy.Time.now()
+        header.frame_id = 'camera_depth_optical_frame'  # Change this to your desired frame of reference
+
+        # Define the fields for the PointCloud2 message
+        fields = [
+            PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1),
+        ]
+        
+        # Create the PointCloud2 message
+        cloud_msg = pc2.create_cloud(header, fields, point_cloud)
+
+        return cloud_msg
+
     def crop_pts_sphere(self, points, crop_center, radius=0.2):
         crop_mask = np.linalg.norm(points - crop_center, axis=1) < radius
         return points[crop_mask].tolist()

src/active_grasp/controller.py

@@ -113,9 +113,6 @@ class GraspController:
             while not self.policy.done:
                 depth_img, seg_image, pose, q = self.get_state()
                 target_seg_id = self.get_target_id(TargetIDRequest()).id
-                # sleep 1s
-                for i in range(self.control_rate*1):
-                    r.sleep()
                 self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
                 # Wait for the robot to move to its desired camera pose
                 moving_to_The_target = True
@@ -129,6 +126,9 @@ class GraspController:
                     if(linear_d < self.move_to_target_threshold):
                         # Arrived
                         moving_to_The_target = False
+                # sleep 3s to wait for the arrival of the robot
+                secs = 3
+                for i in range(self.control_rate*secs):
                     r.sleep()
         elif(self.policy.policy_type=="multi_view"):
             while not self.policy.done: