diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface index 21ce302..5873a47 100755 --- a/stretch_demos/nodes/clean_surface +++ b/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.') diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index cef959a..3963755 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): + 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)