从零开始做自动驾驶定位(三): 软件框架

转载自知乎专栏：从零开始做自动驾驶定位(三): 软件框架
https://zhuanlan.zhihu.com/p/105512661

本文章配套源代码地址：https://github.com/Little-Potato-1990/localization_in_auto_driving

测试数据：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg
提取码: n9ys

本篇文章对应的代码Tag为 3.0

目录

• 1. 概述
• 2. ROS工程设计
• 3. 一个小例程
• 参考文献

1. 概述

为了写好我们这套定位系统，框架自然是首要考虑的事情，设计框架要结合需求和环境针对性地设计。
此处我们的环境就是ROS系统，而需求就是从我们上一篇文章制作的bag文件中接收各传感器信息和标定信息，以供算法使用，在算法运算完成以后把结果发送出去。
所以我们框架的基本就包括接收模块、发送模块和与将来编写的算法对接的接口。

• 接收模块，具体包括接收bag文件中GNSS信息、IMU信息、雷达点云信息和各传感器之间的标定信息。
• 发送模块，具体包括发送当前点云、全局地图、局部地图、里程计信息、载体运动轨迹等。
• 接口，具体就是要设计合理的数据结构，能够把算法需要的输入信息和算法的输出信息条例合理地规划好，以使输入输入输出更清晰方便。

除了以上功能以外，我们还需要一些小的技巧，以使工程结构更清晰，文件中代码也更清晰。当然其中一些可能只是我个人的一些使用习惯，各位如果有更好的习惯也欢迎交流。
本篇文章对应的工程包是工程文件中的lidar_localization

2. ROS工程设计

这一部分是本篇文章的重点，但是关于怎样使用ROS建立一个简单的工程包，这里不做详细介绍，网上的资料随处可见，了解不太多的读者麻烦先看一些资料，抱歉。
为了使工程架构更符合我们的任务需求，我们对工程做了一些改动，此处重点要介绍的就是这些改动部分，这可能和大家经常见到的工程包的结构不一样。

2.1 消息的订阅和发布

消息的订阅和发布大家应该不会陌生，这是每个ROS工程都必备的东西，我们常见的使用方式是在main函数中定义subscriber和publisher，每个subscriber会有一个callback函数与之对应。

• 缺点：如果订阅的topic比较多，那这个node文件就会充斥大量的callback函数，而且如果有些信息需要在callback内部做比较多的解析和处理，那这个node文件的代码长度会很长，这会影响程序的清晰度。
• 解决方案：我们把每一类信息的订阅和发布封装成一个类，它的callback做为类内函数存在，这样我们在node文件中想要订阅这个消息的时候只需要在初始化的时候定义一个类的对象，就可以在正常使用过程中从类内部直接取它的数据了。

这样用文字说可能比较抽象，我们找其中一个订阅类来举例子。
就用订阅GNSS信息的例子好了。代码中，它的头文件是gnss_subscriber.hpp，源文件是gnss_subscriber.cpp。在头文件中，类的声明如下：

class GNSSSubscriber {
  public:
    GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);
    GNSSSubscriber() = default;
    void ParseData(std::deque<GNSSData>& deque_gnss_data);

  private:
    void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr);

  private:
    ros::NodeHandle nh_;
    ros::Subscriber subscriber_;

    std::deque<GNSSData> new_gnss_data_;
};

其中msg_callback就是它的callback函数，也就是接收和处理信息的地方，它在源文件中的实现如下：

void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) {
    GNSSData gnss_data;
    gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec();
    gnss_data.latitude = nav_sat_fix_ptr->latitude;
    gnss_data.longitude = nav_sat_fix_ptr->longitude;
    gnss_data.altitude = nav_sat_fix_ptr->altitude;
    gnss_data.status = nav_sat_fix_ptr->status.status;
    gnss_data.service = nav_sat_fix_ptr->status.service;

    new_gnss_data_.push_back(gnss_data);
}

类中的函数ParseData就是实现从类里取数据的功能，在源文件中的实现如下：

void GNSSSubscriber::ParseData(std::deque<GNSSData>& gnss_data_buff) {
    if (new_gnss_data_.size() > 0) {
        gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end());
        new_gnss_data_.clear();
    }
}

经过这样的改造，我们在node文件中使用它时，只需要完成类对象定义和取数据两步：

// 定义类对象指针
std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
ros::Rate rate(100);
while (ros::ok()) {
    ros::spinOnce();
    //取数据
    gnss_sub_ptr->ParseData(gnss_data_buff);
    rate.sleep();
}

这样node文件中代码量就会大大减少，使程序更清晰。

2.2 传感器数据结构

每种传感器专门封装了对应的数据结构，在sensor_data文件夹下，目前有imu_data.hpp、gnss_data.hpp、cloud_data.hpp分别对应IMU数据、GNSS数据、点云数据。
这种封装就是为了适应一开始提到的接口功能，同时也可以配合第一步封装的订阅类和发布类使用，把订阅的数据直接封装好再供主程序取，这样封闭性更强。

