基于视觉的机器人抓取方法首先要获取抓取位姿，在机器人抓取（六）—— 平面抓取位姿估计（ gqcnn代码测试与解读） 中，我们对抓取位姿估计网络gqcnn做了详细讲解。
 
在进行抓取位姿估计时，首先要获取一张深度图。我们可以通过 ros 的 topic 机制不断的发送深度图，不过其实我们并不需要实时读取每一帧的图像，只需要在做抓取位姿估计时读取一张深度图即可。本着不浪费计算资源的原则，用 ros Service 机制更为合适。
本文的相机 ros 服务流程：当客户端需要一张图像做抓取位姿估计时，向相机服务端发送一个请求，服务端接收到请求后调用 pyk4a 接口获取一张深度图，返回给客户端。
 
关于 ros service 机制请参考 ROS学习笔记（四）—— 服务 Service 详解
关于 pyk4a 以及 Azure kinect 相机请参考机器人抓取（三）—— Azure Kinect SDK 及 ROS 驱动安装。
 
Azure Kinect 相机ros服务端的代码 camera_server_node.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import pyk4a
from pyk4a import Config, PyK4A
import numpy as np
import cv2
import rospy
from pick_place.srv import Depth
import ros_numpy
from sensor_msgs.msg import Image
# from perception import DepthImage


class Camera:
    def __init__(self,
                 color_resolution=pyk4a.ColorResolution.RES_720P,
                 depth_mode=pyk4a.DepthMode.NFOV_UNBINNED,  # 640x576
                 ):

        self.k4a = PyK4A(
            Config(
                color_resolution=color_resolution,
                depth_mode=depth_mode,
                synchronized_images_only=False,
            )
        )
        self.k4a.start()
        print("--------Azure Kinect camera is working -------")

    def get_resize_transformed_depth(self, request):  
        # 注意：即使 srv 没有请求数据,回调函数的定义中也不能减省 request 参数
        """
        raw depth image is 640x576, transform to color camera frame is 720p
        resize_k is resize scale factor.
        @param resize_k: resize scale factor, if k=0, not resize. default=0.5
        @param transformed: if True, return transformed_depth
        @return: depth
        """
        resize_k = 0.5
        transformed = True

        capture = self.k4a.get_capture()
        if transformed:
            depth = capture.transformed_depth
        else:
            depth = capture.depth
        depth = depth.astype(np.float32) / 1000
        if resize_k != 0:
            shape = depth.shape
            depth = cv2.resize(depth, (int(resize_k * shape[1]), int(resize_k * shape[0])), interpolation=cv2.INTER_NEAREST)
        # depth_msg = DepthImage(depth, frame="k4a_rgb").rosmsg
        depth_msg = ros_numpy.msgify(Image, depth, '32FC1')
        return depth_msg


if __name__ == "__main__":

    rospy.init_node("Camera_Server")
    camera = Camera()  # camera start

    get_depth = rospy.Service("get_depth_image", Depth,
                              camera.get_resize_transformed_depth)
    rospy.spin()

 
Depth 是定义在 pick_place 包中的 srv，结构如下

# request params
---
# response params
sensor_msgs/Image depth_image

 
启动服务端的 launch 文件,主要内容如下

<launch>
  <!-- Namespace for the node and services -->
  <arg name="ns"  default="gqcnn" />
  <node name="get_depth_image"  pkg="pick_place" type="camera_server_node.py" ns="$(arg ns)" output="screen" />
  </node>
</launch>

 
客户端主要代码如下

from pick_place.srv import Depth
...
rospy.wait_for_service("gqcnn/get_depth_image")
get_depth = rospy.ServiceProxy("gqcnn/get_depth_image", Depth)
...
print("=========== Press 'Enter to get depth image ===========")
raw_input()  
depth_msg = get_depth().depth_image
...