add new srv

CMakeLists.txt

@@ -19,6 +19,7 @@ add_service_files(
   FILES
   Reset.srv
   Seed.srv
+  TargetID.srv
 )
 
 generate_messages(

scripts/bt_sim_node.py

@@ -73,6 +73,7 @@ class BtSimNode:
             SwitchController,
             self.switch_controller,
         )
+        rospy.Service("get_target_seg_id", Trigger, self.get_target_seg_id)
 
     def seed(self, req):
         self.sim.seed(req.seed)
@@ -86,6 +87,11 @@ class BtSimNode:
         bbox = self.sim.reset()
         self.activate_plugins()
         return ResetResponse(to_bbox_msg(bbox))
+    
+    def seed(self, req):
+        self.sim.seed(req.seed)
+        rospy.loginfo(f"Seeded the rng with {req.seed}.")
+        return SeedResponse()
 
     def switch_controller(self, req):
         for controller in req.stop_controllers:

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):
+    def depth_image_to_ap_input(self, depth_img, seg_img):
-        print(self.intrinsic.K)
+        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:

srv/TargetID.srv

@@ -0,0 +1,2 @@
+---
+int16 id