#%% grasp_policy = CrossEntropyRobustGraspingPolicy(cfg['policy']) # grasp_policy = RobustGraspingPolicy(cfg['policy']) #%% img = rosco.rgb d = rosco.depth #%% plt.imshow(img) #%% plt.imshow(d) #print(img) #print(d) #%% color_im = ColorImage(img.astype(np.uint8), encoding="bgr8", frame='pcl') depth_im = DepthImage(d.astype(np.float32), frame='pcl') color_im = color_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor']) depth_im = depth_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor']) rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) rgbd_state = RgbdImageState(rgbd_im, camera_int) #%% grasp = grasp_policy(rgbd_state) #%% img2 = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) img2 = cv2.circle(img2,(int(grasp.grasp.center.x),int(grasp.grasp.center.y)),2,(255,0,0),3) plt.imshow(img2) #%% img3 = cv2.cvtColor(d, cv2.COLOR_BGR2RGB) img3 = cv2.circle(img3,(int(grasp.grasp.center.x),int(grasp.grasp.center.y)),2,(255,0,0),3) plt.imshow(img3) #%%
def plan_grasp(self, req): """ Grasp planner request handler . Parameters --------- req: :obj:`ROS ServiceRequest` ROS ServiceRequest for grasp planner service """ rospy.loginfo('Planning Grasp') # set min dimensions pad = max( math.ceil( np.sqrt(2) * (float(self.cfg['policy']['metric']['crop_width']) / 2)), math.ceil( np.sqrt(2) * (float(self.cfg['policy']['metric']['crop_height']) / 2))) min_width = 2 * pad + self.cfg['policy']['metric']['crop_width'] min_height = 2 * pad + self.cfg['policy']['metric']['crop_height'] # get the raw depth and color images as ROS Image objects raw_color = req.color_image raw_depth = req.depth_image segmask = None raw_segmask = req.segmask # get the raw camera info as ROS CameraInfo object raw_camera_info = req.camera_info # get the bounding box as a custom ROS BoundingBox msg bounding_box = req.bounding_box # wrap the camera info in a perception CameraIntrinsics object camera_intrinsics = CameraIntrinsics( raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) ### create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ### try: color_image = ColorImage(self.cv_bridge.imgmsg_to_cv2( raw_color, "rgb8"), frame=camera_intrinsics.frame) depth_image = DepthImage(self.cv_bridge.imgmsg_to_cv2( raw_depth, desired_encoding="passthrough"), frame=camera_intrinsics.frame) segmask = BinaryImage(self.cv_bridge.imgmsg_to_cv2( raw_segmask, desired_encoding="passthrough"), frame=camera_intrinsics.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) # check image sizes if color_image.height != depth_image.height or \ color_image.width != depth_image.width: rospy.logerr( 'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d' % (color_image.height, color_image.width, depth_image.height, depth_image.width)) raise rospy.ServiceException( 'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d' % (color_image.height, color_image.width, depth_image.height, depth_image.width)) if color_image.height < min_height or color_image.width < min_width: rospy.logerr( 'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d' % (min_height, min_width, color_image.height, color_image.width)) raise rospy.ServiceException( 'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d' % (min_height, min_width, color_image.height, color_image.width)) # inpaint images color_image = color_image.inpaint( rescale_factor=self.cfg['inpaint_rescale_factor']) depth_image = depth_image.inpaint( rescale_factor=self.cfg['inpaint_rescale_factor']) # visualize if self.cfg['vis']['color_image']: vis.imshow(color_image) vis.show() if self.cfg['vis']['depth_image']: vis.imshow(depth_image) 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_image = RgbdImage.from_color_and_depth(color_image, depth_image) # calc crop parameters minX = bounding_box.minX - pad minY = bounding_box.minY - pad maxX = bounding_box.maxX + pad maxY = bounding_box.maxY + pad # contain box to image->don't let it exceed image height/width bounds if minX < 0: minX = 0 if minY < 0: minY = 0 if maxX > rgbd_image.width: maxX = rgbd_image.width if maxY > rgbd_image.height: maxY = rgbd_image.height centroidX = (maxX + minX) / 2 centroidY = (maxY + minY) / 2 # compute width and height width = maxX - minX height = maxY - minY # crop camera intrinsics and rgbd image cropped_camera_intrinsics = camera_intrinsics.crop( height, width, centroidY, centroidX) cropped_rgbd_image = rgbd_image.crop(height, width, centroidY, centroidX) cropped_segmask = None if segmask is not None: cropped_segmask = segmask.crop(height, width, centroidY, centroidX) # visualize if self.cfg['vis']['cropped_rgbd_image']: vis.imshow(cropped_rgbd_image) vis.show() # create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics image_state = RgbdImageState(cropped_rgbd_image, cropped_camera_intrinsics, segmask=cropped_segmask) # execute policy try: return self.execute_policy(image_state, self.grasping_policy, self.grasp_pose_publisher, cropped_camera_intrinsics.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!' )