rospy 编写 RGBD.srv 同步获取 ros+kinec

• 使用 roscpp 可参考这篇文章：如何使用ROS的service读取Kinect图像
• 本文使用 rospy

启动 kinect v1

roslaunch freenect_launch freenect.launch

查看现有 Topic，可见发布了很多，我们订阅加了注释的 Topic

深度图
/camera/depth/camera_info
/camera/depth/disparity
/camera/depth/image  # sensor_msgs.msg.Image
/camera/depth/image/compressed # sensor_msgs.msg.CompressedImage
/camera/depth/image/compressed/parameter_descriptions
/camera/depth/image/compressed/parameter_updates
/camera/depth/image/compressedDepth
rgb 图
/camera/rgb/image_raw # sensor_msgs.msg.Image
/camera/rgb/image_raw/compressed # sensor_msgs.msg.CompressedImage
/camera/rgb/image_raw/compressed/parameter_descriptions
/camera/rgb/image_raw/compressed/parameter_updates
/camera/rgb/image_raw/compressedDepth

1. 使用 CompressedImage

RGBD_Compressed_Image.srv

bool start
---
sensor_msgs/CompressedImage rgb
sensor_msgs/CompressedImage depth

rgbd_compress_server.py

#!/usr/bin/env python
# coding=utf-8
import rospy
from sensor_msgs.msg import CompressedImage
from wali.srv import RGBD_Compressed_Image, RGBD_Compressed_ImageResponse

res = RGBD_Compressed_ImageResponse()


def rgb_callback(ros_data):  # ros_data, CompressedImage type
    res.rgb = ros_data


def depth_callback(ros_data):
    res.depth = ros_data


def get_rgbd_compressed_image(req):
    if req.start:
        rospy.Subscriber('/camera/rgb/image_color/compressed', CompressedImage, rgb_callback, queue_size=1)
        rospy.Subscriber('/camera/depth/image_raw/compressed', CompressedImage, depth_callback, queue_size=1)
        return res


def rgbd_compressed_server():
    rospy.init_node('rgbd_compressed_server')
    rospy.Service('get_rgbd_compressed_image', RGBD_Compressed_Image, get_rgbd_compressed_image)
    rospy.spin()


if __name__ == "__main__":
    rgbd_compressed_server()

注：rospy.spin() 订阅多个 Topic 时自带多线程，不用像 roscpp 要写出来

rgbd_compressed_client.py

# coding=utf-8
import rospy
import cv2
import numpy as np
from wali.srv import RGBD_Compressed_Image


def rgbd_compressed_client(start=True):
    rospy.wait_for_service('get_rgbd_compressed_image')
    try:
        get_rgbd_compressed_image = rospy.ServiceProxy('get_rgbd_compressed_image', RGBD_Compressed_Image)
        cnt = 0
        while True:
            res = get_rgbd_compressed_image(start)
            rgb_msg, depth_msg = res.rgb, res.depth
            # np CompressedImage msg and decode np to image
            rgb_np = np.fromstring(rgb_msg.data, np.uint8)
            depth_np = np.fromstring(depth_msg.data, np.uint8)
            if rgb_np.any() and depth_np.any():
                rgb = cv2.imdecode(rgb_np, cv2.IMREAD_COLOR)
                cv2.imshow('rgb', rgb)
                depth = cv2.imdecode(depth_np, cv2.IMREAD_ANYDEPTH)
                cv2.imshow('depth', depth)
                print cnt
                if cv2.waitKey(50) == ord('q'):  # 20fps
                    break
                cnt += 1

    except rospy.ServiceException, e:
        print "Service call failed: %s" % e


if __name__ == '__main__':
    rgbd_compressed_client()

效果：

发现问题：CompressedImage 深度图似乎有问题

换用 CvBridge 转化图片，结果依然一样，说明是图片本身问题

from cv_bridge import CvBridge

bridge = CvBridge()
res = get_rgbd_compressed_image(start)
rgb_msg, depth_msg = res.rgb, res.depth
if rgb_msg.data and depth_msg.data:
    rgb = bridge.compressed_imgmsg_to_cv2(rgb_msg)
    cv2.imshow('rgb', rgb)
    depth = bridge.compressed_imgmsg_to_cv2(depth_msg)
    cv2.imshow('depth', depth)

需要放弃使用 CompressedImage，图片本身信息不足。

2. 使用 Image

RGBD_Image.srv

bool start
---
sensor_msgs/Image rgb
sensor_msgs/Image depth

rgbd_image_server.py

#!/usr/bin/env python
# coding=utf-8
import rospy
from sensor_msgs.msg import Image
from wali.srv import RGBD_Image, RGBD_ImageResponse

res = RGBD_ImageResponse()


def rgb_callback(ros_data):  # ros_data, Image type
    res.rgb = ros_data


def depth_callback(ros_data):
    res.depth = ros_data


def get_rgbd_image(req):
    if req.start:
        rospy.Subscriber('/camera/rgb/image_color', Image, rgb_callback)
        rospy.Subscriber('/camera/depth/image_raw', Image, depth_callback)
        return res
    else:
        return None


def rgbd_server():
    rospy.init_node('rgbd_image_server')
    rospy.Service('get_rgbd_image', RGBD_Image, get_rgbd_image)
    rospy.spin()


if __name__ == "__main__":
    rgbd_server()

rgbd_image_client.py

# coding=utf-8
import rospy
import cv2
import numpy as np
from wali.srv import RGBD_Image
from cv_bridge import CvBridge

bridge = CvBridge()


def rgbd_client(start=True):
    rospy.wait_for_service('get_rgbd_image')
    try:
        get_rgbd_image = rospy.ServiceProxy('get_rgbd_image', RGBD_Image)
        cnt = 0
        while True:
            res = get_rgbd_image(start)
            rgb_msg, depth_msg = res.rgb, res.depth
            if rgb_msg.data and depth_msg.data:
                rgb = bridge.imgmsg_to_cv2(rgb_msg, rgb_msg.encoding)
                cv2.imshow('rgb', rgb)
                depth = bridge.imgmsg_to_cv2(depth_msg, depth_msg.encoding)
                cv2.imshow('depth', depth)
                print cnt
                if cv2.waitKey(50) == ord('q'):
                    break
                cnt += 1
                
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e


if __name__ == '__main__':
    rgbd_client()

效果：

可以看出这种 Depth 更加连续，数值更多。