From bfcb977bcdc3f781a6259bfad7b8b582a4092302 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 29 Sep 2023 02:39:08 -0700 Subject: [PATCH] rework grasp_point --- stretch_demos/empty_pc.pkl | Bin 0 -> 280 bytes stretch_demos/nodes/grasp_point | 267 +++++++++++-------------------- stretch_demos/nodes/place_point | 268 ++++++++++++++++++++++++++++++++ 3 files changed, 360 insertions(+), 175 deletions(-) create mode 100644 stretch_demos/empty_pc.pkl create mode 100644 stretch_demos/nodes/place_point 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`vpIwG{tC24>vMz@)XU+DX~*D zdPIs#Qjm0bq$Z}M7ES44gEC<1yg7S#i}H&LD)oS5NoH>9lpdB45Cv175Dv_=9>$6(-VEOC-i)b98NyJf fGFDFUW 0: - at_goal = self.move_base.forward(delta_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m, floor_tf='map') - else: - at_goal = self.move_base.backward(delta_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m, floor_tf='map') - return at_goal - - def grasp_object(self, target): - target['elongated'] = False - pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer) - lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) - lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1) - self.move_to_pose({'joint_lift': lift_to_pregrasp_m}) - - if self.tool == "tool_stretch_dex_wrist": - self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}) - - pregrasp_yaw = self.manip.get_pregrasp_yaw(target, self.tf2_buffer) - self.move_to_pose({'joint_wrist_yaw': pregrasp_yaw + self.yaw_offset}) - - self.move_to_pose({'gripper_aperture': 0.125}) - - pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer) - self.drive_base(pregrasp_base_m + self.mb_offset) - - arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01) - arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5) - self.move_to_pose({'wrist_extension': arm_to_pregrasp_m}) - - rospy.sleep(1) - - grasp_base_m, grasp_lift_m, grasp_arm_m = self.manip.get_grasp_from_pregrasp(target, self.tf2_buffer) - lift_to_grasp_m = max(self.lift_position + grasp_lift_m, 0.1) - lift_to_grasp_m = min(lift_to_grasp_m, 1.1) - arm_to_grasp_m = max(self.arm_position + grasp_arm_m, 0.01) - arm_to_grasp_m = min(arm_to_grasp_m, 0.5) - if self.tool == "tool_stretch_dex_wrist": - lift_to_grasp_m -= 0.03 - arm_to_grasp_m += 0.02 - self.move_to_pose({'translate_mobile_base': grasp_base_m + self.mb_offset, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m}) - - gripper_to_grasp_m = target['width_m'] - 0.18 - self.move_to_pose({'gripper_aperture': gripper_to_grasp_m}) - - rospy.sleep(1) - - self.move_to_pose({'joint_lift': lift_to_grasp_m + 0.2}) - - self.location_above_surface_m = target['location_above_surface_m'] - self.is_holding_object = True + def reset_callback(self, msg): + self.surface_mask = None + self.grasp_targets = None + self.best_target = None def grasp_point_callback(self, point_msg): self.grasp_point(point_msg) - def grasp_clicked_point_callback(self, clicked_point_msg): - if not self.is_holding_object: - self.grasp_point(clicked_point_msg) - else: - self.place_object() + def clicked_point_callback(self, clicked_point_msg): + self.grasp_point(clicked_point_msg) def grasp_point(self, point_msg): if self.grasp_targets is None: - print('cannot assign clicked point to object because no grasp targets available') + print('cannot assign point to object because no grasp targets available') return self.update_perception = False @@ -128,64 +74,10 @@ class GraspObjectNode(hm.HelloNode): print('couldnt find grasp target close to point') self.update_perception = True return + self.best_target = best_target - if self.publish_markers: # TODO: doesn't always appear in Rviz - image_copy = self.manip.max_height_im.image.copy() - image_copy[best_target['object_selector'] <= 0] = 0 - target_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) - self.grasp_target_marker_pub.publish(target_pc) - - self.grasp_object(best_target) - self.update_perception = True - - def place_point_callback(self, point_msg): - self.place_object() - - def place_object(self): - if self.placement_target is None: - print('cannot place object on surface because no placement targets are available') - return - self.update_perception = False - - target = copy.copy(self.placement_target) - pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer) - lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) - lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1) - self.move_to_pose({'joint_lift': lift_to_pregrasp_m}) - - if self.tool == "tool_stretch_dex_wrist": - self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}) - - pregrasp_yaw = self.manip.get_pregrasp_yaw(target, self.tf2_buffer) - self.move_to_pose({'joint_wrist_yaw': pregrasp_yaw}) - - pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer) - self.drive_base(pregrasp_base_m) - - arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01) - arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5) - self.move_to_pose({'wrist_extension': arm_to_pregrasp_m}) - - rospy.sleep(1) - - grasp_base_m, grasp_lift_m, grasp_arm_m = self.manip.get_grasp_from_pregrasp(target, self.tf2_buffer) - lift_to_grasp_m = max(self.lift_position + grasp_lift_m, 0.1) - lift_to_grasp_m = min(lift_to_grasp_m, 1.1) - arm_to_grasp_m = max(self.arm_position + grasp_arm_m, 0.01) - arm_to_grasp_m = min(arm_to_grasp_m, 0.5) - if self.tool == "tool_stretch_dex_wrist": - lift_to_grasp_m -= 0.03 - arm_to_grasp_m += 0.02 - self.move_to_pose({'translate_mobile_base': grasp_base_m, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m}) - - self.move_to_pose({'gripper_aperture': 0.125}) - - rospy.sleep(1) - - self.move_to_pose({'joint_lift': lift_to_grasp_m + 0.2}) - - self.location_above_surface_m = None - self.is_holding_object = False + rospy.sleep(10) + # self.manip.perform_cartesian_grasp(best_target) self.update_perception = True def perceive_objects(self): @@ -196,73 +88,98 @@ class GraspObjectNode(hm.HelloNode): surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, None) if surface_mask is None: return + self.surface_mask = surface_mask + + grasp_targets = sm.find_objects_to_grasp(surface_mask, plane_parameters, self.manip.max_height_im) + if len(grasp_targets) == 0: + return + self.grasp_targets = grasp_targets - if self.publish_markers: + def publish_markers(self): + if self.surface_mask is not None: image_copy = self.manip.max_height_im.image.copy() - image_copy[surface_mask <= 0] = 0 + image_copy[self.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) - if not self.is_holding_object: - grasp_targets = sm.find_objects_to_grasp(surface_mask, plane_parameters, self.manip.max_height_im) - if len(grasp_targets) == 0: - return + if self.grasp_targets is not None: + grasp_targets = self.grasp_targets + objects_mask = grasp_targets[0]['object_selector'].copy() + for target in grasp_targets: + objects_mask = objects_mask | target['object_selector'] - if self.publish_markers: - objects_mask = grasp_targets[0]['object_selector'].copy() - for target in grasp_targets: - objects_mask = objects_mask | target['object_selector'] + if self.best_target is not None: + not_best_target_mask = ~ self.best_target['object_selector'].copy() + objects_mask = objects_mask & not_best_target_mask - image_copy = self.manip.max_height_im.image.copy() - image_copy[objects_mask <= 0] = 0 - objects_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) - self.objects_marker_pub.publish(objects_pc) - self.grasp_targets = grasp_targets + image_copy = self.manip.max_height_im.image.copy() + image_copy[objects_mask <= 0] = 0 + objects_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.objects_marker_pub.publish(objects_pc) else: - placement_target = sm.find_placement_target_on_surface(surface_mask, self.location_above_surface_m, self.manip.max_height_im) - if placement_target is None: - return - - if self.publish_markers: - image_copy = self.manip.max_height_im.image.copy() - image_copy[placement_target['surface_convex_hull_mask'] <= 0] = 0 - surface_placement_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) - self.surface_placement_marker_pub.publish(surface_placement_pc) - self.placement_target = placement_target + self.objects_marker_pub.publish(self.empty_pc) - def joint_states_callback(self, joint_states): - self.arm_position, _, _ = hm.get_wrist_state(joint_states) - self.lift_position, _, _ = hm.get_lift_state(joint_states) - self.left_finger_position, _, _ = hm.get_left_finger_state(joint_states) - i = joint_states.name.index('joint_wrist_yaw') - self.yaw_position = joint_states.position[i] - i = joint_states.name.index('joint_wrist_pitch') - self.pitch_position = joint_states.position[i] - i = joint_states.name.index('joint_wrist_roll') - self.roll_position = joint_states.position[i] + if self.best_target is not None: + image_copy = self.manip.max_height_im.image.copy() + image_copy[self.best_target['object_selector'] <= 0] = 0 + surface_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.gtpc_marker_pub.publish(surface_pc) + + marker = Marker() + marker.header.stamp = rospy.Time.now() + marker.header.frame_id = 'map' + marker.type = marker.SPHERE + marker.action = marker.ADD + marker.lifetime = rospy.Duration(0.2) + x_voi = self.best_target['location_xy_pix'][0] * self.manip.max_height_im.m_per_pix + y_voi = self.best_target['location_xy_pix'][1] * self.manip.max_height_im.m_per_pix + z_voi = self.best_target['location_z_pix'] * self.manip.max_height_im.m_per_height_unit + point_in_voi = np.array([x_voi, y_voi, z_voi, 1.0]) + point_in_world_frame = np.matmul(self.manip.max_height_im.voi.points_in_voi_to_frame_id_mat, point_in_voi) + marker.pose.position.x = point_in_world_frame[0] + marker.pose.position.y = point_in_world_frame[1] + marker.pose.position.z = point_in_world_frame[2] + marker.pose.orientation.w = 1.0 + marker.scale.x = 0.05 + marker.scale.y = 0.05 + marker.scale.z = 0.05 + marker.color.r = 0.0 + marker.color.g = 0.1 + marker.color.b = 0.9 + marker.color.a = 1.0 + # self.gtaxes_marker_pub.publish(marker) + else: + self.gtpc_marker_pub.publish(self.empty_pc) + marker = Marker() + marker.action = marker.DELETEALL + self.gtaxes_marker_pub.publish(marker) def main(self): - hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=True) - self.surface_marker_pub = rospy.Publisher('/grasp_object/surface_marker', PointCloud2, queue_size=1) - self.surface_placement_marker_pub = rospy.Publisher('/grasp_object/surface_placement_marker', PointCloud2, queue_size=1) - self.objects_marker_pub = rospy.Publisher('/grasp_object/objects_marker', PointCloud2, queue_size=1) - self.grasp_target_marker_pub = rospy.Publisher('/grasp_object/grasp_target_marker', PointCloud2, queue_size=1) - self.grasp_clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.grasp_clicked_point_callback) - self.joint_states_sub = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) - self.grasp_point_sub = rospy.Subscriber('/grasp_object/grasp_point', PointStamped, self.grasp_point_callback) - self.place_point_sub = rospy.Subscriber('/grasp_object/place_point', PointStamped, self.place_point_callback) + hm.HelloNode.main(self, 'grasp_point', 'grasp_point', wait_for_first_pointcloud=True) + self.surface_marker_pub = rospy.Publisher('/grasp_point/surface_marker', PointCloud2, queue_size=1) + self.objects_marker_pub = rospy.Publisher('/grasp_point/objects_marker', PointCloud2, queue_size=1) + self.gtpc_marker_pub = rospy.Publisher('/grasp_point/gtpc_marker', PointCloud2, queue_size=1) + self.gtaxes_marker_pub = rospy.Publisher('/grasp_point/gtaxes_marker', Marker, queue_size=1) + + self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback) + self.trigger_sub = rospy.Subscriber('/grasp_point/trigger_grasp_point', PointStamped, self.grasp_point_callback) + self.reset_sub = rospy.Subscriber('/grasp_point/reset', Empty, self.reset_callback) + self.result_pub = rospy.Publisher('/grasp_point/result', Empty, queue_size=1) + self.manip = mp.ManipulationView(self.tf2_buffer, None, self.get_tool()) - self.move_base = nv.MoveBase(self) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): self.perceive_objects() + self.publish_markers() rate.sleep() if __name__ == "__main__": try: - node = GraspObjectNode() + node = GraspPointNode() node.main() except KeyboardInterrupt: pass diff --git a/stretch_demos/nodes/place_point b/stretch_demos/nodes/place_point new file mode 100644 index 0000000..41fd913 --- /dev/null +++ b/stretch_demos/nodes/place_point @@ -0,0 +1,268 @@ +#!/usr/bin/env python3 + +import copy +import numpy as np + +import rospy +from geometry_msgs.msg import PointStamped +from sensor_msgs.msg import PointCloud2, JointState +import hello_helpers.hello_misc as hm + +import stretch_pyfunmap.navigate as nv +import stretch_pyfunmap.manipulation_planning as mp +import stretch_pyfunmap.segment_max_height_image as sm + + +class GraspObjectNode(hm.HelloNode): + + def __init__(self): + hm.HelloNode.__init__(self) + self.update_perception = True + self.publish_markers = True + + self.grasp_targets = None + self.is_holding_object = False + self.location_above_surface_m = None + self.placement_target = None + + self.arm_position = None + self.lift_position = None + self.left_finger_position = None + self.yaw_position = None + self.pitch_position = None + self.roll_position = None + + # TODO: investigate a better place for these to exist + self.yaw_offset = 0.09 + self.mb_offset = 0.02 + + def drive_base(self, delta_m): + tolerance_distance_m = 0.005 + if delta_m > 0: + at_goal = self.move_base.forward(delta_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m, floor_tf='map') + else: + at_goal = self.move_base.backward(delta_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m, floor_tf='map') + return at_goal + + def grasp_object(self, target): + target['elongated'] = False + pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer) + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1) + self.move_to_pose({'joint_lift': lift_to_pregrasp_m}) + + if self.tool == "tool_stretch_dex_wrist": + self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}) + + pregrasp_yaw = self.manip.get_pregrasp_yaw(target, self.tf2_buffer) + self.move_to_pose({'joint_wrist_yaw': pregrasp_yaw + self.yaw_offset}) + + self.move_to_pose({'gripper_aperture': 0.125}) + + pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer) + self.drive_base(pregrasp_base_m + self.mb_offset) + + arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01) + arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5) + self.move_to_pose({'wrist_extension': arm_to_pregrasp_m}) + + rospy.sleep(1) + + grasp_base_m, grasp_lift_m, grasp_arm_m = self.manip.get_grasp_from_pregrasp(target, self.tf2_buffer) + lift_to_grasp_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_to_grasp_m = min(lift_to_grasp_m, 1.1) + arm_to_grasp_m = max(self.arm_position + grasp_arm_m, 0.01) + arm_to_grasp_m = min(arm_to_grasp_m, 0.5) + if self.tool == "tool_stretch_dex_wrist": + lift_to_grasp_m -= 0.03 + arm_to_grasp_m += 0.02 + self.move_to_pose({'translate_mobile_base': grasp_base_m + self.mb_offset, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m}) + + gripper_to_grasp_m = target['width_m'] - 0.18 + self.move_to_pose({'gripper_aperture': gripper_to_grasp_m}) + + rospy.sleep(1) + + self.move_to_pose({'joint_lift': lift_to_grasp_m + 0.2}) + + self.location_above_surface_m = target['location_above_surface_m'] + self.is_holding_object = True + + def grasp_point_callback(self, point_msg): + self.grasp_point(point_msg) + + def grasp_clicked_point_callback(self, clicked_point_msg): + if not self.is_holding_object: + self.grasp_point(clicked_point_msg) + else: + self.place_object() + + def grasp_point(self, point_msg): + if self.grasp_targets is None: + print('cannot assign clicked point to object because no grasp targets available') + return + self.update_perception = False + + points_to_image_mat, _ = self.manip.max_height_im.get_points_to_image_mat(point_msg.header.frame_id, self.tf2_buffer) + p_x = point_msg.point.x + p_y = point_msg.point.y + p_z = point_msg.point.z + point_xyz = np.array([p_x, p_y, p_z, 1.0]) + point_pix = np.matmul(points_to_image_mat, point_xyz)[:3] + i_x, i_y, _ = point_pix + + h, w = self.manip.max_height_im.image.shape + if not (i_x >= 0 and i_x < w and i_y >= 0 and i_y < h): + print('point does not fall within the max height image') + self.update_perception = True + return + + min_distance = float('inf') + best_target = None + for target in self.grasp_targets: + dist = np.sqrt((i_x - target['location_xy_pix'][0]) ** 2 + (i_y - target['location_xy_pix'][1]) ** 2) + if dist < min_distance: + min_distance = dist + best_target = target + if best_target is None: + print('couldnt find grasp target close to point') + self.update_perception = True + return + + if self.publish_markers: # TODO: doesn't always appear in Rviz + image_copy = self.manip.max_height_im.image.copy() + image_copy[best_target['object_selector'] <= 0] = 0 + target_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.grasp_target_marker_pub.publish(target_pc) + + self.grasp_object(best_target) + self.update_perception = True + + def place_point_callback(self, point_msg): + self.place_object() + + def place_object(self): + if self.placement_target is None: + print('cannot place object on surface because no placement targets are available') + return + self.update_perception = False + + target = copy.copy(self.placement_target) + pregrasp_lift_m = self.manip.get_pregrasp_lift(target, self.tf2_buffer) + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, 1.1) + self.move_to_pose({'joint_lift': lift_to_pregrasp_m}) + + if self.tool == "tool_stretch_dex_wrist": + self.move_to_pose({'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}) + + pregrasp_yaw = self.manip.get_pregrasp_yaw(target, self.tf2_buffer) + self.move_to_pose({'joint_wrist_yaw': pregrasp_yaw}) + + pregrasp_base_m, pregrasp_arm_m = self.manip.get_pregrasp_planar_translation(target, self.tf2_buffer) + self.drive_base(pregrasp_base_m) + + arm_to_pregrasp_m = max(self.arm_position + pregrasp_arm_m, 0.01) + arm_to_pregrasp_m = min(arm_to_pregrasp_m, 0.5) + self.move_to_pose({'wrist_extension': arm_to_pregrasp_m}) + + rospy.sleep(1) + + grasp_base_m, grasp_lift_m, grasp_arm_m = self.manip.get_grasp_from_pregrasp(target, self.tf2_buffer) + lift_to_grasp_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_to_grasp_m = min(lift_to_grasp_m, 1.1) + arm_to_grasp_m = max(self.arm_position + grasp_arm_m, 0.01) + arm_to_grasp_m = min(arm_to_grasp_m, 0.5) + if self.tool == "tool_stretch_dex_wrist": + lift_to_grasp_m -= 0.03 + arm_to_grasp_m += 0.02 + self.move_to_pose({'translate_mobile_base': grasp_base_m, 'joint_lift': lift_to_grasp_m, 'wrist_extension': arm_to_grasp_m}) + + self.move_to_pose({'gripper_aperture': 0.125}) + + rospy.sleep(1) + + self.move_to_pose({'joint_lift': lift_to_grasp_m + 0.2}) + + self.location_above_surface_m = None + self.is_holding_object = False + self.update_perception = True + + def perceive_objects(self): + if not self.update_perception: + return + + self.manip.update(self.point_cloud, self.tf2_buffer) + surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, None) + if surface_mask is None: + return + + if self.publish_markers: + 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 not self.is_holding_object: + grasp_targets = sm.find_objects_to_grasp(surface_mask, plane_parameters, self.manip.max_height_im) + if len(grasp_targets) == 0: + return + + if self.publish_markers: + objects_mask = grasp_targets[0]['object_selector'].copy() + for target in grasp_targets: + objects_mask = objects_mask | target['object_selector'] + + image_copy = self.manip.max_height_im.image.copy() + image_copy[objects_mask <= 0] = 0 + objects_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.objects_marker_pub.publish(objects_pc) + self.grasp_targets = grasp_targets + else: + placement_target = sm.find_placement_target_on_surface(surface_mask, self.location_above_surface_m, self.manip.max_height_im) + if placement_target is None: + return + + if self.publish_markers: + image_copy = self.manip.max_height_im.image.copy() + image_copy[placement_target['surface_convex_hull_mask'] <= 0] = 0 + surface_placement_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy) + self.surface_placement_marker_pub.publish(surface_placement_pc) + self.placement_target = placement_target + + def joint_states_callback(self, joint_states): + self.arm_position, _, _ = hm.get_wrist_state(joint_states) + self.lift_position, _, _ = hm.get_lift_state(joint_states) + self.left_finger_position, _, _ = hm.get_left_finger_state(joint_states) + i = joint_states.name.index('joint_wrist_yaw') + self.yaw_position = joint_states.position[i] + i = joint_states.name.index('joint_wrist_pitch') + self.pitch_position = joint_states.position[i] + i = joint_states.name.index('joint_wrist_roll') + self.roll_position = joint_states.position[i] + + def main(self): + hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=True) + self.surface_marker_pub = rospy.Publisher('/grasp_object/surface_marker', PointCloud2, queue_size=1) + self.surface_placement_marker_pub = rospy.Publisher('/grasp_object/surface_placement_marker', PointCloud2, queue_size=1) + self.objects_marker_pub = rospy.Publisher('/grasp_object/objects_marker', PointCloud2, queue_size=1) + self.grasp_target_marker_pub = rospy.Publisher('/grasp_object/grasp_target_marker', PointCloud2, queue_size=1) + self.grasp_clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.grasp_clicked_point_callback) + self.joint_states_sub = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) + self.grasp_point_sub = rospy.Subscriber('/grasp_object/grasp_point', PointStamped, self.grasp_point_callback) + self.place_point_sub = rospy.Subscriber('/grasp_object/place_point', PointStamped, self.place_point_callback) + self.manip = mp.ManipulationView(self.tf2_buffer, None, self.get_tool()) + self.move_base = nv.MoveBase(self) + + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + self.perceive_objects() + rate.sleep() + + +if __name__ == "__main__": + try: + node = GraspObjectNode() + node.main() + except KeyboardInterrupt: + pass