使用ROS发布16bit原始图像

使用ROS发布16bit原始图像

前提：当你有一个相机SDK但不拥有其ROS版本

目标：将你的读程序改写成ROS版本

问题：发布的内容并不是8位图像，而是14位raw图，每个像素灰度被2个字节存储

手段：依旧是采用cv_bridge将图像转换成cv::Mat进行发布

—————————————————————正文————————————————————————

近期有一个开发项目是将红外热像仪进行ROS架构下的采集开发，已经拥有了红外热像仪的开发sdk，但是缺少其ros版本。因此首先需要做的是将该sdk改写成ros版本，构成ros节点。

1.按照ros教程创建自己的功能包(creating a ROS package)

这里很关键的是，需要修改CMakeLists.txt和package.xml文件内容以保证你的节点能够链接到相关的库文件和生成相应的可执行文件。

CMakeLists.txt中

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  cv_bridge
  image_transport
)
FIND_PACKAGE( OpenCV REQUIRED )

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  /home/yang/Desktop/catkin_ws/src/tau2camera-ros/inc
)

add_executable(tau2_test_node src/tau2_ros_node.cpp)

add_executable(tau2_raw_image_pub src/tau2_raw_image_pub.cpp)

add_executable(tau2_raw_image_sub src/tau2_raw_image_sub.cpp)

 target_link_libraries(tau2_test_node
   ${catkin_LIBRARIES}
   netprob
 )
 target_link_libraries(tau2_raw_image_pub
   ${OpenCV_LIBS}
   ${catkin_LIBRARIES}
   netprob
 )
 target_link_libraries(tau2_raw_image_sub
   ${OpenCV_LIBS}
   ${catkin_LIBRARIES}
 )

观察上述改动

• 首先是package的依赖库中要新增 sensor_msgs，cv_bridge，image_transport三个ros包，负责在ROS中发布cv::Mat类型的图像信息。
• 其次是新增了一个头文件路径，这里还是很有必要的，不然生成可执行文件的时候找不到main函数对应的头文件。这样也方便管理，你不需要把相机SDK中的文件全放在catkin_ws/src/package_name/src下，显得很乱。
• 再次是增加编译可执行文件的一句话，这里不用多说。
• 最后是链接库文件，这个库文件在这里叫libnetprob.so，所以你只需要填netprob就可以了，其他的cmake自动帮你补齐。值得提到的是，这里的库是相机sdk库，你可以将它放入/usr/lib/中去，这样cmake在寻找时就能找到而不报错。另外，好像链接库文件的写法还有一种更简单的，就是先将库全都定义到一起，最后用一个macro变量直接填在这里就行了，不需要写那么多重复的句子。这里我还没太搞明白咋写，不过相信读者都很聪明，参考一下其他demo的写法就行了。

package.xml中

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>image_transport</exec_depend>

这里无非是添加了刚才的那几个ROS依赖包。无须赘述

2.将程序改成ROS版本

这里的目标是，使用sdk读取到相机图像，之后再将其发布出来，直接看代码吧。

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
//这是调用相机库需要的头文件
#include "Clinet_API.h"
#include "netserver.h"

#define IMG_HEIGHT 640
#define IMG_WIDTH 512

#define BUFFER_SIZE 640*512*2 //这里能看到，需要一个分辨率*2大小的空间来存储16bit的图像

using namespace std;

cv::Mat buffer2img(char* buffer, int height, int width);

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("tau2_camera/image", 1);

    bool bInit = NET_VIDEO_Init();
    if(bInit)
    {
        NET_SERVER_DEVICEINFO deviceInfo;
        char *ip = "192.168.1.201";
        int port = 8000;
        long ret = NET_VIDEO_Login(ip, port, &deviceInfo);
        if(ret == 0)
        {
            NET_SERVER_CLIENTINFO clientInfo;
            clientInfo.lChannel = 0;
            clientInfo.lLinkMode = 0;
            clientInfo.hPlayWnd = NULL;
            clientInfo.pdc = NULL;
            clientInfo.sMultiCastIP = NULL;

            ret = NET_VIDEO_RealPlay(ip, port, &clientInfo);
            if(ret == 0)
            {
                // retrive old and new images
                cv::Mat img;
                int cnt;
                int type = 0x640;
                while(true)
                {
                    char buffer[BUFFER_SIZE];
                    BOOL bResult = NET_VIDEO_RevData((char*)buffer, BUFFER_SIZE, &cnt, &type);
                    img = buffer2img(buffer,IMG_HEIGHT,IMG_WIDTH);
                    sensor_msgs::ImagePtr msg;
                    //ros::Rate loop_rate(5);
                    if(nh.ok()&&bResult)
                    {
                        msg = cv_bridge::CvImage(std_msgs::Header(), "mono16", img).toImageMsg();
                        pub.publish(msg);
                        ROS_INFO(" publishing!! [%d]", cnt);
                        //ros::spinOnce();
                        //loop_rate.sleep();
                    }
                }
            }
        }
    }
    return 0;
}

cv::Mat buffer2img(char* buffer, int height, int width)
{
    int ii=0;
    int jj=0;
    cv::Mat image_holder(height,width, CV_16UC1);
    for(int i=0; i<BUFFER_SIZE-1;i++)
    {
        if(i % 2 == 0 )
        {
            image_holder.at<ushort>(ii,jj)=(buffer[i]+256*buffer[i+1]);
            ii++;
            if(ii==height)
            {
                ii=0;
                jj++;
            }
        }
    }
    return image_holder;
}

仔细看一遍就知道，ROS发布图像的流程就是首先使用相机sdk读取到图像缓存buffer，然后将缓存进行转换buffer2img()，这时候cv::Mat格式图像被赋值，之后再将其赋值到sensor_msgs::ImagePtr msg中去，再发布出来pub.publish(msg);
这里很关键的一步是如何将2进制图像转换成10进制。640*512*2个字节中，从第0个开始，相邻两个字节构成一个像素，这时候直接读取buffer(i)会单独将该字节的内容翻译成10进制，这时候你只需要低位+256*高位就可以从两个字节中还原出一个像素灰度值。
另外还有一点是，该buffer的存储是按列进行的，也就是说它是：列由上到下，列加一，再由上到下，的形式，因此你的转换函数需要关注这一点。
最后是这一句：msg = cv_bridge::CvImage(std_msgs::Header(), "mono16", img).toImageMsg();里的mono是16进制的意思，你可以在ROS的网站上查到这些代号。在这里你需要用到mono因为是16进制的图片。

3.运行你的节点

运行
roscore
rosrun package_name node_name 在这里是rosrun tau2_camera tau2_raw_image_pub
最后再用rviz订阅发布的图像即可，这里rviz自动将16位图像映射到8位来显示了

4.后续工作

现在的想法是后续需要参考并了解ROS下相机发布时所需要的其他topic信息，完成一个proper的ROS节点。