如何在ROS中定义并使用自己的消息类型-以image类型为例

ROS自带的消息类型非常丰富，在ROS Kinetic上使用命令rostopic list可以查看所有的消息类型。但是总有时候系统自带的消息类型无法满足工程的特殊需要，这时我们可以自定义消息类型。

   我们要在板子上测试其图像处理能力，要使用一个image_buf的类型进行cache，iamge_buf是一个image的vector。这里，我们直接自己定义一个image类型，然后生成对应的消息类型，并根据这个image，再生成image_buf。

自定义第一个消息类型my_imgdata

1. 建立自定义消息类型package

   cd ~/catkin_ws/src/

   catkin_creat_pkg imgdata_msg

   cd imgdata_msg/

   mkdir msg

2. 输入消息格式到消息文件，这里图像数据格式参考：http://blog.youkuaiyun.com/redskydeng/article/details/49362155

   echo "uint8[] data" >> msg/my_imgdata.msg

    echo "string actor_time" >> msg/my_imgdata.msg

3. 在imgdata_msg的CMakeLists.txt文件中添加如下语句

  find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs)

4. 取消 add_message_files 的相关注释，并添加为如下：
   add_message_files（FILES my_imgdata.msg )

5. 取消generate_messages的相关注释，并添加修改为如下：
   generate_messages（DEPENDENCIES std_msgs  ）

6. 在catkin_package中添加：CATKIN_DEPENDS message_runtime

7. 在imgdata_msg的.xml文件中添加：//indigo用的是run_depend，为Kinetic用的是exec_depend

  <build_depend>message_generation</build_depend>

  <exec_depend>message_runtime</exec_depend>

8. cd ~/catkin_ws/  &&  catkin_make -DCATKIN_WHITELIST_PACKAGES="imgdata_msg"

9. source devel/setup.bash

10. 至此第一个自定义的消息定义完成，你可以用rosmsg(在indigo上是rostopic)查看完整的消息定义

    rosmsg show imgdata_msg/my_imgdata

下面建立第二个消息类型Img_buf，该消息会使用之前我们自定义的消息类型my_imgdata

1. cd ~/catkin_ws/src/imgdata_msg/ 

2. 输入消息格式到消息文件

    echo "std_msgs/Header  header" >> msg/Img_buf.msg

    echo "imgdata_msg/my_imgdata[]  buf" >> msg/Img_buf.msg

3. 在建立第一个消息的基础上，在 add_message_files 添加如下红色部分：
   add_message_files（FILES my_imgdata.msg Img_buf.msg )

4. cd ~/catkin_ws/  &&  catkin_make -DCATKIN_WHITELIST_PACKAGES="imgdata_msg"

5 source devel/setup.bash

6 至此第一个自定义的消息定义完成，你可以用rosmsg show imgdata_msg/Img_buf 查看完整的消息定义

使用自定义的消息类型：

在使用该消息的工程中添加如下头文件即可，同时要把devel/include目录下的imgdata_msg整体复制到使用该消息的include目录下

#include <imgdata_msg/Img_buf.h>

该头文件是系统在建立自定义消息类型时候自动生成的。

这次实践，自己定义了一个消息类型my_imgdata，里面包含一个数组（或者说是vector）和一个string，然后又针对my_imgdata封装了一个新的消息类型Img_buf，里面包含了一个my_imgdata的数组（或者说是vector）。虽然这次实践的内容看来可能没有实际意义，（定义一个消息类型，又定义一个该消息类型的vetor作为一个新的消息类型）但是我们的最终目标是根据别人提供的任何消息类型，进行二次封装（目的是为了实现缓存），往里面再添加一些其他的成员，如时间、长度等信息，然后就可以利用ros提供的序列化和反序列化，这样就可以不考虑别人提供的数据（这部分数据也有序列化和反序列化）。

这里给出我们这次写出来的测试程序：

#include <memory>
#include <vector>
#include <algorithm>
#include <string>
#include <pthread.h>
#include <map>
#include <fstream>
#include <iostream>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include "zlib.h"
#include "LRUCache.hpp"
#include "imgdata_msg/Img_buf.h"
#include "imgdata_msg/my_imgdata.h"

