add new srv

src/active_grasp/active_perception_policy.py

@@ -19,7 +19,7 @@ class ActivePerceptionPolicy(MultiViewPolicy):
         super().activate(bbox, view_sphere)
 
     def update(self, img, seg, x, q):
-        self.depth_image_to_ap_input(img)
+        self.depth_image_to_ap_input(img, seg)
         # if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable():
         #     self.done = True
         # else:
@@ -41,8 +41,25 @@ class ActivePerceptionPolicy(MultiViewPolicy):
 
         #     self.x_d = nbv
     
-    def depth_image_to_ap_input(self, depth_img):
-        print(self.intrinsic.K)
+    def depth_image_to_ap_input(self, depth_img, seg_img):
+        K = self.intrinsic.K
+        depth_shape = depth_img.shape
+        seg_shape = seg_img.shape
+        if(depth_shape == seg_shape):
+            img_shape = depth_shape
+        else:
+            print("Depth image shape and segmentation image shape are not the same")
+            return None
+        
+        # Depth image to PCD
+        u_indices , v_indices = np.meshgrid(np.arange(img_shape[0]), np.arange(img_shape[1]))
+        x_factors = (u_indices - K[0, 2]) / K[0, 0]
+        y_factors = (v_indices - K[1, 2]) / K[1, 1]
+
+        for i in range(img_shape[0]):
+            for j in range(img_shape[1]):
+                seg_id = seg_img[i, j]
+
 
     def best_grasp_prediction_is_stable(self):
         if self.best_grasp: