From 79a2bb4afa2581bff08c3e59f91a88bc2af31b00 Mon Sep 17 00:00:00 2001
From: seungback <shback@gist.ac.kr>
Date: Mon, 27 Sep 2021 13:35:16 +0900
Subject: [PATCH] ICRA2022 version

---
 .gitignore                  |  1 +
 README.md                   |  2 +-
 src/contact_grasp_server.py | 23 ++++++-----------------
 src/uoais                   |  2 +-
 4 files changed, 9 insertions(+), 19 deletions(-)
 mode change 120000 => 160000 src/uoais

diff --git a/.gitignore b/.gitignore
index ab25d90..61c7a0e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,6 @@
 .vscode/
 pointnet2/
+uoais/
 
 # Byte-compiled / optimized / DLL files
 __pycache__/
diff --git a/README.md b/README.md
index 790efdd..51f9962 100644
--- a/README.md
+++ b/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
diff --git a/src/contact_grasp_server.py b/src/contact_grasp_server.py
index d49e146..6e9154a 100755
--- a/src/contact_grasp_server.py
+++ b/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:
diff --git a/src/uoais b/src/uoais
deleted file mode 120000
index cabda89..0000000
--- a/src/uoais
+++ /dev/null
@@ -1 +0,0 @@
-/home/demo/Workspace/seung/UOAMask
\ No newline at end of file
diff --git a/src/uoais b/src/uoais
new file mode 160000
index 0000000..6e2ac53
--- /dev/null
+++ b/src/uoais
@@ -0,0 +1 @@
+Subproject commit 6e2ac5355d8ef3d992fc3b3167bf27be2c7bd7af