淘先锋技术网

首页 1 2 3 4 5 6 7

从零开始做自动驾驶1 GNSS模块

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言

提示：跟随大神手敲一遍自动驾驶代码。

一、自动驾驶开源代码

来源：
https://zhuanlan.zhihu.com/p/105512661
记录出现的问题和进度

二、GNSS模块

1.GNSS 数据结构

头文件：

  • 类中为什么设置static 成员函数、成员变量的？static成员变量为啥类外初始化？
  • GeographicLib::localCartesia的作用是什么？感觉像是gnss坐标转换器
  • deque的底层实现是什么?常见用法？
  • namespace的原理和使用方法？
  • GNSS的数据结构

1.类中为什么设置static 成员函数、成员变量的？static成员变量为啥类外初始化？

为啥设置static成员变量：这个变量存储在静态存储区，每个类有只有一个副本 和对象无关 所有对象共用这个副本，因为是类对象 访问范围为该类。一般在外部类使用该static变量的时候就是用calssName::(static)varName来访问
为什么设置成员函数：与类实例无关。把类名当成namespace用。控制该函数的访问权限。控制类内的static变量。
static成员变量为啥类外初始化: static 非const成员变量在类中只是声明，必须在类外定义.静态数据成员实际上是类域中的全局变量。所以，静态数据成员的定义(初始化)不应该被放在头文件中,应放到对应的cpp文件。
其定义方式与全局变量相同,不要在头文件中定义(初始化)静态数据成员.
2.GeographicLib::localCartesia的作用是什么？
GeographicLib
.GeographicLib::localCartesia
将地理坐标系准变成xyz坐标系。
设置坐标原点，用地理坐标系初始化。
void GeographicLib::LocalCartesian::Reset ( real lat0,
real lon0,
real h0 = 0
)
将地理坐标系转换成xyz坐标系
void GeographicLib::LocalCartesian::Forward ( real lat,
real lon,
real h,
real & x,
real & y,
real & z
) const
3.deque的底层实现是什么?常见用法？
deque
4.namespace的原理和使用方法？
namespace
5.GNSS的数据结构

#include <deque>
#include "GeographicLib/src/LocalCartesian.hpp"

namespace lidar_localization { 
class GNSSData {
    private:
        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 updataXYZ();
        static bool SyncData(std::deque<GNSSData> unsyncedData, std::deque<GNSSData> syncedData, double sync_time);

};
}

源文件：

#include "lidar_localization/sensor_data/gnss_data.hpp"
#include "glog/logging.h"

bool lidar_localization::sensor_data::origin_position_inited = false;
GeographicLib::LocalCartesian lidar_localization::sensor_data:: geo_converter;

namespace lidar_loacalization{

void GNSSData::InitOriginPosition() {
    geo_converter.Reset(latitude, longtitude, altitude);
    origin_position_inited = true;
}

void GNSSData::updataXYZ(){
    if (!origin_position_inited){
        LOG(WARNING) << "GeoConverter has not not set origin position!";
    }
    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
}

bool GNSSData::SyncData(std::deque<GNSSData>& UnsyncedData, std::deque<GNSSData>& SyncedData, double sync_time) {
    // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置
    // 即找到与同步时间相邻的左右两个数据
    // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值
    while (UnsyncedData.size() >= 2) {
        if (UnsyncedData.front().time > sync_time)
            return false;
        if (UnsyncedData.at(1).time < sync_time) {
            UnsyncedData.pop_front();
            continue;
        }
        if (sync_time - UnsyncedData.front().time > 0.2) {
            UnsyncedData.pop_front();
            return false;
        }
        if (UnsyncedData.at(1).time - sync_time > 0.2) {
            return false;
        }
        break;
    }
    if (UnsyncedData.size() < 2)
        return false;

    GNSSData front_data = UnsyncedData.at(0);
    GNSSData back_data = UnsyncedData.at(1);
    GNSSData synced_data;

    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
    double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
    synced_data.time = sync_time;
    synced_data.status = back_data.status;
    synced_data.longitude = front_data.longitude * front_scale + back_data.longitude * back_scale;
    synced_data.latitude = front_data.latitude * front_scale + back_data.latitude * back_scale;
    synced_data.altitude = front_data.altitude * front_scale + back_data.altitude * back_scale;
    synced_data.local_E = front_data.local_E * front_scale + back_data.local_E * back_scale;
    synced_data.local_N = front_data.local_N * front_scale + back_data.local_N * back_scale;
    synced_data.local_U = front_data.local_U * front_scale + back_data.local_U * back_scale;

    SyncedData.push_back(synced_data);
    
}
}

其中比较重要的是传感器数据的时间同步：
1.传感器数据按时间序列存储，为同步的时间点找到相应的位置;
2.即找到相应时刻的相邻两个数据;
3.值得注意的是相邻数据有一个离同步的时刻间隔太远，证明有数据的丢失，时间间隔太远则不适合进行插值;
算法逻辑：（t0, t1,t，tx:更新频率）
1.对数据进行遍历处理，直到找到同步时刻相邻的数据; 若队列小于2，仍未找到，则结束遍历
2. 如果t0 > t 则直接返回false, 不然就满足了 t0 < t;如果 t1 < t 则将队首元素弹出，直到找到t < t1;如果t-t0 >tx**,则弹出t0，**并结束;如果t1 - t >tx ,则返回并退出;
3. 找到相邻两元素后，进行线性插值计算：
在这里插入图片描述

2.GNSS的订阅模块

头文件：

 
#ifndef LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_
#define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_

#include <ros/ros.h>
#include "sersor_msgs/NavSatFix.h"
#include "lidar_localization/sensor_data/gnss_data.hpp"
#include <deque>

namespace lidar_localization {
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::NavSatFixPtr& nav_sat_fix_ptr);
private:
    ros::NodeHandle nh_;
    ros::Subscriber subscriber_;
    std::deque<GNSSData> new_gnss_data;
};
}
#endif // !LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP

源文件：

#include "lidar_localization/subscriber/gnss_subscriber.hpp"

#include "glog/logging.h"

namespace lidar_localization {
GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 
    :nh_(nh) {
    subscriber_ = nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this);
}

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);
}

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();
    }
}
}

其中：ParseData函数是用来读取接收到的传感器数据，类似于缓冲区

三、点云模块

1.CloudData数据结构

总结