rework grasp_point

stretch_demos/nodes/grasp_point

@@ -1,11 +1,16 @@
 #!/usr/bin/env python3
 
 import copy
+import pickle
+import pathlib
+import subprocess
 import numpy as np
 
 import rospy
+from std_msgs.msg import Empty
 from geometry_msgs.msg import PointStamped
 from sensor_msgs.msg import PointCloud2, JointState
+from visualization_msgs.msg import Marker
 import hello_helpers.hello_misc as hm
 
 import stretch_pyfunmap.navigate as nv
@@ -13,93 +18,34 @@ import stretch_pyfunmap.manipulation_planning as mp
 import stretch_pyfunmap.segment_max_height_image as sm
 
 
-class GraspObjectNode(hm.HelloNode):
+class GraspPointNode(hm.HelloNode):
 
     def __init__(self):
         hm.HelloNode.__init__(self)
         self.update_perception = True
-        self.publish_markers = True
 
+        self.surface_mask = None
         self.grasp_targets = None
-        self.is_holding_object = False
-        self.location_above_surface_m = None
-        self.placement_target = None
+        self.best_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
+        pc_pkl_fpath = str(pathlib.Path(subprocess.Popen("rospack find stretch_demos", shell=True, stdout=subprocess.PIPE).stdout.read().decode()[:-1]) / 'empty_pc.pkl')
+        with open(pc_pkl_fpath, 'rb') as inp:
+            self.empty_pc = pickle.load(inp)
 
-        # 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 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

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