#define MAX_BUF 100
#define STORE_PATH "/home/oldking/Msg-File/"

using std::string;
using std::vector;

imgdata_msg::Img_buf w_buf;

pthread_mutex_t buf_lock;

lru::Cache<std::string, imgdata_msg::Img_buf> cache(5,0);//first param is cache size

unsigned long ReadFromFile(std::string file_name, unsigned char*& data)
{
        std::ifstream rs(file_name.c_str(), std::ios::binary);

        rs.seekg(0, std::ios_base::end);
        unsigned long length = rs.tellg();
        rs.seekg(0, std::ios_base::beg);

        data = new unsigned char[length];
        rs.read((char*)data, length);
        rs.close();

    return length;
}

int ForceWrite()
{
    mode_t mode = 0755;
        mkdir(STORE_PATH, mode);
    
    size_t serial_size = ros::serialization::serializationLength(w_buf);
        boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
        ros::serialization::OStream stream(buffer.get(), serial_size);
    ros::serialization::serialize(stream, w_buf);

        unsigned char* write = buffer.get();
        uLong comprLen = compressBound(serial_size);
        Byte *compr = new Byte[comprLen];
        int err;
        err = compress(compr, &comprLen, (const Bytef*)write, serial_size);
        if(err != Z_OK)
        {
                printf("compress write file failed !\n");
                return 1;
        }

        std::string file_name = STORE_PATH + w_buf.write_buffer[0].actor_time;
    printf("file_name : %s\n", file_name.c_str());
    //std::ofstream rs(file_name.c_str(), std::ios::binary);
        //rs.write((const char*)compr, comprLen);
        //rs.close();

    FILE* file;
    if((file = fopen(file_name.c_str(), "wb")) == NULL)
    {
        printf("cannot create %s!\n", file_name.c_str());
        return -1;
    }
    uLong flen = serial_size;
    fwrite(&flen, sizeof(uLong), 1, file);
    fwrite(&comprLen, sizeof(uLong), 1, file);
    fwrite(compr, sizeof(unsigned char), comprLen, file);
    fclose(file);
    
    delete[] compr;
    
    //Initialize vector
        w_buf.write_buffer.clear();
    //w_buf.num = 0;

    return 0;
}

int InsertToBuffer(imgdata_msg::my_imgdata &Msg)
{
        if(w_buf.write_buffer.size() < MAX_BUF)
    {
        pthread_mutex_lock(&buf_lock);
                w_buf.write_buffer.push_back(Msg);
                //w_buf.num += 1;//我们没有这个变量
        pthread_mutex_unlock(&buf_lock);

        if(w_buf.write_buffer.size() == MAX_BUF)
            ForceWrite();
    }
    else
    {
        ForceWrite();

        pthread_mutex_lock(&buf_lock);
                w_buf.write_buffer.push_back(Msg);
              //  w_buf.num += 1;
        pthread_mutex_unlock(&buf_lock);
    }

    return 0;
}

int MsgStore(imgdata_msg::my_imgdata &Msg)
{
        InsertToBuffer(Msg);
        return 0;
}

