From b5cc301c5444155a14dec8c03a49fdfa87f299ee Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Fri, 15 Sep 2023 03:18:22 -0400
Subject: [PATCH] Surface scanning working well

---
 hello_helpers/src/hello_helpers/hello_misc.py |   2 +-
 stretch_demos/empty_pc.pkl                    | Bin 0 -> 280 bytes
 stretch_demos/nodes/grasp_object              |  29 +++++++++++++-----
 3 files changed, 23 insertions(+), 8 deletions(-)
 create mode 100644 stretch_demos/empty_pc.pkl

diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py
index bbbf7fd..540c574 100644
--- a/hello_helpers/src/hello_helpers/hello_misc.py
+++ b/hello_helpers/src/hello_helpers/hello_misc.py
@@ -178,7 +178,7 @@ class HelloNode:
                 continue
             rate.sleep()
 
-    def get_point_cloud():
+    def get_point_cloud(self):
         assert(self._point_cloud is not None)
         return self._point_cloud
 
diff --git a/stretch_demos/empty_pc.pkl b/stretch_demos/empty_pc.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..8b6b9760c544108f1a9466065c662aaeff3c5665
GIT binary patch
literal 280
zcmZo*oyyC|00uoW#i@D4`9<-$#p%U*KteA*AU`v&#5pIwG{tC24>vMz@)XU+DX~*D
zdPIs#Qjm0bq$Z}M7ES44gEC<1yg7S#i}H&LD)oS5NoH>9lpdB45Cv1<C3}-O)$3f^
zHks6<9_HM{f+?v<-i+Q1AUmY7JIpOJH78|C4;R>75Dv_=9>$6(-VEOC-i)b98NyJf
fGFDFUW<eLLn&Qm?5lij#=J955W|-2EnxqE+8hvDz

literal 0
HcmV?d00001

diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object
index b917067..5fde814 100755
--- a/stretch_demos/nodes/grasp_object
+++ b/stretch_demos/nodes/grasp_object
@@ -24,6 +24,8 @@ import argparse as ap
 import numpy as np
 import os
 import pickle
+import subprocess
+import pathlib
 
 import hello_helpers.hello_misc as hm
 import stretch_funmap.navigate as nv
@@ -40,6 +42,10 @@ class GraspObjectNode(hm.HelloNode):
         self.lift_position = None
         self.manipulation_view = None
         self.debug_directory = None
+        pc_pkl_fpath = str(pathlib.Path(subprocess.Popen("rospack find stretch_demos", shell=True, stdout=subprocess.PIPE).stdout.read().decode()[:-1]) / 'empty_pc.pkl')
+        with open(pc_pkl_fpath, 'rb') as inp:
+            self.empty_pc = pickle.load(inp)
+        self.surface_search_timeout = time.time()
 
     def joint_states_loop(self):
         self.wrist_position, _, _, _ = self.get_joint_state('joint_arm')
@@ -197,14 +203,23 @@ class GraspObjectNode(hm.HelloNode):
     def visualization_publish_loop(self):
         self.manip.update(self.get_point_cloud(), self._tf2_buffer)
 
-        height, width = self.manip.max_height_im.image.shape
-        robot_xy_pix = [width/2, 0]
-        surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, robot_xy_pix)
+        surface_mask = None
+        _, _, _, head_tilt_moving = self.get_joint_state('joint_head_tilt')
+        _, _, _, head_pan_moving = self.get_joint_state('joint_head_pan')
+        head_moving = head_tilt_moving or head_pan_moving
+        if not head_moving:
+            height, width = self.manip.max_height_im.image.shape
+            robot_xy_pix = [width/2, 0]
+            surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, robot_xy_pix)
         if surface_mask is not None:
-            image_copy = self.manip.max_height_im.image.copy()
-            image_copy[surface_mask <= 0] = 0
-            surface_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy)
-            self.surface_marker_pub.publish(surface_pc)
+            if time.time() > self.surface_search_timeout:
+                image_copy = self.manip.max_height_im.image.copy()
+                image_copy[surface_mask <= 0] = 0
+                surface_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy)
+                self.surface_marker_pub.publish(surface_pc)
+        else:
+            self.surface_marker_pub.publish(self.empty_pc)
+            self.surface_search_timeout = time.time() + 3.0
 
         self.manip.publish_visualizations(self.voi_marker_pub, self.mhi_marker_pub)