successfully applies segmentation input for policy class

README.md

@@ -25,6 +25,20 @@ cd <path-to-your-ws>/src/active_grasp/src/active_grasp/active_perception/modules
 pip install -e .
 ```
 
+# Updated Features
+* Added our baseline: src/active_grasp/active_perception_policy.py
+* Added RGB and Segmentation image publishers. The segmentation ID 1 corresponds to the grasping target object.
+
+
+
+
+
+
+
+
+
+
+
 # Closed-Loop Next-Best-View Planning for Target-Driven Grasping
 
 This repository contains the implementation of our IROS 2022 submission, _"Closed-Loop Next-Best-View Planning for Target-Driven Grasping"_. [[Paper](http://arxiv.org/abs/2207.10543)][[Video](https://youtu.be/67W_VbSsAMQ)]

cfg/active_grasp.rviz

@@ -23,7 +23,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: SceneCloud
@@ -54,6 +53,8 @@ Visualization Manager:
       Value: true
     - Class: rviz/TF
       Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
@@ -86,6 +87,15 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_hand_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_hand_tcp:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         panda_leftfinger:
           Alpha: 1
           Show Axes: false
@@ -96,46 +106,85 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link0_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link1_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link2_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link3_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link4_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link5_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link6_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_link7_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link8:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         panda_rightfinger:
           Alpha: 1
           Show Axes: false
@@ -242,10 +291,7 @@ Visualization Manager:
       Name: Markers
       Namespaces:
         bbox: true
-        grasp: true
+        roi: true
-        ig_views: true
-        path: true
-        views: true
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -364,7 +410,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.2000000476837158
+      Distance: 1.3440001010894775
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -372,9 +418,9 @@ Visualization Manager:
         Value: false
       Field of View: 0.7799999713897705
       Focal Point:
-        X: 0.44999998807907104
+        X: 0.40848347544670105
-        Y: 0
+        Y: 0.02209819294512272
-        Z: 0.4000000059604645
+        Z: 0.4073639214038849
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -416,7 +462,7 @@ Window Geometry:
   Hide Right Dock: true
   InfraImage:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -425,6 +471,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
+  Width: 1157
-  X: 0
+  X: 1309
-  Y: 27
+  Y: 90

cfg/active_grasp.yaml

@@ -16,6 +16,7 @@ grasp_controller:
     frame_id: camera_depth_optical_frame
     info_topic: /camera/depth/camera_info
     depth_topic: /camera/depth/image_rect_raw
+    seg_topic: /camera/segmentation/image_rect_raw
     min_z_dist: 0.3
 
 cartesian_velocity_controller:

src/active_grasp/active_perception_policy.py

@@ -18,7 +18,7 @@ class ActivePerceptionPolicy(MultiViewPolicy):
     def activate(self, bbox, view_sphere):
         super().activate(bbox, view_sphere)
 
-    def update(self, img, x, q):
+    def update(self, img, seg, x, q):
         self.depth_image_to_ap_input(img)
         # if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable():
         #     self.done = True

src/active_grasp/controller.py

@@ -32,6 +32,7 @@ class GraspController:
         self.T_grasp_ee = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
         self.cam_frame = rospy.get_param("~camera/frame_id")
         self.depth_topic = rospy.get_param("~camera/depth_topic")
+        self.seg_topic = rospy.get_param("~camera/seg_topic")
         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")
@@ -71,10 +72,14 @@ class GraspController:
 
     def init_camera_stream(self):
         self.cv_bridge = cv_bridge.CvBridge()
-        rospy.Subscriber(self.depth_topic, Image, self.sensor_cb, queue_size=1)
+        rospy.Subscriber(self.depth_topic, Image, self.depth_cb, queue_size=1)
+        rospy.Subscriber(self.seg_topic, Image, self.seg_cb, queue_size=1)
 
-    def sensor_cb(self, msg):
+    def depth_cb(self, msg):
         self.latest_depth_msg = msg
+    
+    def seg_cb(self, msg):
+        self.latest_seg_msg = msg
 
     def run(self):
         bbox = self.reset()
@@ -102,8 +107,8 @@ class GraspController:
         timer = rospy.Timer(rospy.Duration(1.0 / self.control_rate), self.send_vel_cmd)
         r = rospy.Rate(self.policy_rate)
         while not self.policy.done:
-            img, pose, q = self.get_state()
+            depth_img, seg_image, pose, q = self.get_state()
-            self.policy.update(img, pose, q)
+            self.policy.update(depth_img, seg_image, pose, q)
             r.sleep()
         rospy.sleep(0.2)  # Wait for a zero command to be sent to the robot.
         self.policy.deactivate()
@@ -113,9 +118,11 @@ class GraspController:
     def get_state(self):
         q, _ = self.arm.get_state()
         msg = copy.deepcopy(self.latest_depth_msg)
-        img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001
+        depth_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001
+        msg = copy.deepcopy(self.latest_seg_msg)
+        seg_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)
         pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
-        return img, pose, q
+        return depth_img, seg_img, pose, q
 
     def send_vel_cmd(self, event):
         if self.policy.x_d is None or self.policy.done:

src/active_grasp/nbv.py

@@ -84,7 +84,7 @@ class NextBestView(MultiViewPolicy):
     def activate(self, bbox, view_sphere):
         super().activate(bbox, view_sphere)
 
-    def update(self, img, x, q):
+    def update(self, img, seg, x, q):
         if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable():
             self.done = True
         else:

src/active_grasp/policy.py

@@ -75,7 +75,7 @@ class Policy:
         rospy.sleep(1.0)  # Wait for tf tree to be updated
         self.vis.roi(self.task_frame, 0.3)
 
-    def update(self, img, x, q):
+    def update(self, img, seg, x, q):
         raise NotImplementedError
 
     def filter_grasps(self, out, q):
@@ -106,7 +106,7 @@ def select_best_grasp(grasps, qualities):
 
 
 class SingleViewPolicy(Policy):
-    def update(self, img, x, q):
+    def update(self, img, seg, x, q):
         linear, _ = compute_error(self.x_d, x)
         if np.linalg.norm(linear) < 0.02:
             self.views.append(x)