diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface index 5873a47..97cf300 100755 --- a/stretch_demos/nodes/clean_surface +++ b/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)) diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index 3963755..be618bf 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/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