获取深度图后要对目标抓取位姿进行估计，在机器人抓取（六）—— 平面抓取位姿估计（ gqcnn代码测试与解读） 中对抓取位姿估计项目 gqcnn 的源码做了详细解读。
为方便应用，本文用 FullyConvolutionalGraspingPolicyParallelJaw 模型重写了抓取位姿服务器节点 grasp_planner_node.py。代码如下

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Original author Vishal Satish & Jeff Mahler
Adaptor zxxRobot-2021-03-22
"""
import json
import os
import matplotlib.pyplot as plt
import numpy as np
import rospy
import ros_numpy
from visualization import Visualizer2D

from autolab_core import YamlConfig
from gqcnn.grasping import (RgbdImageState, FullyConvolutionalGraspingPolicyParallelJaw)
from perception import (CameraIntrinsics, ColorImage, DepthImage, RgbdImage)
import utils
from pick_place.msg import GQCNNGrasp
from pick_place.srv import GQCNNGraspPlanner


class GetGraspPose:
    """Calculate the grasping pose and q_value from the depth image"""

    def __init__(self,
                 model_path="/home/ur/ur5/src/gqcnn/models/FC-GQCNN-4.0-PJ",  # 抓取模型存储路径
                 config_filename="/home/ur/ur5/src/gqcnn/cfg/examples/fc_gqcnn_ur5.yaml",
                 camera_intr_filename="/home/ur/ur5/src/gqcnn/data/calib/k4a/k4a_depth.intr",  # 相机内参
                 camera_external_filename="/home/ur/.ros/easy_handeye/ur5_k4a_handeyecalibration_eye_on_base.yaml",  # 相机外参，通过手眼标定获得
                 ):
        # 1. Get model configs.
        self.model_config = json.load(open(os.path.join(model_path, "config.json"), "r"))
        try:
            gqcnn_config = self.model_config["gqcnn"]
            # gripper_mode = gqcnn_config["gripper_mode"]
        except KeyError as e:
            print('except:', e)

        # 2.Read config.
        self.config = YamlConfig(config_filename)
        self.policy_config = self.config["policy"]

        # Confirm that the model_path is absolute
        if "gqcnn_model" in self.policy_config["metric"]:
            assert os.path.isabs(self.policy_config["metric"]["gqcnn_model"]), "the model_path must be an absolute path"
            self.policy_config["metric"]["gqcnn_model"] = model_path

        # 3. Setup depth camera.
        self.camera_intr = CameraIntrinsics.load(camera_intr_filename)

        # Set input sizes for fully-convolutional policy.
        self.policy_config["metric"]["fully_conv_gqcnn_config"][
            "im_height"] = self.camera_intr.height  # depth_im.shape[0]
        self.policy_config["metric"]["fully_conv_gqcnn_config"][
            "im_width"] = self.camera_intr.width  # depth_im.shape[1]

        # 4. read camera external parameter
        self.camera_extr = utils.read_camera_external_parameter(camera_external_filename)

        self.grasp_approach_dir = np.array([0, 0, 1]) # = self.transform_world_to_camera([0.707106781, -0.707106781, 0])

        # 5. setup policy
        if self.policy_config["type"] == "fully_conv_pj":
            self.policy = FullyConvolutionalGraspingPolicyParallelJaw(self.policy_config)
        else:
            raise ValueError( "Invalid fully-convolutional policy type: {}".format(
                    self.policy_config["type"]))

    # def __call__(self, depth_data):
    #     return self.get_grasp_pose(depth_data)

    def transform_world_to_camera(self, vector):
        """transform direction vector from world_frame to camera_frame"""
        trans44 = utils.pose_to_mat44(self.camera_extr)
        rot33 = np.transpose(trans44[:3][:, :3])  # 旋转逆变换
        return np.dot(rot33, np.array(vector))

    def get_grasp_pose(self, rqe):

        #  Read depth_images. Depth images only support float arrays
        depth = ros_numpy.numpify(rqe.depth_image)
        depth_im = DepthImage(depth, frame=self.camera_intr.frame)
        # color_im = ColorImage(color, frame=self.camera_intr.frame)

        color_im = ColorImage(np.zeros([depth_im.height, depth_im.width,3]).astype(np.uint8),
                              frame=self.camera_intr.frame)
        segmask = depth_im.invalid_pixel_mask().inverse()

        # Inpaint.
        depth_im = depth_im.inpaint(rescale_factor=self.config["inpaint_rescale_factor"])  # 0.5

        # Create state (RGB-D). In fact, color images are not used
        rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
        state = RgbdImageState(rgbd_im, self.camera_intr, segmask=segmask)

        grasp_action = self.policy(state)

        grasp_pose_cameraframe = grasp_action.grasp.pose(self.grasp_approach_dir).pose_msg

        # transform grasp_pose from camera_frame to robot_frame
        grasp_pose_robotframe = utils.transform_pose(grasp_pose_cameraframe, self.camera_extr)

        gqcnn_grasp = GQCNNGrasp()
        gqcnn_grasp.pose = grasp_pose_robotframe
        gqcnn_grasp.q_value = grasp_action.q_value

        # # Vis final grasp.
        if self.policy_config["vis"]["final_grasp"]:
            Visualizer2D.figure(size=(10, 10))
            Visualizer2D.imshow(rgbd_im.depth,
                                vmin=self.policy_config["vis"]["vmin"],
                                vmax=self.policy_config["vis"]["vmax"])
            Visualizer2D.grasp(grasp_action.grasp, scale=2.5, show_center=False, show_axis=True)
            Visualizer2D.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format(
                grasp_action.grasp.depth, grasp_action.q_value))
            Visualizer2D.show("depth")
            plt.pause(2)
            plt.close("all")

        return gqcnn_grasp


if __name__ == "__main__":
    rospy.init_node("Grasp_Sampler_Server")
    camera_intr_filename = "/home/ur/ur5/src/gqcnn/data/calib/k4a/k4a_resize.intr"  # 图片太大了会拖慢推理速度甚至超出 jetson AGX Xavier的负载，所以做了 resize
    grasp_planner = GetGraspPose(camera_intr_filename=camera_intr_filename)
    grasp_planning_service = rospy.Service("grasp_planner", GQCNNGraspPlanner,
                                           grasp_planner.get_grasp_pose)
    rospy.loginfo("Grasping Policy Initialized")

    # Spin forever.
    rospy.spin()

grasp_plannning_service.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 name="grasp_planner"  pkg="pick_place" type="grasp_planner_node.py" ns="$(arg ns)" output="screen" >

  </node>
</launch>

 
在客户端节点调用 grasp_planner 服务即可获取抓取位姿。主要代码节选如下：

...
rospy.init_node('pick_and_place', anonymous=True)

print("wait for get_depth_image service and create service proxy......")
rospy.wait_for_service("gqcnn/get_depth_image")
get_depth = rospy.ServiceProxy("gqcnn/get_depth_image", Depth)

print("wait for grasp planning service and create service proxy......")
rospy.wait_for_service("gqcnn/grasp_planner")
plan_grasp = rospy.ServiceProxy("gqcnn/grasp_planner", GQCNNGraspPlanner)
...
while not rospy.is_shutdown():
    print("=========== Press 'Enter to get grasp pose ===========")
    raw_input()
    print("The program is planning grasp pose, Please wait a moment.....")

    depth_msg = get_depth().depth_image

    grasp = plan_grasp(depth_msg).grasp
    grasp_pose = grasp.pose  # 目标位姿
    q_value = grasp.q_value  # 位姿估计质量
...

得到目标位姿后，就可以规划机器人运动轨迹，控制机器人抓取目标物体了。