cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

object grasping update for noetic 

After these changes, object grasping with the following command
worked for me. Stretch grasped a cup from a small nightstand.

$ roslaunch stretch_demos grasp_object.launch

Changes include the following:

+ / => // for some places where an integer output is required

+ changes for scikit_image (import skimage) 0.18.1
  + skimage.measure.label argument change
  + skimage.measure.regionprops argument change
pull/24/head
Charlie Kemp 3 years ago
parent
066e57edff
commit
4e29f5d054
2 changed files with 33 additions and 17 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +2
    -2
      stretch_demos/nodes/grasp_object
  2. +31
    -15
      stretch_funmap/src/stretch_funmap/segment_max_height_image.py

+ 2
- 2
stretch_demos/nodes/grasp_object View File

@ -1,6 +1,6 @@
#!/usr/bin/env python
#!/usr/bin/env python3
from __future__ import print_function
#from __future__ import print_function
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist

+ 31
- 15
stretch_funmap/src/stretch_funmap/segment_max_height_image.py View File

@ -63,16 +63,20 @@ def find_object_to_grasp(height_image, display_on=False):
if display_on:
rgb_image = height_image.rgb_image.copy()
rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0]
rgb_image[obstacle_selector] = (rgb_image[obstacle_selector]/2) + [0, 0, 127]
#rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0]
#rgb_image[obstacle_selector] = (rgb_image[obstacle_selector]/2) + [0, 0, 127]
rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]//2) + [0, 127, 0]
rgb_image[obstacle_selector] = (rgb_image[obstacle_selector]//2) + [0, 0, 127]
cv2.imshow('obstacles', rgb_image)
obstacle_mask = np.uint8(obstacle_selector)
if display_on:
rgb_image = height_image.rgb_image.copy()
rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0]
rgb_image[obstacle_mask > 0] = (rgb_image[obstacle_mask > 0]/2) + [0, 0, 127]
#rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0]
#rgb_image[obstacle_mask > 0] = (rgb_image[obstacle_mask > 0]/2) + [0, 0, 127]
rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]//2) + [0, 127, 0]
rgb_image[obstacle_mask > 0] = (rgb_image[obstacle_mask > 0]//2) + [0, 0, 127]
# Find the convex hull of the surface points to represent the full
# surface, overcoming occlusion holes, noise, and other phenomena.
@ -88,7 +92,8 @@ def find_object_to_grasp(height_image, display_on=False):
# and other phenomena.
kernel_width_pix = 5 #3
iterations = 3 #5
kernel_radius_pix = (kernel_width_pix - 1) / 2
#kernel_radius_pix = (kernel_width_pix - 1) / 2
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)
use_dilation = True
@ -102,8 +107,10 @@ def find_object_to_grasp(height_image, display_on=False):
# Process the candidate object points
# Treat connected components of candidate object points as objects. Fit ellipses to these segmented objects.
label_image, max_label_index = sk.measure.label(obstacles_on_surface, neighbors=8, background=0, return_num=True, connectivity=None)
region_properties = sk.measure.regionprops(label_image, intensity_image=None, cache=True, coordinates='xy')
#label_image, max_label_index = sk.measure.label(obstacles_on_surface, neighbors=8, background=0, return_num=True, connectivity=None)
label_image, max_label_index = sk.measure.label(obstacles_on_surface, background=0, return_num=True, connectivity=2)
#region_properties = sk.measure.regionprops(label_image, intensity_image=None, cache=True, coordinates='xy')
region_properties = sk.measure.regionprops(label_image, intensity_image=None, cache=True)
if display_on:
rgb_image = height_image.rgb_image.copy()
color_label_image = sk.color.label2rgb(label_image, image=rgb_image, colors=None, alpha=0.3, bg_label=0, bg_color=(0, 0, 0), image_alpha=1, kind='overlay')
@ -146,7 +153,8 @@ def find_object_to_grasp(height_image, display_on=False):
print('object max height = {0} cm'.format(object_max_height_m * 100.0))
print('object mean height = {0} cm'.format(object_mean_height_m * 100.0))
rgb_image = height_image.rgb_image.copy()
rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]/2) + [0, 127, 0]
#rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]/2) + [0, 127, 0]
rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]//2) + [0, 127, 0]
rgb_image[label_image == object_region.label] = [0, 0, 255]
draw_ellipse_axes_from_region(rgb_image, largest_region, color=[255, 255, 255])
cv2.imshow('object to grasp', rgb_image)
@ -210,7 +218,8 @@ def draw_grasp(rgb_image, grasp_target):
surface_convex_hull_mask = grasp_target['surface_convex_hull_mask']
object_selector = grasp_target['object_selector']
object_ellipse = grasp_target['object_ellipse']
rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]/2) + [0, 127, 0]
#rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]/2) + [0, 127, 0]
rgb_image[surface_convex_hull_mask > 0] = (rgb_image[surface_convex_hull_mask > 0]//2) + [0, 127, 0]
rgb_image[object_selector] = [0, 0, 255]
draw_ellipse_axes(rgb_image, object_ellipse, color=[255, 255, 255])
@ -259,8 +268,10 @@ def find_closest_flat_surface(height_image, robot_xy_pix, display_on=False):
if remove_floor:
segments_image[floor_mask > 0] = 0
label_image, max_label_index = sk.measure.label(segments_image, neighbors=8, background=0, return_num=True, connectivity=None)
region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True, coordinates='xy')
#label_image, max_label_index = sk.measure.label(segments_image, neighbors=8, background=0, return_num=True, connectivity=None)
label_image, max_label_index = sk.measure.label(segments_image, background=0, return_num=True, connectivity=2)
#region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True, coordinates='xy')
region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True)
if display_on:
color_label_image = sk.color.label2rgb(label_image, image=image_rgb, colors=None, alpha=0.3, bg_label=0, bg_color=(0, 0, 0), image_alpha=1, kind='overlay')
@ -428,7 +439,9 @@ def render_segments(segments_image, segment_info, output_key_image=False):
font_scale = 1.1 * (size/100.0)
line_width = 1
line_color = (0,0,0)
cv2.putText(key_image_color, '%.3f m' % h, ((i*size) + size/10, size/2),
#cv2.putText(key_image_color, '%.3f m' % h, ((i*size) + size/10, size/2),
# font, font_scale, line_color, line_width, cv2.LINE_AA)
cv2.putText(key_image_color, '%.3f m' % h, ((i*size) + size//10, size//2),
font, font_scale, line_color, line_width, cv2.LINE_AA)
else:
key_image_color = None
@ -694,7 +707,8 @@ def histogram_segment(segments_image, image,
if max_value <= epsilon:
if verbose:
print('zero encountered')
max_bin = (left_bin + right_bin) / 2
#max_bin = (left_bin + right_bin) / 2
max_bin = (left_bin + right_bin) // 2
# Find heights associated with the left of the min bin, the
# right of the max bin, and center of the most heavily
@ -960,8 +974,10 @@ def draw_text(image, text, x, y):
def full_segment(image, image_rgb, segmentation_scale, m_per_unit, zero_height=0.0, visualize=True, visualize_title='', verbose=False):
segments_image, segment_info, height_to_segment_id = segment(image, m_per_unit, zero_height, segmentation_scale, verbose)
label_image, max_label_index = sk.measure.label(segments_image, neighbors=8, background=0, return_num=True, connectivity=None)
region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True, coordinates='xy')
#label_image, max_label_index = sk.measure.label(segments_image, neighbors=8, background=0, return_num=True, connectivity=None)
label_image, max_label_index = sk.measure.label(segments_image, background=0, return_num=True, connectivity=2)
#region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True, coordinates='xy')
region_properties = sk.measure.regionprops(label_image, intensity_image=image, cache=True)
if image_rgb is None:
color_label_image = sk.color.label2rgb(label_image, image=None, colors=None, alpha=0.3, bg_label=0, bg_color=(0, 0, 0), image_alpha=1, kind='overlay')
else:

Write Preview
Loading…
Cancel
Save