def _plan_grasp(self,
color_im,
depth_im,
camera_intr,
bounding_box=None,
segmask=None):
"""Grasp planner request handler.
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS `ServiceRequest` for grasp planner service.
"""
rospy.loginfo("Planning Grasp")
# Inpaint images.
color_im = color_im.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
depth_im = depth_im.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
# Init segmask.
if segmask is None:
segmask = BinaryImage(255 *
np.ones(depth_im.shape).astype(np.uint8),
frame=color_im.frame)
# Visualize.
if self.cfg["vis"]["color_image"]:
vis.imshow(color_im)
vis.show()
if self.cfg["vis"]["depth_image"]:
vis.imshow(depth_im)
vis.show()
if self.cfg["vis"]["segmask"] and segmask is not None:
vis.imshow(segmask)
vis.show()
# Aggregate color and depth images into a single
# BerkeleyAutomation/perception `RgbdImage`.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# Mask bounding box.
if bounding_box is not None:
# Calc bb parameters.
min_x = bounding_box.minX
min_y = bounding_box.minY
max_x = bounding_box.maxX
max_y = bounding_box.maxY
# Contain box to image->don't let it exceed image height/width
# bounds.
if min_x < 0:
min_x = 0
if min_y < 0:
min_y = 0
if max_x > rgbd_im.width:
max_x = rgbd_im.width
if max_y > rgbd_im.height:
max_y = rgbd_im.height
# Mask.
bb_segmask_arr = np.zeros([rgbd_im.height, rgbd_im.width])
bb_segmask_arr[min_y:max_y, min_x:max_x] = 255
bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8),
segmask.frame)
segmask = segmask.mask_binary(bb_segmask)
# Visualize.
if self.cfg["vis"]["rgbd_state"]:
masked_rgbd_im = rgbd_im.mask_binary(segmask)
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(masked_rgbd_im.color)
vis.subplot(1, 2, 2)
vis.imshow(masked_rgbd_im.depth)
vis.show()
# Create an `RgbdImageState` with the cropped `RgbdImage` and
# `CameraIntrinsics`.
rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Execute policy.
try:
return self.execute_policy(rgbd_state, self.grasping_policy,
self.grasp_pose_publisher,
camera_intr.frame)
except NoValidGraspsException:
rospy.logerr(
("While executing policy found no valid grasps from sampled"
" antipodal point pairs. Aborting Policy!"))
raise rospy.ServiceException(
("While executing policy found no valid grasps from sampled"
" antipodal point pairs. Aborting Policy!"))
frame=camera_intr.frame)
# Optionally read a segmask.
mask = np.zeros((camera_intr.height, camera_intr.width, 1), dtype=np.uint8)
c = np.array([165, 460, 500, 135])
r = np.array([165, 165, 370, 370])
rr, cc = skimage.draw.polygon(r, c, shape=mask.shape)
mask[rr, cc, 0] = 255
segmask = BinaryImage(mask)
if segmask_filename is not None:
segmask = BinaryImage.open(segmask_filename)
valid_px_mask = depth_im.invalid_pixel_mask().inverse()
if segmask is None:
segmask = valid_px_mask
else:
segmask = segmask.mask_binary(valid_px_mask)
# Create new cloud.
point_cloud = camera_intr.deproject(depth_im)
point_cloud.remove_zero_points()
pcl_cloud = pcl.PointCloud(point_cloud.data.T.astype(np.float32))
tree = pcl_cloud.make_kdtree()
# Find large clusters (likely to be real objects instead of noise).
ec = pcl_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(CLUSTER_TOL)
ec.set_MinClusterSize(MIN_CLUSTER_SIZE)
ec.set_MaxClusterSize(MAX_CLUSTER_SIZE)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
num_clusters = len(cluster_indices)
def plan_grasp(self, camera_data, n_candidates=1):
"""Grasp planner.
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS `ServiceRequest` for grasp planner service.
"""
self._camera_data = camera_data
# --- Inpaint images --- #
color_im = camera_data.rgb_img.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
depth_im = camera_data.depth_img.inpaint(
rescale_factor=self.cfg["inpaint_rescale_factor"])
# --- Init segmask --- #
if camera_data.seg_img is None:
segmask = BinaryImage(255 *
np.ones(depth_im.shape).astype(np.uint8),
frame=color_im.frame)
else:
segmask = camera_data.seg_mask
# --- Aggregate color and depth images into a single
# BerkeleyAutomation/perception `RgbdImage` --- #
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# --- Mask bounding box --- #
if camera_data.bounding_box is not None:
# Calc bb parameters.
min_x = camera_data.bounding_box['min_x']
min_y = camera_data.bounding_box['min_y']
max_x = camera_data.bounding_box['max_x']
max_y = camera_data.bounding_box['max_y']
# Contain box to image->don't let it exceed image height/width
# bounds.
if min_x < 0:
min_x = 0
if min_y < 0:
min_y = 0
if max_x > rgbd_im.width:
max_x = rgbd_im.width
if max_y > rgbd_im.height:
max_y = rgbd_im.height
# Mask.
bb_segmask_arr = np.zeros([rgbd_im.height, rgbd_im.width])
bb_segmask_arr[min_y:max_y, min_x:max_x] = 255
bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8),
segmask.frame)
segmask = segmask.mask_binary(bb_segmask)
# --- Create an `RgbdImageState` with the cropped `RgbdImage` and `CameraIntrinsics` --- #
rgbd_state = RgbdImageState(rgbd_im, camera_data.intrinsic_params, segmask=segmask)
# --- Execute policy --- #
try:
grasps_and_predictions = self.execute_policy(rgbd_state, self.grasping_policy,
camera_data.intrinsic_params.frame,
n_candidates)
self._dexnet_gp = grasps_and_predictions
# --- project planar grasps to 3D space --- #
l = []
for gp in grasps_and_predictions:
# my method
pose_6d = self.transform_grasp_to_6D(gp[0], camera_data.intrinsic_params)
pos = pose_6d[:3,3]
rot = pose_6d[:3, :3]
grasp_6D = Grasp6D(position=pos, rotation=rot,
width=gp[0].width, score= gp[1],
ref_frame=camera_data.intrinsic_params.frame)
l.append(grasp_6D)
# dexnet method --> needs autolab_core installed as catkin package
# 6D_gp = gp[0].pose()
self.grasp_poses = l
self.best_grasp = l[0]
self.visualize()
return True
except NoValidGraspsException:
warnings.warn(("While executing policy found no valid grasps from sampled antipodal point pairs!"))
return False
def _plan_grasp(self,
color_im,
depth_im,
camera_intr,
bounding_box=None,
segmask=None):
""" Grasp planner request handler .
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS ServiceRequest for grasp planner service
"""
rospy.loginfo('Planning Grasp')
# inpaint images
color_im = color_im.inpaint(
rescale_factor=self.cfg['inpaint_rescale_factor'])
depth_im = depth_im.inpaint(
rescale_factor=self.cfg['inpaint_rescale_factor'])
# init segmask
if segmask is None:
segmask = BinaryImage(255 *
np.ones(depth_im.shape).astype(np.uint8),
frame=color_im.frame)
# visualize
if self.cfg['vis']['color_image']:
vis.imshow(color_im)
vis.show()
if self.cfg['vis']['depth_image']:
vis.imshow(depth_im)
vis.show()
if self.cfg['vis']['segmask'] and segmask is not None:
vis.imshow(segmask)
vis.show()
# aggregate color and depth images into a single perception rgbdimage
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
# mask bounding box
if bounding_box is not None:
# calc bb parameters
min_x = bounding_box.minX
min_y = bounding_box.minY
max_x = bounding_box.maxX
max_y = bounding_box.maxY
# contain box to image->don't let it exceed image height/width bounds
if min_x < 0:
min_x = 0
if min_y < 0:
min_y = 0
if max_x > rgbd_im.width:
max_x = rgbd_im.width
if max_y > rgbd_im.height:
max_y = rgbd_im.height
# mask
bb_segmask_arr = np.zeros([rgbd_image.height, rgbd_image.width])
bb_segmask_arr[min_y:max_y, min_x:max_x] = 255
bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8),
segmask.frame)
segmask = segmask.mask_binary(bb_segmask)
# visualize
if self.cfg['vis']['rgbd_state']:
masked_rgbd_im = rgbd_im.mask_binary(segmask)
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(masked_rgbd_im.color)
vis.subplot(1, 2, 2)
vis.imshow(masked_rgbd_im.depth)
vis.show()
# create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics
rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# execute policy
try:
return self.execute_policy(rgbd_state, self.grasping_policy,
self.grasp_pose_publisher,
camera_intr.frame)
except NoValidGraspsException:
rospy.logerr(
'While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!'
)
raise rospy.ServiceException(
'While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!'
)
