知乎:从零开始做自动驾驶定位；注释详解（一）

消息发布与订阅

针对这个问题，作者把每一类信息的订阅和发布封装成一个类，它的callback做为类内函数存在，这样我们在node文件中想要订阅这个消息的时候只需要在初始化的时候定义一个类的对象，就可以在正常使用过程中从类内部直接取它的数据了。

以订阅GNSS信息为例子，代码中，它的头文件是gnss_subscriber.hpp，源文件是gnss_subscriber.cpp。在头文件中，类的声明如下：

/** @Description: * @Author: Ren Qian* @Date: 2019-03-31 12:58:10*/
#ifndef LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_
#define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_#include <deque>
#include <ros/ros.h>
#include "sensor_msgs/NavSatFix.h"
#include "lidar_localization/sensor_data/gnss_data.hpp" //这里是包含gnss信息类型的头文件namespace lidar_localization { //namespace
class GNSSSubscriber {public:GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);//带参构造函数GNSSSubscriber() = default; // 注释1.1/*函数ParseData就是实现从类里取数据的功能*/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_;/*注释1.2*/std::deque<GNSSData> new_gnss_data_; //这里是定义GNSSData类型的对象
};
}
#endif

注释1.1
注释1.2

2. 缓冲区机制

这种机制完全是由于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容器里的（这样后续可以在容器里面寻找对应当前clouddata时间戳的传感器数据，而不是只能用最新的数据）。

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->longitude;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); //这个是队列/*  std::deque<GNSSData> new_gnss_data_;  */
}

这样算法再使用数据的时候，应该从容器中去找。只不过找的时候要注意，多个传感器产生了多个容器，往算法模块里输入的时候，应该按照各容器第一个数据的时间戳，把最早的那个输入送进去，循环这个过程，直到所有容器数据送完为止。

经过这样的改造，我们在node文件中使用发布和订阅的时候，只需要完成类对象定义和取数据两步（订阅对象subscriber_会在定义对象的时候，自动调用构造函数，然后在构造函数里面给定消息名称和回调函数）：

// 定义类对象指针
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文件中代码量就会大大减少，使程序更清晰。

3. CMakeLists文件规划

注释：

注释1.1
注释1.1：对于 `GNSSSubscriber() = default;`的注释：

  /** 在C++中约定如果一个类中自定义了带参数的构造函数，那么编译器就不会再自动生成默认构造函数* 也就是说该类将不能默认创建对象，只能携带参数进行创建一个对象；* 但有时候需要创建一个默认的对象但是类中编译器又没有自动生成一个默认构造函数，* 那么为了让编译器生成这个默认构造函数就需要default这个属性。*     * 无论是显式的默认构造函数（=default），还是隐式合成的默认构造函数（编译器生成），* 都是用来控制默认初始化过程的。它按照如下规则初始化类的数据成员：* 1.如果存在类内的初始值，用它来初始化成员。* 2.如果不存在类内的初始值，默认初始化该成员。*/GNSSSubscriber() = default;

返回原阅读位置

注释1.2
注释1.2：对于 `std::deque<GNSSData> new_gnss_data_;`我们在这里定义了`GNSSData`类型的队列对象，是因为，对于每一种传感器信息，作者都专门专门封装了对应的数据结构，在sensor_data文件夹下，目前有imu_data.hpp、gnss_data.hpp、cloud_data.hpp分别对应IMU数据、GNSS数据、点云数据。

这种封装就是为了适应一开始提到的接口功能，同时也可以配合第一步封装的订阅类和发布类使用，把订阅的数据直接封装好再供主程序取，这样封闭性更强。

以gnss_data 为例，gnss_data.hpp如下：

/** @Description: * @Author: * @Date: 2019-07-17 18:25:13*/
#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_#include <deque>#include "Geocentric/LocalCartesian.hpp"namespace lidar_localization{
class GNSSData {
public:double time = 0.0;double longitude = 0.0;double latitude = 0.0;double altitude = 0.0;double local_E = 0.0;double local_N = 0.0;double local_U = 0.0;int status = 0;int service = 0;private:static GeographicLib::LocalCartesian geo_converter;//查一下static bool origin_position_inited;public: void InitOriginPosition();void UpdateXYZ();static bool SyncData(std::deque<GNSSData>& UnsyncedData, std::deque<GNSSData>& SyncedData, double sync_time);};
}

基本同样的形式，点云数据的为：

namespace lidar_localization {
class CloudData {public:using POINT = pcl::PointXYZ; //点using CLOUD = pcl::PointCloud<POINT>; //由点构成的点云using CLOUD_PTR = CLOUD::Ptr;  //点云指针public:CloudData()     //构造函数:cloud_ptr(new CLOUD()) {    //初始化点云指针}public:double time = 0.0;  //时间戳?CLOUD_PTR cloud_ptr;
};
}

