ICRA2022 version

.gitignore

@@ -1,5 +1,6 @@
 .vscode/
 pointnet2/
+uoais/
 
 # Byte-compiled / optimized / DLL files
 __pycache__/

README.md

@@ -101,7 +101,7 @@ ROS_NAMESPACE=azure1 roslaunch azure_kinect_ros_driver driver.launch color_resol
 
 contact graspnet server
 ```
-ros27 && roscd deep_grasping_ros $$ python src/contact_grasp_server.py
+ros27 && roscd deep_grasping_ros && python src/contact_grasp_server.py
 ```
 
 contact graspnet client

src/contact_grasp_server.py

@@ -29,10 +29,6 @@ def __init__(self):
 
         # initialize dnn server 
         self.grasp_sock, _ = su.initialize_server('localhost', 5555)
-        rospy.loginfo("Wating for camera info")
-        self.camera_info = rospy.wait_for_message("/azure1/rgb/camera_info", CameraInfo)
-        self.data = {}
-        self.data["intrinsics_matrix"] = np.array(self.camera_info.K).reshape(3, 3)
         rospy.loginfo("Successfully got camera info")
 
         self.bridge = cv_bridge.CvBridge()
@@ -77,24 +73,17 @@ def get_target_grasp_pose(self, msg):
                 H_cam_to_hand = orh.msg_to_se3(H_cam_to_hand)
                 break
 
-        rgb = rospy.wait_for_message("/azure1/rgb/image_raw", Image)
-        depth = rospy.wait_for_message("/azure1/depth_to_rgb/image_raw", Image)
-        rgb = self.bridge.imgmsg_to_cv2(rgb, desired_encoding='bgr8')
-        depth = self.bridge.imgmsg_to_cv2(depth, desired_encoding='32FC1')
-        self.data["rgb"] = rgb
-        self.data["depth"] = depth 
+
         self.initialize_marker()
         self.marker_id = 0
 
 
-        # send image to dnn client
-        rospy.loginfo_once("Sending rgb, depth to Contact-GraspNet client")
-        su.sendall_pickle(self.grasp_sock, self.data)
+        # send start signal to dnn client
+        su.sendall_pickle(self.grasp_sock, 1)
         pred_grasps_cam, scores, contact_pts, segm_result = su.recvall_pickle(self.grasp_sock)
-        segmap, amodal_vis, visible_vis, occlusions = \
-            self.bridge.cv2_to_imgmsg(segm_result["segm"]), segm_result["amodal_vis"], segm_result["visible_vis"], segm_result["occlusions"].tolist()
-        self.uoais_vm_pub.publish(self.bridge.cv2_to_imgmsg(visible_vis))
-        self.uoais_am_pub.publish(self.bridge.cv2_to_imgmsg(amodal_vis))
+        segmap, vis, occlusions = \
+            self.bridge.cv2_to_imgmsg(segm_result["segm"]), segm_result["vis"], segm_result["occlusions"].tolist()
+        self.uoais_am_pub.publish(self.bridge.cv2_to_imgmsg(vis))
 
 
         if pred_grasps_cam is None: