used to take final surface cleaning autonomy videos with and without spiderman

stretch_demos/nodes/clean_surface

@@ -96,17 +96,16 @@ class CleanSurfaceNode(hm.HelloNode):
         
     def trigger_clean_surface_callback(self, request):
 
+        tool_width_m = 0.06
+        tool_length_m = 0.06
+        min_extension_m = 0.01
+        max_extension_m = 0.5
+        
         self.look_at_surface()
-        strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer)
+        strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer, tool_width_m, tool_length_m)
 
         print('********* lift_to_surface_m = {0} **************'.format(lift_to_surface_m))
 
-        tool_width_m = 0.06 #0.03
-        #safety_from_edge = 0.06 #0.03
-        tool_length_m = 0.06 #0.03
-        min_extension_m = 0.01
-        max_extension_m = 0.5
-        
         if True and (strokes is not None) and (len(strokes) > 0):
 
             if (self.lift_position is not None) and (self.wrist_position is not None): 
@@ -127,7 +126,8 @@ class CleanSurfaceNode(hm.HelloNode):
                 initial_wrist_position = self.wrist_position 
                 extension_m = start['start_wrist_extension_m']
 
-                start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
+                #start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
+                start_extension_m = initial_wrist_position + extension_m
                 pose = {'wrist_extension': start_extension_m}
                 rospy.loginfo('Extend tool above surface.')
                 self.move_to_pose(pose)
@@ -153,8 +153,8 @@ class CleanSurfaceNode(hm.HelloNode):
                         rospy.sleep(0.2) # wait for new lift position
 
                 extension_m = start['end_wrist_extension_m']
-                end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0)
-                #end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) - safety_from_edge
+                #end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0)
+                end_extension_m = initial_wrist_position + extension_m
                 pose = {'wrist_extension': end_extension_m}
                 rospy.loginfo('Extend tool to end of surface.')
                 self.move_to_pose(pose)
@@ -171,15 +171,16 @@ class CleanSurfaceNode(hm.HelloNode):
                     self.move_to_pose(pose)
 
                     extension_m = m['start_wrist_extension_m']
-                    start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
+                    #start_extension_m = initial_wrist_position + extension_m + (tool_length_m/2.0)
+                    start_extension_m = initial_wrist_position + extension_m
                     start_extension_m = max(start_extension_m, min_extension_m)
                     pose = {'wrist_extension': start_extension_m}
                     rospy.loginfo('Extend tool above surface.')
                     self.move_to_pose(pose)
 
                     extension_m = m['end_wrist_extension_m']
-                    #end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) - safety_from_edge
-                    end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0)
+                    #end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0)
+                    end_extension_m = initial_wrist_position + extension_m
                     end_extension_m = min(end_extension_m, max_extension_m)
                     pose = {'wrist_extension': end_extension_m}
                     rospy.loginfo('Extend tool to end of surface.')

stretch_funmap/src/stretch_funmap/manipulation_planning.py

@@ -675,7 +675,7 @@ class ManipulationView():
         return grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m
     
     
-    def get_surface_wiping_plan(self, tf2_buffer):
+    def get_surface_wiping_plan(self, tf2_buffer, tool_width_m, tool_length_m):
         strokes = None
         movements = None
         surface_height_m = None
@@ -685,6 +685,9 @@ class ManipulationView():
             m_per_unit = h.m_per_height_unit
             m_per_pix = h.m_per_pix
 
+            tool_width_pix = tool_width_m / m_per_pix
+            tool_length_pix = tool_length_m / m_per_pix
+            
             wrist_frame = 'link_aruco_top_wrist'
             wrist_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(wrist_frame, tf2_buffer)
             
@@ -714,9 +717,9 @@ class ManipulationView():
                 # Dilate the obstacles to create a safety margin.
                 dilate_obstacles = True
                 if dilate_obstacles:
-                    kernel_width_pix = 3
+                    kernel_radius_pix = int(round(max(tool_width_pix, tool_length_pix)/2.0))
+                    kernel_width_pix = 1 + (2 * kernel_radius_pix)
                     iterations = 1
-                    kernel_radius_pix = (kernel_width_pix - 1) / 2
                     kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8)
                     cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1)
                     obstacle_mask = cv2.dilate(obstacle_mask, kernel, iterations=iterations)
@@ -724,9 +727,9 @@ class ManipulationView():
                 # Erode the surface to create a safety margin.
                 erode_surface = True
                 if erode_surface:
-                    kernel_width_pix = 5
+                    kernel_radius_pix = int(round(max(tool_width_pix, tool_length_pix)/2.0))
+                    kernel_width_pix = 1 + (2 * kernel_radius_pix)
                     iterations = 1
-                    kernel_radius_pix = (kernel_width_pix - 1) / 2
                     kernel = np.zeros((kernel_width_pix, kernel_width_pix), np.uint8)
                     cv2.circle(kernel, (kernel_radius_pix, kernel_radius_pix), kernel_radius_pix, 255, -1)
                     surface_mask = cv2.erode(surface_mask, kernel, iterations=iterations)