项目实训第六周记录

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，出现部分段错误的情况待修复。