From 5eb73d1cecfdbcdfa0c6bbff9b5b7f7f7b55a514 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 15 Sep 2023 01:58:33 -0400 Subject: [PATCH] Surface marker publishing successfully --- stretch_demos/nodes/grasp_object | 23 +++++++++++-------- .../src/stretch_funmap/max_height_image.py | 8 +++---- .../stretch_funmap/ros_max_height_image.py | 4 ++-- .../segment_max_height_image.py | 2 +- 4 files changed, 20 insertions(+), 17 deletions(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 0b7cc0f..2381f6a 100755 --- a/stretch_demos/nodes/grasp_object +++ b/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() diff --git a/stretch_funmap/src/stretch_funmap/max_height_image.py b/stretch_funmap/src/stretch_funmap/max_height_image.py index a40a605..2d2bcf8 100644 --- a/stretch_funmap/src/stretch_funmap/max_height_image.py +++ b/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] diff --git a/stretch_funmap/src/stretch_funmap/ros_max_height_image.py b/stretch_funmap/src/stretch_funmap/ros_max_height_image.py index 9a073cc..f2a048c 100644 --- a/stretch_funmap/src/stretch_funmap/ros_max_height_image.py +++ b/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) diff --git a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py index 519736c..a270d91 100755 --- a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py +++ b/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']