Merge pull request #69 from hello-robot/feature/missing_grasp_target_noetic

Failing gracefully in Stretch Demos

stretch_demos/nodes/grasp_object

@@ -119,103 +119,106 @@ class GraspObjectNode(hm.HelloNode):
         self.look_at_surface(scan_time_s = 3.0)
         
         grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer)
-        
-        if grasp_target is not None: 
-            pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer)
-
-            if (self.lift_position is None):
-                return TriggerResponse(
-                    success=False,
-                    message='lift position unavailable'
-                )
-
-            if actually_move:
-                rospy.loginfo('Raise tool to pregrasp height.')
-                lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
-                lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m)
-                pose = {'joint_lift': lift_to_pregrasp_m}
-                self.move_to_pose(pose)
+        if grasp_target is None:
+            return TriggerResponse(
+                success=False,
+                message='Failed to find grasp target'
+            )
 
-            pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer)
-            print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw))
-            print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi)))
+        pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer)
 
-            if actually_move:
-                rospy.loginfo('Rotate the gripper for grasping.')
-                pose = {'joint_wrist_yaw': pregrasp_yaw}
-                self.move_to_pose(pose)
-                
-                rospy.loginfo('Open the gripper.')
-                pose = {'gripper_aperture': 0.125}
-                self.move_to_pose(pose)
+        if (self.lift_position is None):
+            return TriggerResponse(
+                success=False,
+                message='lift position unavailable'
+            )
 
-            pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer)
+        if actually_move:
+            rospy.loginfo('Raise tool to pregrasp height.')
+            lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
+            lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m)
+            pose = {'joint_lift': lift_to_pregrasp_m}
+            self.move_to_pose(pose)
+
+        pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer)
+        print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw))
+        print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi)))
+
+        if actually_move:
+            rospy.loginfo('Rotate the gripper for grasping.')
+            pose = {'joint_wrist_yaw': pregrasp_yaw}
+            self.move_to_pose(pose)
             
-            print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m))
-            print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m))
-
-            if actually_move:
-                rospy.loginfo('Drive to pregrasp location.')
-                self.drive(pregrasp_mobile_base_m)
-
-                if pregrasp_wrist_extension_m > 0.0:
-                    extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m)
-                    extension_m = min(extension_m, max_extension_m)
-                    rospy.loginfo('Extend tool above surface.')
-                    pose = {'wrist_extension': extension_m} 
-                    self.move_to_pose(pose)
-                else:
-                    print('negative wrist extension for pregrasp, so not extending or retracting.')
-
-            grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
-            print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m))
-
-            if actually_move: 
-                rospy.loginfo('Move the grasp pose from the pregrasp pose.')
-
-                lift_m = max(self.lift_position + grasp_lift_m, 0.1)
-                lift_m = min(lift_m, max_lift_m)
-                
-                extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m)
+            rospy.loginfo('Open the gripper.')
+            pose = {'gripper_aperture': 0.125}
+            self.move_to_pose(pose)
+
+        pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer)
+
+        print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m))
+        print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m))
+
+        if actually_move:
+            rospy.loginfo('Drive to pregrasp location.')
+            self.drive(pregrasp_mobile_base_m)
+
+            if pregrasp_wrist_extension_m > 0.0:
+                extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m)
                 extension_m = min(extension_m, max_extension_m)
-                
-                pose = {'translate_mobile_base': grasp_mobile_base_m,
-                        'joint_lift': lift_m,  
-                        'wrist_extension': extension_m}
+                rospy.loginfo('Extend tool above surface.')
+                pose = {'wrist_extension': extension_m}
                 self.move_to_pose(pose)
+            else:
+                print('negative wrist extension for pregrasp, so not extending or retracting.')
 
-                rospy.loginfo('Attempt to close the gripper on the object.')
-                gripper_aperture_m = grasp_target['width_m'] - 0.18
-                pose = {'gripper_aperture': gripper_aperture_m}
-                self.move_to_pose(pose)
-                
-                # Lifting appears to happen before the gripper has
-                # finished unless there is this sleep. Need to look
-                # into this issue.
-                rospy.sleep(3.0)
-
-                rospy.loginfo('Attempt to lift the object.')
-                object_lift_height_m = 0.1
-
-                lift_m = max(self.lift_position + object_lift_height_m, 0.2)
-                lift_m = min(lift_m, max_lift_m)
-                
-                pose = {'joint_lift': lift_m}
-                self.move_to_pose(pose)
+        grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
+        print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m))
 
-                rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.')
-                pose = {'gripper_aperture': gripper_aperture_m + 0.005}
-                self.move_to_pose(pose)
+        if actually_move:
+            rospy.loginfo('Move the grasp pose from the pregrasp pose.')
 
+            lift_m = max(self.lift_position + grasp_lift_m, 0.1)
+            lift_m = min(lift_m, max_lift_m)
 
-            if actually_move:
-                rospy.loginfo('Retract the tool.')
-                pose = {'wrist_extension': 0.01}
-                self.move_to_pose(pose)
+            extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m)
+            extension_m = min(extension_m, max_extension_m)
 
