项目实训 - 智能车系统 - 第六周记录

项目实训 - 智能车系统 - 第六周记录

日期：3.28 – 4.03

项目进度

本周工作进展：

• 完成了所有中间话题的移植
• 将参数服务器改成读取文件的形式

（这周状态不太好，工作进展不是很大。，。）

1、剩余话题的移植

上周完成了TF框架的搭建，这周开始着手其他话题的移植。

先不管数据包发布的原始数据、以及发布给rviz的话题，剩余的节点之间的话题还有3个

• /odometry/imu_Incremental
  • imu节点发布；imu节点、image节点接收（imu节点中维护了两个类，是一个类发布的话题，另一个类订阅）
• /lio_sam/mapping/odometry_Incremental
  • map节点发布；imu节点接收
• /lio_sam/mapping/odometry
  • map节点发布；imu节点接收

现在开始对这三个节点的移植。

对于这三个话题的移植具体过程不做具体说。展示一下移植后的流程图：

在移植过程中遇到了一些问题，在后面的bug记录中记录。

2、参数服务器移植

ros原本的参数服务器机制：将一些参数记录在.yaml文件中，启动ros项目时，参数服务器读取并解析yaml文件中的数据，赋值给变量，以便于每个节点使用。

我的做法是：使用简单的.txt文本文件替代yaml文件，通过文件流读取txt文件，按照我自己指定的规范解析参数；然后让每个类继承读取类

• 后面知道c++有处理yaml文件的库，但是已经完成了我自己设想的做法，所以后续如果有时间的话会使用给yaml库重写

utility_struct.txt:

utility_struct.h

// 从文件中读取
    ParamServer_struct()
    {
        ifstream infile;
        infile.open("/home/cislc2019/catkin_ws/src/LIO-SAM-master/config/utility_struct.txt");
        if(!infile) {
            cout<<"open utility config files error!"<<endl;
            return ;
        }
        //读取最原始的文本数据，然后开始解析
        string temp_name;
        while(infile>>temp_name) 
        {
            inital_data_from_file.push_back(temp_name);
        }
        for(int i = 0; i < inital_data_from_file.size(); i++)
        {
            string key;
            string value;
            splitString(inital_data_from_file[i], key, value);
            key_value.insert(std::make_pair(key, value));
        }
//splitString函数：将字符串解析成键值对

    void splitString(string init_str, string& key_str, string& value_str)
    {
        int middle_number = init_str.find(":");
        key_str = init_str.substr(0,middle_number);
        value_str = init_str.substr(middle_number + 1,init_str.length() - middle_number - 1);
    }
        
//定义了一些类解析函数，按照不同类型将数据进行解析
    }
    /**
     * @brief 给string类型的变量赋值
     * 搜索vector，如果有对应的key，则转换成对应类型后赋值；否则赋默认值
     * 
     * @param key 参数名称
     * @param out 变量
     * @param default_value 默认参数值
     */
    void param_string(string key, string &out, string default_value) 
    {
        auto search = key_value.find(key);
        if(search != key_value.end()) {
            string value = search->second;
            
            //string不需要转换 直接赋值
            for(int i = 0; i < value.size(); i++)
            {
                if(value[i] == '-') value[i] = '/';
            }
            out = value;
            cout<<out<<endl;
        }
        else {
            //没有搜索到，赋值默认值 
            out = default_value;
        }
    }

    void param_bool(string key, bool &out, bool default_value)
    {
        auto search = key_value.find(key);
        if(search != key_value.end()) {
            string value = search->second;
            
            if(value == "true") {
                out = true;
            } else {
                out = false;
            }
        } 
        else {
            out = default_value;
        }
        cout<<out<<endl;
    }

    void param_float(string key, float &out, float default_value)
    {
        auto search = key_value.find(key);
        if(search != key_value.end()) {
            string value = search->second;
            //string转float
            float real_value = stof(value);
            out = real_value;
        } 
        else {
            out = default_value;
        }
        cout<<out<<endl;
    }
......

技术难点

正常工作

bug记录

bug1 ros::Time定义错误

发现是因为ros::Time类型中的sec和nsec是uint32_t，而我之前一直用的是int32_t

bug2 如何解决接收端connect函数晚于create函数的问题

由于多进程且多话题的原因，会出现接收端早于发布端初始化的情况。

如何解决接收端connect函数晚于create函数的问题

