ros 获取 kinect RGBD 视频流 py + cpp

曾用 py 写过一个版本，后来另一个 cpp 项目也需要使用 RGBD 视频流，就又写了一个 cpp 的接收版本。

RGBD_Image.srv

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

1. py

rgbd_image_server.py

• 发布 RGBD_Image
• service name: get_rgbd_image

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

# sensor_msgs/Image, std_msgs/Header can be seen in RGBD_ImageResponse class

res = RGBD_ImageResponse()


def rgb_callback(ros_data):  # ros_data, Image type
    res.rgb = ros_data  # '/camera/rgb/image_color' return Type: sensor_msgs/Image


def depth_callback(ros_data):
    res.depth = ros_data  # '/camera/depth/image_raw' return Type: sensor_msgs/Image


def get_rgbd_image(req):
    if req.start:  # request.start
        rospy.Subscriber('/camera/rgb/image_color', Image,
                         rgb_callback)  # set response.rgb
        rospy.Subscriber('/camera/depth/image_raw', Image,
                         depth_callback)  # set response.depth
        # if res.rgb.data and res.depth.data: (not sync rgb and depth, so can't set this if)
        # Error processing request: not all arguments converted during string formatting
        # rospy.loginfo('send rgb', res.rgb.header.frame_id)
        return res  # RGBD_ImageResponse
    # else:
    #     return None


def rgbd_server():
    rospy.init_node('rgbd_image_server')
    # return type, service callback function
    rospy.Service('get_rgbd_image', RGBD_Image, get_rgbd_image)
    rospy.spin()


if __name__ == "__main__":
    rgbd_server()

rgbd_image_client.py

• 显示 rgb 和 depth 视频
• 按 r 保存视频帧到本地，再次按 r 结束此次录制，第 3 次按 r 开始一轮新的录制
• 按 q 退出程序

# coding=utf-8
import os
import shutil
import cv2
import rospy
from cv_bridge import CvBridge
from wali.srv import RGBD_Image


def mk_record_dir(root):
    rgb_path = os.path.join(root, 'rgb')
    depth_path = os.path.join(root, 'depth')
    # remake
    if os.path.exists(root):
        shutil.rmtree(root)
    os.mkdir(root)
    os.mkdir(rgb_path)
    os.mkdir(depth_path)


def rgbd_client(start=True):
    rospy.wait_for_service('get_rgbd_image')
    try:
        get_rgbd_image = rospy.ServiceProxy('get_rgbd_image', RGBD_Image)  # get the func in client
        root = 'kinect'
        RECORD_FLAG = False
        RECORD_CNT = 0
        cnt = 0
        bridge = CvBridge()
        while True:
            res = get_rgbd_image(start)
            rgb_msg, depth_msg = res.rgb, res.depth
            # print rgb_msg.height, depth_msg.height
            # change process: 0 0 -> 0 480 -> 480 480
            # rgg, depth, not sync and not ready in the beginning
            if rgb_msg.data and depth_msg.data:  # only use valid rgbd
                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)
                if RECORD_FLAG:
                    cv2.imwrite(root + str(RECORD_CNT) + '/rgb/' + str(cnt) + '.jpg', rgb)
                    cv2.imwrite(root + str(RECORD_CNT) + '/depth/' + str(cnt) + '.png', depth)
                    print cnt
                    cnt += 1
                # receive cmd
                cmd = cv2.waitKey(200)
                if cmd == ord('r'):
                    RECORD_FLAG = not RECORD_FLAG
                    if RECORD_FLAG:
                        cnt = 0
                        print 'record begin'
                        # mkdir
                        mk_record_dir(root + str(RECORD_CNT))
                    else:
                        print 'record stop'
                        RECORD_CNT += 1  # prepare for next record
                if cmd == ord('q'):
                    print 'exit'
                    break

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


if __name__ == '__main__':
    rgbd_client()

2. cpp

get_rgbd_image.cpp

• 显示 rgb 和 depth 视频

#include "ros/ros.h"
#include "wali/RGBD_Image.h" // py has built
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp> // ros opencv 3.3.1
#include <opencv2/imgproc/imgproc.hpp>

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "rgbd_cpp_client");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<wali::RGBD_Image>("get_rgbd_image");
    wali::RGBD_Image srv;
    srv.request.start = true;

    cv_bridge::CvImagePtr cv_ptr_rgb;
    cv_bridge::CvImagePtr cv_ptr_depth;

    while (ros::ok()) {
        if (client.call(srv)) {
            ROS_INFO("I got it!");
            // cvt ros::sensor_msgs/Image to cv::Mat
            cv_ptr_rgb = cv_bridge::toCvCopy(srv.response.rgb);
            cv_ptr_depth = cv_bridge::toCvCopy(srv.response.depth);

            if (cv_ptr_rgb->image.rows > 0 && cv_ptr_depth->image.rows > 0) { // both have rgb and depth
                cv::imshow("rgb", cv_ptr_rgb->image);
                cv::imshow("depth", cv_ptr_depth->image);
                if (cv::waitKey(200) == 'q') {
                    break;
                }
            }
        } else {
            ROS_ERROR("Failed to call service get_rgbd_image");
            return 1;
        }
    }

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(wali_cpp)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  sensor_msgs
  wali
  cv_bridge
  OpenCV
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES wali
#  CATKIN_DEPENDS roscpp sensor std_msgs
#  DEPENDS system_lib
)

include_directories(
  include 
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(get_rgbd_image
  src/get_rgbd_image.cpp
)
add_dependencies(get_rgbd_image ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(get_rgbd_image
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}  
)

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>wali_cpp</name>
  <version>0.0.0</version>
  <description>The wali package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="turtle@todo.todo">turtle</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>wali</build_depend>    
  <build_depend>cv_bridge</build_depend>    
  <build_depend>opencv</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>  
  <build_export_depend>wali</build_export_depend>   
  <build_export_depend>cv_bridge</build_export_depend>    
  <build_export_depend>opencv</build_export_depend>       
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>wali</exec_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>opencv</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>