int BufRead(string file_name, vector<imgdata_msg::my_imgdata> &MsgVec)
{
        imgdata_msg::Img_buf data;

    if(cache.contains(file_name))
    {
        data = cache.get(file_name);
    }
    else
    {
        uLong flen;  
            unsigned char* fbuf = NULL;  
            uLong ulen;  
            unsigned char* ubuf = NULL;

        FILE *file;
        if((file = fopen(file_name.c_str(), "rb")) == NULL)  
            {  
                printf("Can\'t open %s!\n", file_name.c_str());  
                return -1;  
            }
    
        fread(&ulen, sizeof(uLong), 1, file);
        fread(&flen, sizeof(uLong), 1, file);
        if((fbuf = (unsigned char*)malloc(sizeof(unsigned char) * flen)) == NULL)  
            {  
                printf("No enough memory!\n");  
                fclose(file);  
                return -1;  
            }  
            fread(fbuf, sizeof(unsigned char), flen, file);

        if((ubuf = (unsigned char*)malloc(sizeof(unsigned char) * ulen)) == NULL)  
            {  
                printf("No enough memory!\n");  
                fclose(file);  
                return -1;  
            }  
            if(uncompress(ubuf, &ulen, fbuf, flen) != Z_OK)  
            {  
                printf("Uncompress failed!\n");  
                return -1;  
            }  
            fclose(file);

        ros::serialization::IStream stream((uint8_t*)ubuf, ulen);
                ros::serialization::Serializer<imgdata_msg::Img_buf>::read(stream, data);

        cache.insert(file_name, data);

        delete[] ubuf;
            delete[] fbuf;
    }

    long i;
        for(i=0;i<data.write_buffer.size();i++)
        MsgVec.push_back(data.write_buffer[i]);

    return 0;
}

int main()
{
    
    long i;
        std::string actor_time = "201803";

        timeval write_time_start,write_time_end;
        gettimeofday(&write_time_start, NULL);

    for(i=0;i<1000;i++)
    {
                imgdata_msg::my_imgdata tmp;

                /*tmp.uav_pose.position.x=i+0.4;
          tmp.uav_pose.position.y=i+0.6;
          tmp.uav_pose.position.z=i+0.3;
          tmp.uav_pose.orientation.x=i+0.9;
          tmp.uav_pose.orientation.y=i+0.2;
          tmp.uav_pose.orientation.z=i+0.4;
          tmp.uav_pose.orientation.w=i+0.2;

        tmp.sensor_pose.position.x=i+0.4;
                tmp.sensor_pose.position.y=i+0.6;
                tmp.sensor_pose.position.z=i+0.3;
                tmp.sensor_pose.orientation.x=i+0.9;
                tmp.sensor_pose.orientation.y=i+0.2;
                tmp.sensor_pose.orientation.z=i+0.4;
                tmp.sensor_pose.orientation.w=i+0.2;*/

        tmp.actor_time = actor_time + std::to_string(i);
/*
        tmp.sensor_msg.header.seq=1;
        tmp.sensor_msg.header.stamp.sec=1;
        tmp.sensor_msg.header.stamp.nsec=1;
        tmp.sensor_msg.status.status=1;
        tmp.sensor_msg.status.service=2;
        tmp.sensor_msg.latitude=1.0;
        tmp.sensor_msg.longitude=2.0;
                */
                int a;
                long j;
                for(j=0;j<30000;j++)
        {
                        //tmp.data[j]=100;
                        a=100;
                        tmp.data.push_back(a);
        }    
                //tmp.sensor_msg.position_covariance_type=0;

                MsgStore(tmp);//write into file
    }
        gettimeofday(&write_time_end, NULL);
        long write_time = 1000000 * ( write_time_end.tv_sec - write_time_start.tv_sec ) + write_time_end.tv_usec - write_time_start.tv_usec;
        printf("write_time       : %ld\n", write_time);

        /*
    long read_time = 0;
    timeval read_time_start;
    timeval read_time_end;


        std::vector<imgdata_msg::my_imgdata> result;

    gettimeofday(&read_time_start, NULL);
        BufRead("/home/olding/Msg-File/201803100", result);
    gettimeofday(&read_time_end, NULL);
    read_time = 1000000 * ( read_time_end.tv_sec - read_time_start.tv_sec ) + read_time_end.tv_usec - read_time_start.tv_usec;
    printf("read_time       : %ld\n", read_time);

    result.clear();

    gettimeofday(&read_time_start, NULL);
        BufRead("/home/oldking/Msg-File/201803100", result);
        gettimeofday(&read_time_end, NULL);
        read_time = 1000000 * ( read_time_end.tv_sec - read_time_start.tv_sec ) + read_time_end.tv_usec - read_time_start.tv_usec;
        printf("read_time       : %ld\n", read_time);
*/
    return 0;
} 

参考：http://blog.youkuaiyun.com/u011906844/article/details/72156215