code for final(?) autonomy surface cleaning videos

stretch_demos/nodes/clean_surface

@@ -96,13 +96,14 @@ class CleanSurfaceNode(hm.HelloNode):
         
     def trigger_clean_surface_callback(self, request):
 
-        tool_width_m = 0.06
-        tool_length_m = 0.06
+        tool_width_m = 0.08 #0.06
+        tool_length_m = 0.08 #0.06
+        step_size_m = 0.04 #0.06 #0.1 #0.02
         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, tool_width_m, tool_length_m)
+        strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer, tool_width_m, tool_length_m, step_size_m)
 
         print('********* lift_to_surface_m = {0} **************'.format(lift_to_surface_m))
 

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, tool_width_m, tool_length_m):
+    def get_surface_wiping_plan(self, tf2_buffer, tool_width_m, tool_length_m, step_size_m):
         strokes = None
         movements = None
         surface_height_m = None
@@ -776,7 +776,6 @@ class ManipulationView():
                 tool_start_xy_pix = tool_current_xy_pix - (max_drive_backward_pix * robot_forward_pix)
                 tool_end_xy_pix = tool_current_xy_pix + (max_drive_forward_pix * robot_forward_pix)
 
-                step_size_m = 0.06 #0.1 #0.02
                 step_size_pix = step_size_m / m_per_pix
                 max_extension_m = 0.5
                 max_extension_pix = max_extension_m / m_per_pix