• 先connect的话，得到的共享内存首地址会与后面的create的首地址不一样（其实是建立了两块共享内存）
• 解决方案：在connect的多线程函数里，在第一次执行实际代码段之前一直重复连接，并定义一个标记变量由于判断，
• 当发现可以进入实际代码段后，意味着匹配到了真正的create出来的首地址
• 这时候就可以修改标记变量，不在重复连接

    while(true) 
    {
        if(!is_real_point) { //判断是否连接到了正确的共享内存
            unMap(subLaserOdometry_sharemem, sizeof(nav_msgs_Odometry_List));
            subLaserOdometry_sharemem = connectShareMemory("/home/cislc2019/catkin_ws/src/LIO-SAM-master/src/communication_LVI_SAM/files/mapOptmization_files/lio_sam-mapping-odometry",sizeof(nav_msgs_Odometry_List));
            queue = (nav_msgs_Odometry_List* ) subLaserOdometry_sharemem->point;
        }
        now_number = queue->count;
        if(now_number != last_number)
        {     
            is_real_point = true; //修改标记，不再进行连接操作
            ...

bug3 话题的发布和接收频率不匹配问题

问题：话题的发布频率较快，而处理函数速度较慢，导致频率无法匹配

• 在回调函数中加了锁，所以可以认为即使是多线程处理回调函数，同一时刻也应该只有一个回调函数再执行
• 在我们自定义的函数中，回调函数的处理频率会逐步落后于发布端
• 但是ros原生机制下，则不会落后

经过观察打印后发现：发送端存在等待机制

• 会有一个发送端的时间间隔远大于其他打印
• 结合tcp的确认接收机制
• 以及noDelay设置为true和false，雀氏会影响send的等待频率

解决方案

• 在发送端使用一个变量强制阻塞发送端，直到处理完成
• 具体机制如下
  • 定义一个全局变量：表示接收端的处理进度
  • 在发送端定义一个类变量，标识当前发送的进度
  • 当二者差值达到一定数值后，阻塞发送端，直到进度相同
  • 差值：5
  • 注意，没写判定是否int越界的：暂时也不用

但是当时导致项目跑不通的真正原因时把tf删了，rviz搜索不到base_link了

但是雀氏意外的发现了ros的一些隐藏的小机制

• ros的topic话题机制使用的是tcp通信
  • 正常情况下，如果发送队列阻塞的数目达到一定数量，即使没有到达发送队列的预设长度，也会阻塞发送，直到接收端完全处理所有的缓冲数据；
    • 针对这个现象，虽然没有读源码，但是分析应该是tcp的确认帧机制导致的发送队列阻塞（计网没白学）
  • 对于sub函数里的一个参数： ros::TransportHints().tcpNoDelay(false/true)，用于设置是否对tcp进行较低延迟的传输
    • 虽然对于官方的解释没咋看懂，但现象是：使用这个参数会使发送端的缓冲数据在较少的情况下也会阻塞
      • 相当于减少了发送端缓冲队列的拥挤程度

经过测试，发送端不进行自定义的阻塞也可以运行，但是为了更加的还原原始项目，所以保留了发送端的阻塞

bug4 O3优化问题

有关多线程的编译优化的问题。

当项目开了O3优化后，会对项目做出一些比较激进的优化

• 之前一直存在的问题：

• 问题代码：

      int last_number = 0;
      int now_number = 0;
      int now_pos = 0;
  
      lio_sam_msgs_cloud_info_List*  queue_cloud_info = (lio_sam_msgs_cloud_info_List* ) subCloud_sharemem->point;
      while(true) {
  
              now_number = queue_cloud_info->count;
              now_pos = queue_cloud_info->now;
  
              //now_number = 1;
              // if(number == 0) {
              //     std::cout<<number<<' '<<now_number<<'m';
              //  number++;
              // }
  
          if(now_number != last_number) {
              //std::cout<<"sub start 11"<<endl;
  
              // 说明有数据更新了
              last_number = now_number;
              ...
          }
      }
  

  经过O3优化后，if(now_number != last_number) { 被优化掉，一直不执行（之前的博客记录过）。

  原因是知道的：第5行的赋值操作，优化认为这个赋值一直没有变化过，所以后续不会进入if

  • 但是实际上这地方是通过共享内存来进行赋值，所以实际上值是会变化的，但是优化认为没有变化

• 解决：在第五行的queue_cloud_info前加一个关键字volatile，该关键字的作用是强制声明该变量是会变化的，从而不会将下面的死代码优化掉

  • 小插曲：之前在now_number前加过volatile，没用（经过师兄的点拨发现问题其实在共享内存那里）

另一个问题：

• 跟楼上的问题类似，也是O3优化将死代码优化掉

     if(tcp_publisher_number - tcp_subscriber_number > 5) {
          //认为发送端应该等待接收端了 所以进行阻塞
          while(tcp_publisher_number != tcp_subscriber_number + 1) {
               //cout<<"in send : "<<tcp_publisher_number<<' '<<tcp_subscriber_number<<endl;
              //在这个过程中接收端会逐渐处理 缓存的数据，知道处理完成接收的所有数据，开始等待
              // if(temp_num == 0) {
              //     cout<<"in send cout"<<tcp_publisher_number<<' '<<tcp_subscriber_number<<endl;
              //     temp_num++;
              // }
              ;
          }
      }
  

• tcp_publisher_number和tcp_subscriber_number都是全局变量

• 这里的tcp_subscriber_number不会在当前函数内被修改，而是在同一时间执行的某函数（多线程）修改，但是优化还是认为这个值不会被修改，所以继续优化掉

• 解决：

  volatile int tcp_publisher_number = 0;
  

到今天为止

• 所有的中间话

其他

到第六周为止的工作总结

• 所有的中间话题都已经移植完成
• 自定义的tf框架也已经写出来了，后续可以代替ros的tf机制
• 重写了一些pcl和tf库的类和函数
• 重写了ros里的一些函数
• 定义了若干用于中间移植的临时的函数
• 给可视化（我的上游）写了个小的数据转换接口
• 重写了参数服务器

后续工作

• 传感器获取的数据的处理
  • 雷达
  • imu
• 与可视化工具的对接