place point works

stretch_demos/nodes/grasp_point

@@ -9,11 +9,10 @@ import numpy as np
 import rospy
 from std_msgs.msg import Empty, Float32
 from geometry_msgs.msg import PointStamped
-from sensor_msgs.msg import PointCloud2, JointState
+from sensor_msgs.msg import PointCloud2
 from visualization_msgs.msg import Marker
 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
 

stretch_demos/nodes/place_point

@@ -1,142 +1,50 @@
 #!/usr/bin/env python3
 
 import copy
+import pickle
+import pathlib
+import subprocess
 import numpy as np
 
 import rospy
+from std_msgs.msg import Empty, Float32
 from geometry_msgs.msg import PointStamped
-from sensor_msgs.msg import PointCloud2, JointState
+from sensor_msgs.msg import PointCloud2
 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):
+class PlacePointNode(hm.HelloNode):
 
     def __init__(self):
         hm.HelloNode.__init__(self)
-        self.update_perception = True
-        self.publish_markers = True
+        self.active = False
+        self.update_perception = False
 
-        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})
+        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)
 
-        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 enable_callback(self, msg):
+        self.location_above_surface_m = msg.data
+        print(self.location_above_surface_m)
+        self.placement_target = None
+        self.update_perception = True
+        self.active = True
 
-    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
+    def disable_callback(self, msg):
+        self.active = False
         self.update_perception = False
+        self.location_above_surface_m = None
+        self.placement_target = None
 
-        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 clicked_point_callback(self, clicked_point_msg):
+        self.place_object()
 
     def place_point_callback(self, point_msg):
         self.place_object()
@@ -148,45 +56,13 @@ class GraspObjectNode(hm.HelloNode):
         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.manip.perform_cartesian_grasp(target, self, tooltip_frame='link_grasp_center_real', placing=True)
+        msg = Empty()
+        self.result_pub.publish(msg)
 
         self.location_above_surface_m = None
-        self.is_holding_object = False
-        self.update_perception = True
+        self.update_perception = False
+        self.active = False
 
     def perceive_objects(self):
         if not self.update_perception:
@@ -197,72 +73,42 @@ class GraspObjectNode(hm.HelloNode):
         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']
+        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
+        self.placement_target = placement_target
 
-                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
+    def publish_markers(self):
+        if self.placement_target is not None:
+            image_copy = self.manip.max_height_im.image.copy()
+            image_copy[self.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_target_marker_pub.publish(surface_placement_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
+            self.surface_target_marker_pub.publish(self.empty_pc)
 
-            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 main(self):
+        hm.HelloNode.main(self, 'place_point', 'place_point', wait_for_first_pointcloud=True)
+        self.surface_target_marker_pub = rospy.Publisher('/place_point/surface_target_marker', PointCloud2, queue_size=1)
 
-    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]
+        self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback)
+        self.trigger_sub = rospy.Subscriber('/place_point/trigger_place_point', PointStamped, self.place_point_callback)
+        self.enable_sub = rospy.Subscriber('/place_point/enable', Float32, self.enable_callback)
+        self.disable_sub = rospy.Subscriber('/place_point/disable', Empty, self.disable_callback)
+        self.result_pub = rospy.Publisher('/place_point/result', Empty, queue_size=1)
 
-    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()
+            self.publish_markers()
             rate.sleep()
 
 
 if __name__ == "__main__":
     try:
-        node = GraspObjectNode()
+        node = PlacePointNode()
         node.main()
     except KeyboardInterrupt:
         pass