Surface marker publishing successfully

stretch_demos/nodes/grasp_object

@@ -202,17 +202,21 @@ class GraspObjectNode(hm.HelloNode):
             message='Completed object grasp!'
         )
 
-    def temp_trigger_grasp_object_service2(self, request):
-        with open(f'{self.debug_directory}manip.pkl', 'wb') as outp:
-            pickle.dump(self.manip, outp, pickle.HIGHEST_PROTOCOL)
+    def visualization_publish_loop(self):
+        self.manip.update(self.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, display_on=False)
-        print(surface_mask)
-        # print(type(surface_mask))
-        # print(surface_mask.shape)
+        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)
+
+        self.manip.publish_visualizations(self.voi_marker_pub, self.mhi_marker_pub)
 
+    def temp_trigger_grasp_object_service2(self, request):
         # grasp_target = self.manip.get_grasp_target(self.tf2_buffer)
         grasp_target = True
 
@@ -258,7 +262,7 @@ class GraspObjectNode(hm.HelloNode):
             '/grasp_object/voi_marker', Marker, queue_size=1)
         self.mhi_marker_pub = rospy.Publisher(
             '/grasp_object/mhi_marker', PointCloud2, queue_size=1)
-        self.mhi_marker_pub = rospy.Publisher(
+        self.surface_marker_pub = rospy.Publisher(
             '/grasp_object/surface_marker', PointCloud2, queue_size=1)
 
         self.manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool())
@@ -266,8 +270,7 @@ class GraspObjectNode(hm.HelloNode):
 
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
-            self.manip.update(self.point_cloud, self.tf2_buffer)
-            self.manip.publish_visualizations(self.voi_marker_pub, self.mhi_marker_pub)
+            self.visualization_publish_loop()
             rate.sleep()
 
 

stretch_funmap/src/stretch_funmap/max_height_image.py

@@ -467,9 +467,9 @@ class MaxHeightImage:
         return max_height_image
 
 
-    def to_points(self, colormap=None):
-        
-        h, w = self.image.shape
+    def to_points(self, colormap=None, substitute_image=None):
+        working_image = self.image if substitute_image is None else substitute_image
+        h, w = working_image.shape
         max_num_points = w * h
         points = np.zeros((max_num_points,),
                           dtype=[
@@ -484,7 +484,7 @@ class MaxHeightImage:
         if self.transform_corrected_to_original is not None: 
             points_in_image_to_frame_id_mat = np.matmul(points_in_image_to_frame_id_mat, self.transform_corrected_to_original)
         
-        num_points = nh.numba_max_height_image_to_points(points_in_image_to_frame_id_mat, self.image, points, self.m_per_pix, self.m_per_height_unit)
+        num_points = nh.numba_max_height_image_to_points(points_in_image_to_frame_id_mat, working_image, points, self.m_per_pix, self.m_per_height_unit)
 
         points = points[:num_points]
 

stretch_funmap/src/stretch_funmap/ros_max_height_image.py

@@ -274,8 +274,8 @@ class ROSMaxHeightImage(MaxHeightImage):
             rospy.logwarn('ROSMaxHeightImage.from_rgb_points_with_tf2: failed to update the image likely due to a failure to lookup the transform using TF2. points_frame_id = {0}, points_timestamp = {1}, timeout_s = {2}'.format(points_frame_id, points_timestamp, timeout_s))
 
             
-    def to_point_cloud(self, color_map=None):
-        points = self.to_points(color_map)
+    def to_point_cloud(self, color_map=None, substitute_image=None):
+        points = self.to_points(color_map, substitute_image=substitute_image)
         if self.last_update_time is None:
             self.last_update_time = rospy.Time.now()
         point_cloud = ros_numpy.msgify(PointCloud2, points, stamp=self.last_update_time, frame_id=self.voi.frame_id)

stretch_funmap/src/stretch_funmap/segment_max_height_image.py

@@ -1027,7 +1027,7 @@ def find_floor(segment_info, segments_image, verbose=False):
         return None, None
     max_bin_value = 0.0
     floor_id = None
-    print('segment_max_height_image.py : find_floor')
+    # print('segment_max_height_image.py : find_floor')
     for i in segment_info:
         height_m = segment_info[i]['height_m']
         bin_value = segment_info[i]['bin_value']