-                rospy.loginfo('Reorient the wrist.')
-                pose = {'joint_wrist_yaw': 0.0}
-                self.move_to_pose(pose)
+            pose = {'translate_mobile_base': grasp_mobile_base_m,
+                    'joint_lift': lift_m,
+                    'wrist_extension': extension_m}
+            self.move_to_pose(pose)
+
+            rospy.loginfo('Attempt to close the gripper on the object.')
+            gripper_aperture_m = grasp_target['width_m'] - 0.18
+            pose = {'gripper_aperture': gripper_aperture_m}
+            self.move_to_pose(pose)
+
+            # Lifting appears to happen before the gripper has
+            # finished unless there is this sleep. Need to look
+            # into this issue.
+            rospy.sleep(3.0)
+
+            rospy.loginfo('Attempt to lift the object.')
+            object_lift_height_m = 0.1
+
+            lift_m = max(self.lift_position + object_lift_height_m, 0.2)
+            lift_m = min(lift_m, max_lift_m)
+
+            pose = {'joint_lift': lift_m}
+            self.move_to_pose(pose)
+
+            rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.')
+            pose = {'gripper_aperture': gripper_aperture_m + 0.005}
+            self.move_to_pose(pose)
+
+        if actually_move:
+            rospy.loginfo('Retract the tool.')
+            pose = {'wrist_extension': 0.01}
+            self.move_to_pose(pose)
+
+            rospy.loginfo('Reorient the wrist.')
+            pose = {'joint_wrist_yaw': 0.0}
+            self.move_to_pose(pose)
 
         return TriggerResponse(
             success=True,

stretch_demos/nodes/handover_object

@@ -163,12 +163,12 @@ class HandoverObjectNode(hm.HelloNode):
 
             
     def trigger_handover_object_callback(self, request):
-        with self.move_lock: 
+        with self.move_lock:
             # First, retract the wrist in preparation for handing out an object.
             pose = {'wrist_extension': 0.005}
             self.move_to_pose(pose)
 
-            if self.handover_goal_ready: 
+            if self.handover_goal_ready:
                 pose = {'joint_lift': self.lift_goal_m}
                 self.move_to_pose(pose)
                 tolerance_distance_m = 0.01
@@ -177,9 +177,14 @@ class HandoverObjectNode(hm.HelloNode):
                 self.move_to_pose(pose)
                 self.handover_goal_ready = False
 
-            return TriggerResponse(
-                success=True,
-                message='Completed object handover!'
+                return TriggerResponse(
+                    success=True,
+                    message='Completed object handover!'
+                )
+            else:
+                return TriggerResponse(
+                    success=False,
+                    message='Failed to find handover goal'
                 )
 
     

stretch_funmap/src/stretch_funmap/segment_max_height_image.py

@@ -15,7 +15,7 @@ from stretch_funmap.numba_height_image import numba_create_segment_image_uint8
 import hello_helpers.fit_plane as fp
 
 
-def find_object_to_grasp(height_image, display_on=False): 
+def find_object_to_grasp(height_image, display_on=False):
     h_image = height_image.image
     m_per_unit = height_image.m_per_height_unit
     m_per_pix = height_image.m_per_pix
@@ -251,9 +251,11 @@ def find_closest_flat_surface(height_image, robot_xy_pix, display_on=False):
     segments_image, segment_info, height_to_segment_id = segment(image, m_per_unit, zero_height, segmentation_scale, verbose=False)
     if segment_info is None:
         return None, None
-        
+
     floor_id, floor_mask = find_floor(segment_info, segments_image, verbose=False)
-    
+    if floor_mask is None:
+        return None, None
+
     remove_floor = True
     if remove_floor:
         segments_image[floor_mask > 0] = 0
@@ -1030,15 +1032,9 @@ def find_floor(segment_info, segments_image, verbose=False):
         height_m = segment_info[i]['height_m']
         bin_value = segment_info[i]['bin_value']
 
-        # Old values changed on June 3, 2021 due to no floor segment
-        # being within the bounds, which resulted in an error.
-        
-        #min_floor_m = -0.02
-        #max_floor_m = 0.1
-
-        # New values set on June 3, 2021.
-        min_floor_m = -0.05
-        max_floor_m = 0.05
+        # New values set on June 14, 2022.
+        min_floor_m = -0.1
+        max_floor_m = 0.1
 
         if verbose: 
             print('segment = {0}, histogram bin_value = {1}, height_m = {2}, min_floor_m = {3}, max_floor_m = {4}'.format(i, bin_value, height_m, min_floor_m, max_floor_m))
@@ -1061,8 +1057,7 @@ def find_floor(segment_info, segments_image, verbose=False):
             print('floor_id =', floor_id)
             print('max_bin_value =', max_bin_value)
     else:
-        if verbose: 
-            print('segment_max_height_image.py : no floor segment found...')
+        print('segment_max_height_image.py : no floor segment found...')
     return floor_id, floor_mask