我们要在板子上测试其图像处理能力,要使用一个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
<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.csdn.net/u011906844/article/details/72156215