如何使用ROS的service读取Kinect图像

如何使用ROS的service读取Kinect图像

我们经常会使用各种ROS包，而如何在ROS程序中导入Kinect图像呢？

1.先写一个service文件

学了挺久了，这个还没有好好整理过。其实步骤很简单：

• 先在工作空间下创建一个ros package。

  $ catkin_create_pkg kinect_srv roscpp rospy std_msgs message_generation sensor_msgs

• 然后在这个包下，新建一个srv文件夹，并建立srv文件。

  $ mkdir srv
  $ gedit rgbd_image.srv

  在srv文件里填写自己需要的srv形式。我需要一个输入一个call信号，就可以输出rgb和depth图像的srv。

  
  bool start
  ---
  
  sensor_msgs/Image rgb_image
  sensor_msgs/Image depth_image

• 增添以下内容到package.xml里
  
message_generation
message_runtime

• 修改CMakelists.txt

cmake_minimum_required(VERSION 2.8.3)
project(kinect_srv)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
 sensor_msgs
)

add_service_files(
    FILES
#   Service1.srv
#   Service2.srv
    rgbd_image.srv
)

generate_messages(
   DEPENDENCIES
   sensor_msgs
   std_msgs
)

catkin_package(
   CATKIN_DEPENDS message_runtime
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

到这里就可以使用编译了

$ catkin_make

如果一切正确，应该在工作空间的devel/include/kinect_srv文件下找到rgbd_image.h文件。这个会在后面引用。

2.发布Service

在工作空间下再建一个包

$ catkin_create_pkg kinect_driver roscpp rospy std_msgs sensor_msgs cv_bridge image_transport

在src中新建一个get_image.cpp文件,开始设定service内容。

#include "kinect_driver/common_include.h"  //这里面是opencv和ros的头文件
#include "kinect_srv/rgbd_image.h"

using namespace std;
using namespace cv;

sensor_msgs::Image rgb_image;
sensor_msgs::Image depth_image;

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_rgb;
  image_transport::Subscriber image_sub_depth;
  //image_transport::Subscriber image_sub_point;

public:

  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_rgb = it_.subscribe("/camera/rgb/image_rect_color", 1, &ImageConverter::imageCb_rgb, this);
    image_sub_depth = it_.subscribe("/camera/depth_registered/image_raw", 1, &ImageConverter::imageCb_depth, this);
   //image_sub_point = it_.subscribe("/camera/depth_registered/points", 1, &ImageConverter::imageCb_depth, this);    
///camera/depth_registered/points
  }

  ~ImageConverter()
  {
  }

  void imageCb_rgb(const sensor_msgs::ImageConstPtr& msg)
  {
     rgb_image = *msg;
  }
  void imageCb_depth(const sensor_msgs::ImageConstPtr& msg)
  {  
     depth_image = *msg;
  }

};

bool get_image(kinect_srv::rgbd_image::Request &req,kinect_srv::rgbd_image::Response &res)
{
    if (req.start) 
    {
         res.rgb_image = rgb_image;
         res.depth_image = depth_image;
    }
    ROS_INFO("success");
    return true;
}


int main(int argc, char** argv)
{    

  ros::init(argc, argv, "get_image");
  ImageConverter ic; 
  ros::NodeHandle n;
  ros::ServiceServer service = n.advertiseService("get_image", get_image);

  ros::Rate loop_rate(200);
  while (ros::ok())
  {

     ros::spinOnce();
    loop_rate.sleep();
  }
  ros::spin();
  return 0;
}

为了方便，我们可以再写一个launch文件快速启动。（使用了openni2的驱动）

<launch>
    <include file="$(find openni2_launch)/launch/openni2.launch" />
    <node pkg="kinect_driver" type="get_image" name="get_image"/>
launch>

3.使用Service

为了检测我们能否读到rgbd图像，我们再写一个显示程序。

#include "kinect_driver/common_include.h"
#include "kinect_srv/rgbd_image.h"

using namespace std;
using namespace cv;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "show_image");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<kinect_srv::rgbd_image>("get_image");
    kinect_srv::rgbd_image srv;
    srv.request.start = true;
    sensor_msgs::Image msg_rgb;
    sensor_msgs::Image msg_depth;
    ros::Rate loop_rate(200);

    Mat rgb_image;
    Mat depth_image;

    while (ros::ok())
    {
        if (client.call(srv))
        {

        try
        {
            msg_rgb = srv.response.rgb_image;
            msg_depth = srv.response.depth_image;
            rgb_image = cv_bridge::toCvCopy(msg_rgb, sensor_msgs::image_encodings::TYPE_8UC3)->image;
            depth_image = cv_bridge::toCvCopy(msg_depth, sensor_msgs::image_encodings::TYPE_32FC1)->image;
            //normalize(depth_image,tmp,255,0,NORM_MINMAX);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return 1;
        }
            IplImage ipl_rgb_image = rgb_image;
            cvConvertImage(&ipl_rgb_image , &ipl_rgb_image , CV_CVTIMG_SWAP_RB);

            imshow("rgb",rgb_image);
            imshow("depth",depth_image);
            waitKey(1);
        }
    }
}

这里的大循环中，我们每call一次，接受到一次rgb和depth图像。
最后写这个包的CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(kinect_driver)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages


###########
## Build ##
###########
#ros
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  cv_bridge
  image_transport
)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
#opencv
find_package(OpenCV 3 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
link_directories(${OpenCV_LIBRARY_DIR})
#pcl
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
#self files
include_directories("~/XXX/devel/include")
include_directories(include)

#############
## execute ##
#############
#get_image
add_executable(get_image src/get_image.cpp)
target_link_libraries(get_image
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
   ${PCL_LIBRARIES}
)

#show_image
add_executable(show_image src/show_image.cpp)
target_link_libraries(show_image
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
   ${PCL_LIBRARIES}
)

到这里就全部完成了。

4.检测结果

打开一个终端进入工作空间运行：

$ source devel/setup.bash
$ roslaunch kinect_driver kinect_driver.launch

再打开另外一个终端入工作空间，输入：

$ rosrun kinect_driver show_image

即可显示