2.3 缓冲区机制

这种机制完全是由于ROS自身的缺陷导致的，而且我在以前的试验中也多次遇到过这个问题。
这个问题和ROS订阅信息时缓冲区读取有关，ROS在每次循环时，会逐个遍历各个subscriber的缓冲区，并且把缓冲区中的数据读完，不管有多少。我们在subscriber的callback中解析数据的时候，一般都是把数据赋给一个变量，然后在融合的时候使用最后更新的值作为输入。
如果觉得不好理解，我们使用伪代码举一个小例子，假如目前有雷达和GNSS信息，我们要融合它：

gnss_callback {
  gnss 数据解析，赋给变量 gnss_data
}
lidar_callback {
  雷达数据解析，得到lidar_data
  融合(lidar_data, gnss_data)
}

这样看好像没什么问题，问题在于当融合算法处理时间比较长，超出了传感器信息的发送周期的时候，未被接收的数据会被放在每个subscriber对应的缓冲区中，等当前融合步骤处理完之后，下次ros从缓冲区中读取数据的时候，会先把gnss的数据读完，然后再读lidar的数据，这就导致，我们再一次进入lidar_callback函数时，使用的gnss_data已经不是和这个lidar_data同一时刻的数据了，而是它后面时刻的数据。

• 解决方案：不用单个变量来存储数据，而是用容器。
  • 各位这时候可以去第一步看我们举的那个GNSS信息订阅类的例子，在它的msg_callback函数里，信息解析完之后是放在一个deque容器里的。
  • 这样算法再使用数据的时候，应该从容器中去找。只不过找的时候要注意，多个传感器产生了多个容器，往算法模块里输入的时候，应该按照各容器第一个数据的时间戳，把最早的那个输入送进去，循环这个过程，直到所有容器数据送完为止。

2.4 CMakeLists文件规划

这一部分介绍的都是一些小的使用习惯，我认为这些习惯可以使CMakeLists更清晰。

1. 把各个包放在单独的cmake文件中
   调用一个包，就是常规三步：find_package，include_directions，target_link_libraries。
   有时候还需要一些判断，就加一些if else。
   这样也是同样的问题，包多的时候代码太杂。所以我们把每个包对应的这些操作放在cmake文件夹下对应的XX.cmake文件中，然后在CMakeLists中 include(cmake/XX.cmake)一行代码就可以搞定。

2. 合并变量
   为了避免target_link_libraries后面跟很长一串库的名字，而且库增减的时候它也得跟着增减，我们在CMakeLists文件一开始定义一个变量：

   set(ALL_TARGET_LIBRARIES "")
   

   然后在每个库对应的XX.cmake文件中，把库的名字合并到这个变量中去：

   list(APPEND ALL_TARGET_LIBRARIES ${XX_LIBRARIES})
   

   这样在target_link_libraries使就只使用ALL_TARGET_LIBRARIES这一个变量就好了。

   除了库对应的变量，还有文件名字对应的变量，我们在add_executable的时候要把所需要的cpp文件路径都要写进去，文件多的时候也是太麻烦，所以可以使用下面的指令把所有cpp文件合并到一个变量中：

   file(GLOB_RECURSE ALL_SRCS "*.cpp")
   

   但是，当工程中有多个node文件的时候，就要把他们从这个变量中踢出去，因为多个node文件编到一个可执行文件中会出错。
   用下面的代码踢：

   file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp")
   list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS})
   

2.5 GLog使用

GLog是google开源的代码日志开源库，它把信息分为INFO、WARNING、ERROR几个等级，使用时如果想添加日志信息，只需要一行代码就可以了：

LOG(INFO) << "自定义日志信息";

日志信息会自动存储在你定义的目录中。

总之使用起来还是比直接使用std::cout要方便很多，我们就把它加入到这个工程里面来了，麻烦各位装一个吧。

3. 一个小例程

啰嗦这么多，我们要试一下刚才设计的这些东西好不好使，这个小功能就是在播放bag文件的同时，把采集数据时车的轨迹实时显示出来，并且把当前点云也显示在车当前的位置上。

• 基本思路：订阅GNSS、IMU、lidar信息，然后把GNSS信息中的位置、IMU信息中的姿态信息解析出来，然后用odometry发布出去，把订阅的点云信息按照解析的位姿数据转换到当前车的位置和方向上，再发布出去。
• 该功能对应的node文件是test_frame_node.cpp，文件内容很简单，而且上面的介绍也零散提到一些，此处就不放代码了。

编译完成之后运行程序：

roslaunch lidar_localization test_frame.launch

最终实现的效果应该是这样的：
图3-1 最终实现效果

这时候看到的就不是原地不动的点云了，而是跟着车的运动前进的点云，而且轨迹实时更新显示。

最终bag播放完之后，轨迹效果是这样的：
图3-2 轨迹效果图

参考文献

[1] 从零开始做自动驾驶定位(三): 软件框架