所有传感器的数据，我们都会封装成一个类作为接口，来方便使用。除了点云数据，每个传感器的类中都会有一个SyncData()公有函数,这个函数主要用来同步数据，以IMU数据为例：

bool IMUData::SyncData(std::deque<IMUData>& UnsyncedData, std::deque<IMUData>& SyncedData, double sync_time) 
{// 传感器数据按时间序列排列，在传感器数据中为 需要同步的时间 点找到合适的时间位置// 即找到与 需要同步的时间 相邻的左右两个数据// 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值while (UnsyncedData.size() >= 2) {//这个判断条件,保证UnsyncedData容器里大于两个元素//T& front() :返回容器中第一个元素的引用if (UnsyncedData.front().time > sync_time) //如果第一个元素的时间比同步时间大(即在需要同步的时间后面),即插入时刻的前面没有数据，那么就无从插入，直接退出return false;if (UnsyncedData.at(1).time < sync_time) {//上一个语句,没有ruturn说明第一个数据比插入时刻早，此句判断若第二个数据[UnsyncedData.at(1)]也比插入时刻早，那么第一个时刻的数据是没意义的，应该接着往下找，并删除第一个数据UnsyncedData.pop_front();//pop_front(): 删除容器开头元素 ,这样容器内元素整体向前移动一个continue;}if (sync_time - UnsyncedData.front().time > 0.2) { //0.2s内//如果雷达采集时刻已经处在前两个数据的中间了，但是第一个数据时刻与雷达采集时刻时间差过大，那么中间肯定丢数据了，退出UnsyncedData.pop_front();return false;}if (UnsyncedData.at(1).time - sync_time > 0.2) {//同样，如果第二个数据时刻与雷达采集时刻时间差过大，那么也是丢数据了，也退出UnsyncedData.pop_front();//这里面应该是把(0)删除了,(1)留下了吧return false;}break;}/*上面这段就是索引所需要的四个步骤，核心思想是让容器第一个数据时间比插入时刻早，第二个数据时间比插入时刻晚：1）如果第一个数据时间比雷达时间还要靠后，即插入时刻的前面没有数据，那么就无从插入，直接退出2）如果第一个数据比插入时刻早，第二个数据也比插入时刻早，那么第一个时刻的数据是没意义的，应该接着往下找，并删除第一个数据3）如果雷达采集时刻已经处在前两个数据的中间了，但是第一个数据时刻与雷达采集时刻时间差过大，那么中间肯定丢数据了，退出4）同样，如果第二个数据时刻与雷达采集时刻时间差过大，那么也是丢数据了，也退出以上四个限制条件如果都通过了，那么就算是找到对应位置了。*/if (UnsyncedData.size() < 2)return false;IMUData front_data = UnsyncedData.at(0); IMUData back_data = UnsyncedData.at(1);   IMUData synced_data;
/* 线性插值大家都懂，有两个数据a和b，时刻分别是0和1，那么时间t（0<t<1）时刻的插值就是a*(1-t)+b*t。第一个点: (0,a) ;第二个点(1,b)；中间点(t,y),根据线性插值:(y-a)/(t-0) = (b-a)/(1-0) -> y-a=(b-a)*t -> y= (b-a)*t +a = a*(1-t)+b*t*/double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);//(1-t)double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);//tsynced_data.time = sync_time;//a*(1-t)+b*t//front_data.linear_acceleration.x :a//back_data.linear_acceleration.x: bsynced_data.linear_acceleration.x = front_data.linear_acceleration.x * front_scale + back_data.linear_acceleration.x * back_scale;synced_data.linear_acceleration.y = front_data.linear_acceleration.y * front_scale + back_data.linear_acceleration.y * back_scale;synced_data.linear_acceleration.z = front_data.linear_acceleration.z * front_scale + back_data.linear_acceleration.z * back_scale;synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale;synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale;synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale;// 四元数插值有线性插值和球面插值，球面插值更准确，但是两个四元数差别不大是，二者精度相当// 由于是对相邻两时刻姿态插值，姿态差比较小，所以可以用线性插值// 旋转角度用的四元数: q = w + xi + yj + zksynced_data.orientation.x = front_data.orientation.x * front_scale + back_data.orientation.x * back_scale;synced_data.orientation.y = front_data.orientation.y * front_scale + back_data.orientation.y * back_scale;synced_data.orientation.z = front_data.orientation.z * front_scale + back_data.orientation.z * back_scale;synced_data.orientation.w = front_data.orientation.w * front_scale + back_data.orientation.w * back_scale;// 线性插值之后要归一化;四元数归一化synced_data.orientation.Normlize();SyncedData.push_back(synced_data);return true;
}