From ce7985774eccf0a31a80cf903976f2837b891e9b Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Thu, 14 Sep 2023 23:12:02 -0400 Subject: [PATCH] Create manipulation planning scene more often --- stretch_demos/nodes/grasp_object | 35 ++++++++++++------- .../stretch_funmap/manipulation_planning.py | 10 +++--- .../src/stretch_funmap/max_height_image.py | 4 +++ .../stretch_funmap/ros_max_height_image.py | 4 ++- 4 files changed, 33 insertions(+), 20 deletions(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 11d8c2f..7ef28ed 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -11,6 +11,7 @@ from control_msgs.msg import FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectoryPoint from sensor_msgs.msg import PointCloud2 +from visualization_msgs.msg import Marker, MarkerArray from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse @@ -51,12 +52,6 @@ class GraspObjectNode(hm.HelloNode): self.lift_position = lift_position self.left_finger_position, temp1, temp2 = hm.get_left_finger_state(joint_states) - def lower_tool_until_contact(self): - rospy.loginfo('lower_tool_until_contact') - trigger_request = TriggerRequest() - trigger_result = self.trigger_lower_until_contact_service(trigger_request) - rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - def look_at_surface(self, scan_time_s=None): self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool()) manip = self.manipulation_view @@ -205,6 +200,14 @@ class GraspObjectNode(hm.HelloNode): message='Completed object grasp!' ) + def temp_trigger_grasp_object_service2(self, request): + # grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer) + + return TriggerResponse( + success=True, + message='Completed object grasp!' + ) + def main(self): hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=False) @@ -218,13 +221,9 @@ class GraspObjectNode(hm.HelloNode): Trigger, self.trigger_grasp_object_callback) - rospy.wait_for_service('/funmap/trigger_reach_until_contact') - rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.') - self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger) - - rospy.wait_for_service('/funmap/trigger_lower_until_contact') - rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.') - self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger) + self.trigger_grasp_object_service2 = rospy.Service('/grasp_object/temp', + Trigger, + self.temp_trigger_grasp_object_service2) default_service = '/camera/switch_to_default_mode' high_accuracy_service = '/camera/switch_to_high_accuracy_mode' @@ -236,8 +235,18 @@ class GraspObjectNode(hm.HelloNode): 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.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) + 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) rate.sleep() diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index 4501c36..837c235 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -200,15 +200,14 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text class ManipulationView(): def __init__(self, tf2_buffer, debug_directory=None, tool='tool_stretch_gripper'): self.debug_directory = debug_directory - print('ManipulationView __init__: self.debug_directory =', self.debug_directory) + # print('ManipulationView __init__: self.debug_directory =', self.debug_directory) self.gripper_links = { 'tool_stretch_gripper': 'link_gripper', 'tool_stretch_dex_wrist': 'link_straight_gripper_aligned' } self.tool = tool - # Define the volume of interest for planning using the current - # view. + # Define the volume of interest for planning using the current view. # How far to look ahead. look_ahead_distance_m = 2.0 @@ -249,10 +248,10 @@ class ManipulationView(): voi.change_frame(points_in_old_frame_to_new_frame_mat, new_frame_id) self.voi = voi self.max_height_im = rm.ROSMaxHeightImage(self.voi, m_per_pix, pixel_dtype) - self.max_height_im.print_info() + # self.max_height_im.print_info() self.updated = False - def move_head(self, move_to_pose): + def move_head(self, move_to_pose, head_settle_time=0.5): tilt = -0.8 pan = -1.8 #-1.6 # This head configuration can reduce seeing the hand or arm when they are held high, which can avoid noise due to the hand and arm being to close to the head. @@ -260,7 +259,6 @@ class ManipulationView(): #pan = -0.9 pose = {'joint_head_pan': pan, 'joint_head_tilt': tilt} move_to_pose(pose) - head_settle_time = 0.5 rospy.sleep(head_settle_time) def estimate_reach_to_contact_distance(self, tooltip_frame, tf2_buffer, save_debugging_images=True): diff --git a/stretch_funmap/src/stretch_funmap/max_height_image.py b/stretch_funmap/src/stretch_funmap/max_height_image.py index e3faa3a..ba3ab3f 100644 --- a/stretch_funmap/src/stretch_funmap/max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/max_height_image.py @@ -94,6 +94,9 @@ class VolumeOfInterest: origin.append(1.0) origin = np.array(origin) new_origin = np.matmul(points_in_old_frame_to_new_frame_mat, origin) + new_origin = list(new_origin) + new_origin.pop() + new_origin = np.array(new_origin) # rotate the axes new_axes = np.matmul(points_in_old_frame_to_new_frame_mat[:3,:3], self.axes) # transform transforms @@ -160,6 +163,7 @@ class MaxHeightImage: # pixel_dtype : Numpy dtype for the max height image (MHI) # pixels, which also defines the maximum resolution in height. # + self.last_update_time = None self.supported_dtypes = [np.uint8, np.uint16, np.uint32, np.uint64, np.float32, np.float64] if pixel_dtype not in self.supported_dtypes: 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 38cbeea..9a073cc 100644 --- a/stretch_funmap/src/stretch_funmap/ros_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/ros_max_height_image.py @@ -73,7 +73,7 @@ class ROSVolumeOfInterest(VolumeOfInterest): marker.type = marker.CUBE marker.action = marker.ADD marker.lifetime = rospy.Duration(duration) - marker.text = 'volume of interest' + # marker.text = 'volume of interest' marker.header.frame_id = self.frame_id marker.header.stamp = rospy.Time.now() @@ -276,5 +276,7 @@ class ROSMaxHeightImage(MaxHeightImage): def to_point_cloud(self, color_map=None): points = self.to_points(color_map) + 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) return point_cloud