From 73be7deafea41a276cb29464eb7222e96e9d71d5 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 15 Sep 2023 01:25:48 -0400 Subject: [PATCH] Progress --- stretch_demos/nodes/grasp_object | 55 +++++++++++++------ .../src/stretch_funmap/max_height_image.py | 2 - 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 7ef28ed..0b7cc0f 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -23,10 +23,12 @@ import tf2_ros import argparse as ap import numpy as np import os +import pickle import hello_helpers.hello_misc as hm import stretch_funmap.navigate as nv import stretch_funmap.manipulation_planning as mp +import stretch_funmap.segment_max_height_image as sm class GraspObjectNode(hm.HelloNode): @@ -36,7 +38,7 @@ class GraspObjectNode(hm.HelloNode): self.rate = 10.0 self.joint_states = None self.joint_states_lock = threading.Lock() - self.move_base = nv.MoveBase(self) + # self.move_base = nv.MoveBase(self) self.letter_height_m = 0.2 self.wrist_position = None self.lift_position = None @@ -201,15 +203,32 @@ class GraspObjectNode(hm.HelloNode): ) def temp_trigger_grasp_object_service2(self, request): - # grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer) + with open(f'{self.debug_directory}manip.pkl', 'wb') as outp: + pickle.dump(self.manip, outp, pickle.HIGHEST_PROTOCOL) - return TriggerResponse( - success=True, - message='Completed object grasp!' - ) + 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) + + # grasp_target = self.manip.get_grasp_target(self.tf2_buffer) + grasp_target = True + + if grasp_target is not None: + return TriggerResponse( + success=True, + message='Found target!!' + ) + else: + return TriggerResponse( + success=False, + message='No grasp found...' + ) def main(self): - hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=False) + hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=True) self.debug_directory = rospy.get_param('~debug_directory') rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory)) @@ -225,23 +244,25 @@ class GraspObjectNode(hm.HelloNode): Trigger, self.temp_trigger_grasp_object_service2) - default_service = '/camera/switch_to_default_mode' - high_accuracy_service = '/camera/switch_to_high_accuracy_mode' - rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service) - rospy.wait_for_service(default_service) - rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service) - self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger) - rospy.wait_for_service(high_accuracy_service) - rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service) - self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger) + # default_service = '/camera/switch_to_default_mode' + # high_accuracy_service = '/camera/switch_to_high_accuracy_mode' + # rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service) + # rospy.wait_for_service(default_service) + # rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service) + # self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger) + # rospy.wait_for_service(high_accuracy_service) + # rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service) + # self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger) self.voi_marker_pub = rospy.Publisher( '/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( + '/grasp_object/surface_marker', PointCloud2, queue_size=1) self.manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool()) - self.manip.move_head(self.move_to_pose, head_settle_time=0.75) + self.manip.move_head(self.move_to_pose, head_settle_time=0.0) rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): diff --git a/stretch_funmap/src/stretch_funmap/max_height_image.py b/stretch_funmap/src/stretch_funmap/max_height_image.py index ba3ab3f..a40a605 100644 --- a/stretch_funmap/src/stretch_funmap/max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/max_height_image.py @@ -333,8 +333,6 @@ class MaxHeightImage: def apply_planar_correction(self, plane_parameters, plane_height_pix): # plane_parameters: [alpha, beta, gamma] such that alpha*x + beta*y + gamma = z # plane_height_m: The new height for points on the plane in meters - plane_height_pix = plane_height_pix - self.image, transform_to_corrected = numba_correct_height_image(plane_parameters, self.image, plane_height_pix) self.transform_original_to_corrected = transform_to_corrected self.transform_corrected_to_original = np.linalg.inv(transform_to_corrected)