如何在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.csdn.net/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

  message_generation

  message_runtime

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

如何在ROS中定义并使用自己的消息类型-以image类型为例_第1张图片

下面建立第二个消息类型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"

如何在ROS中定义并使用自己的消息类型-以image类型为例_第2张图片

5 source devel/setup.bash

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

如何在ROS中定义并使用自己的消息类型-以image类型为例_第3张图片

使用自定义的消息类型:

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

#include

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


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

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

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#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 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 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 &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::read(stream, data);

        cache.insert(file_name, data);

        delete[] ubuf;
            delete[] fbuf;
    }

    long i;
        for(i=0;i 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;
} 

如何在ROS中定义并使用自己的消息类型-以image类型为例_第4张图片

如何在ROS中定义并使用自己的消息类型-以image类型为例_第5张图片

参考:http://blog.csdn.net/u011906844/article/details/72156215

你可能感兴趣的:(ROS)