2022.4.4
今天尝试的写了一个demo,模拟含有指针的结构体作为要保存的数据,尝试了数据包中数据部分单个消息的写入和读取,发现了一些指针相关的问题,并对数据包格式作了修正。
2022.4.5
再昨天的基础之上,添加了头信息的读写,测试了整个流程,验证了数据包格式的可行性,但需要对数据包部分格式做更细致的规划,才能进行更系统的代码编写。
demo部分由三个文件组成。
首先是data.h文件,描述了数据格式以及部分方法。
#include<iostream>
struct geometry_msgs_Quaternion
{
double x;
double y;
double z;
double w;
};
struct geometry_msgs_Point
{
double x;
double y;
double z;
};
struct geometry_msgs_Pose
{
geometry_msgs_Point position;
geometry_msgs_Quaternion orientation;
};
struct geometry_msgs_PoseStamped
{
geometry_msgs_Pose pose;
};
struct nav_msgs_Path
{
uint32_t poses_number;
geometry_msgs_PoseStamped* poses;
};
int string_to_int(char * buf,int len)
{
int ans = 0;
for(int i = 0; i < len; ++ i)
{
ans = ans * 10 + buf[i] - '0';
}
return ans;
}
然后是filewrite.cpp文件,负责将数据写成数据包的形式。
#include <fstream>
#include <iostream>
#include "data.h"
using namespace std;
int main()
{
int pose_number = 10;
geometry_msgs_PoseStamped* poses = new geometry_msgs_PoseStamped[pose_number];
uint64_t datas_len = 0;
for(int i = 0; i < pose_number; ++ i)
{
poses[i].pose.position.x = (double)i/pose_number;
poses[i].pose.position.y = (double)i/pose_number + 1;
poses[i].pose.position.z = (double)i/pose_number + 2;
poses[i].pose.orientation.x = (double)i/pose_number;
poses[i].pose.orientation.y = (double)i/pose_number + 1;
poses[i].pose.orientation.z = (double)i/pose_number + 2;
poses[i].pose.orientation.w = (double)i/pose_number - 10;
}
// 创建流
ofstream outfile;
// 初始化流文件,启用写入且覆盖模式,防止文件已经存在
nav_msgs_Path* path = new nav_msgs_Path;
path->poses_number = pose_number;
path->poses = poses;
// 最终采用的二进制模式
//outfile.open("test.bag",ios::out | ios::trunc | ios::binary);
// 测试用的正常模式
outfile.open("test.bag",ios::out | ios::trunc | ios::binary);
// 防止文件打开失败
if(!outfile)
{
cout<<"error"<<endl;
return -1;
}
cout<<"open success!"<<endl;
char header[4096] = {' '};
// 输出头部信息占用的缓冲区
outfile.write(header,sizeof(header));
uint32_t msg_header_len = 0,field_len,field_len1;
// 存字符串还是字符数组呢?有待思考
char buf[] = "topic=nav_msgs_Path";
field_len = sizeof(buf);
msg_header_len += 4 + field_len;
char ptr_num[] = "ptr_num=1";
field_len1 = sizeof(ptr_num);
msg_header_len += 4 + field_len1;
cout<<"start to write!"<<endl;
datas_len += msg_header_len + 4;
// 先写头部信息
outfile.write((char *)&msg_header_len,sizeof(msg_header_len));
cout<<msg_header_len<<endl;
outfile.write((char *)&field_len,sizeof(field_len));
cout<<field_len<<endl;
outfile.write(buf,sizeof(buf));
outfile.write((char *)&field_len1,sizeof(field_len1));
cout<<field_len1<<endl;
outfile.write(ptr_num,sizeof(ptr_num));
// 再写数据长度
uint32_t data_len = sizeof(path);
outfile.write((char *)&data_len,sizeof(data_len));
datas_len += 4;
// 再写数据信息
outfile.write((char *)path,data_len);
datas_len += data_len;
// 再写指针域数据信息
// 先写指针长度
uint32_t ptr_sz = pose_number * sizeof(geometry_msgs_Pose);
outfile.write((char *)&ptr_sz,sizeof(ptr_sz));
// 再写指针数据
outfile.write((char *)poses,pose_number * sizeof(geometry_msgs_Pose));
outfile.flush();
datas_len += ptr_sz + 4;
// 回过头来重新写头部信息
// 重定位到文件头部
outfile.seekp(0,ios::beg);
// 编写头部信息
uint32_t header_len = 0;
uint32_t header_field_len1;
char op2[] = "topic=nav_msgs_Path";
header_field_len1 = sizeof(op2);
header_len += 4 + header_field_len1;
outfile.write((char *)&header_len,sizeof(header_len));
outfile.write((char *)&header_field_len1,sizeof(header_field_len1));
outfile.write(op2,header_field_len1);
// 填写数据长度
outfile.write((char *)&datas_len,sizeof(datas_len));
cout<<datas_len<<endl;
outfile.flush();
cout<<"finish write!"<<endl;
// 关闭文件
outfile.close();
delete []poses;
delete path;
}
最后是fileread.cpp文件,负责将数据包中的数据读出,形成结构体方便接下来的使用。
#include <fstream>
#include <iostream>
#include <cstring>
#include "data.h"
using namespace std;
int main()
{
ifstream inFile("test.bag",ios::in | ios::binary);
if(!inFile)
{
cout<<"error"<<endl;
return -1;
}
// 读取头部信息部分
uint32_t header_len;
inFile.read((char *)&header_len,sizeof(header_len));
cout<<"header_len: "<<header_len<<endl;
while(inFile.tellg() < 4 + header_len)
{
uint32_t field_len;
inFile.read((char *)&field_len,sizeof(field_len));
int pos = 0;
// 规定标准
char field_buf[64];
inFile.read(field_buf,field_len);
int k;
for(int i = 0;field_buf[i];i ++)
{
if(field_buf[i] == '=')
{
k = i;
break;
}
}
char name[32];
memcpy(name,field_buf,k);
if(strcmp(name,"topic") == 0)
{
cout<<"name: topic ";
char value[32];
memcpy(value,field_buf + k + 1,field_len - k - 1);
printf("value: %s\n",value);
}
}
uint64_t datas_len;
inFile.read((char *)&datas_len,sizeof(datas_len));
cout<<"data_len: "<<datas_len<<endl;
// 读取数据部分
inFile.seekg(4096,ios::beg);
uint32_t msg_header_len;
inFile.read((char *)&msg_header_len,sizeof(msg_header_len));
cout<<msg_header_len<<endl;
//char buf[512];
// 读取整个头部信息,接下来进行解析
int cur_size = 0;
int ptr_num = 0;
while(cur_size < msg_header_len)
{
uint32_t len;
inFile.read((char *)&len,sizeof(len));
cur_size += 4 + len;
char buf[32];
inFile.read(buf,len);
int k = 0;
// 分割字符串
for(int i = 0; i < len; ++ i)
{
if(buf[i] == '=')
{
k = i;
break;
}
}
char name[32];
memcpy(name,buf,k);
if(strcmp(name,"topic") == 0)
{
cout<<"name: topic ";
char value[32];
memcpy(value,buf + k + 1,len - k - 1);
printf("value: %s\n",value);
}
else if(strcmp(name,"ptr_num") == 0)
{
cout<<"name: ptr_num ";
// uint32_t value;
// memcpy((char *)&value,buf + k + 1,len - k);
// 这里的数字并非由uint32_t转成的,因此需要由字符串转换为数字,而非字节转换为数字
// 抹去终止符
int value = string_to_int(buf + k + 1,len - k - 2);
ptr_num = value;
cout<<"value: "<<value<<endl;
}
else
{
printf("%s\n",buf);
}
}
// 读取数据长度
uint32_t data_len;
inFile.read((char *)&data_len,sizeof(data_len));
// 读取结构体数据
nav_msgs_Path* path = new nav_msgs_Path;
inFile.read((char *)path,data_len);
// 读取完结构体后似乎读取不到数据? 原来是忘了读取数据长度了
geometry_msgs_PoseStamped* poses;
// char bufs[1024];
// inFile.read(bufs,1024);
// for(int i = 0; i < 1024; ++ i) cout<<bufs[i];
//cout<<endl;
// 读取指针数据
for(int i = 0; i < ptr_num; ++ i)
{
uint32_t ptr_sz;
inFile.read((char *)&ptr_sz,sizeof(ptr_sz));
cout<<ptr_sz<<endl;
poses = new geometry_msgs_PoseStamped[path->poses_number];
inFile.read((char *)poses,ptr_sz);
path->poses = poses;
}
for(int i = 0; i < path -> poses_number; ++ i)
{
double x,y,z,w;
x = path->poses[i].pose.position.x;
y = path->poses[i].pose.position.y;
z = path->poses[i].pose.position.z;
cout<<"position x:"<<x<<" y:"<<y<<" z:"<<z<<endl;
x = path->poses[i].pose.orientation.x;
y = path->poses[i].pose.orientation.y;
z = path->poses[i].pose.orientation.z;
w = path->poses[i].pose.orientation.w;
cout<<"orientation x:"<<x<<" y:"<<y<<" z:"<<z<<" w:"<<w<<endl;
}
inFile.close();
delete []poses;
delete path;
}
2022.4.6
由于通信机制发生了变化,用组员给的接口文件修改了通信机制,对可视化部分进行了调试,完成了新通信机制的适配。由于想给数据包添加压缩功能,于是去学习了boost的iostreams的相关内容,编写了一个demo,编译费了不少功夫才完成,成功测试了bz2的压缩流。
2022.4.7
构建了消息和队列的框架,并进行了测试。
2022.4.8
修正了消息框架和队列框架中存在的问题。
2022.4.9
完善记录器框架,制定了记录器的整个流程。
2022.4.10
实现接收者的功能,将接收到的消息传往队列等待下一步处理。但由于指针较多,需要较多的new和delete,出现部分段错误的情况待修复。