GVINS学习记录1-消息订阅

sports_field.bag 的消息类型

xmu@xmu-NUC8i7BEH:~/data$ rosbag info sports_field.bag 
path:        sports_field.bag
version:     2.0
duration:    25:10s (1510s)
start:       Nov 30 2020 07:18:13.45 (1606691893.45)
end:         Nov 30 2020 07:43:23.61 (1606693403.61)
size:        20.5 GB
messages:    412007
compression: none [20137/20137 chunks]
types:       gnss_comm/GnssEphemMsg         [b4346f99e431c870e392a438684c0fe8]
             gnss_comm/GnssGloEphemMsg      [6893dee0d8b1c9325a6693f3488e0144]
             gnss_comm/GnssMeasMsg          [0eafeb530a3e6637722c4e296734c119]
             gnss_comm/GnssPVTSolnMsg       [d18171357d7a159f76d4d7c0b12fb631]
             gnss_comm/GnssTimePulseInfoMsg [de306035e295e3c4b1e0771a216b495a]
             gnss_comm/StampedFloat64Array  [fb60495edd59d3fcf90e173153ae8a9a]
             gvins/VISensorExternalTrigger  [a5f284fbc0b54d0c397033867d8565f1]
             sensor_msgs/Image              [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu                [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/NavSatFix          [2d3a8cd499b9b4a0249fb98fd05cfa48]
topics:      /cam0/image_raw                  30204 msgs    : sensor_msgs/Image             
             /cam1/image_raw                  30205 msgs    : sensor_msgs/Image             
             /external_trigger                  302 msgs    : gvins/VISensorExternalTrigger 
             /imu0                           302050 msgs    : sensor_msgs/Imu               
             /ublox_driver/ephem               1251 msgs    : gnss_comm/GnssEphemMsg        
             /ublox_driver/glo_ephem            528 msgs    : gnss_comm/GnssGloEphemMsg     
             /ublox_driver/iono_params          698 msgs    : gnss_comm/StampedFloat64Array 
             /ublox_driver/range_meas         15103 msgs    : gnss_comm/GnssMeasMsg         
             /ublox_driver/receiver_lla       15078 msgs    : sensor_msgs/NavSatFix         
             /ublox_driver/receiver_pvt       15078 msgs    : gnss_comm/GnssPVTSolnMsg      
             /ublox_driver/time_pulse_info     1510 msgs    : gnss_comm/GnssTimePulseInfoMsg

订阅GNSS星历信息

星历消息类型： /ublox_driver/ephem 1251 msgs : gnss_comm/GnssEphemMsg

1）src/GVINS/estimator/src/estimator_node.cpp

//gnss星历信息
sub_ephem = n.subscribe(GNSS_EPHEM_TOPIC, 100, gnss_ephem_callback);  

void gnss_ephem_callback(const GnssEphemMsgConstPtr &ephem_msg)
{
    EphemPtr ephem = msg2ephem(ephem_msg);   //看下一节消息处理
    estimator_ptr->inputEphem(ephem);    //同上
}

typedef boost::shared_ptr< ::gnss_comm::GnssEphemMsg const> GnssEphemMsgConstPtr;

2）src/GVINS/estimator/src/parameters.cpp

fsSettings["gnss_ephem_topic"] >> GNSS_EPHEM_TOPIC;

3）src/GVINS/config/visensor_f9p/visensor_left_f9p_config.yaml

gnss_ephem_topic: "/ublox_driver/ephem"               # GPS, Galileo, BeiDou ephemeris 星历

xmu@xmu-NUC8i7BEH:~$ rostopic echo /ublox_driver/ephem 
sat: 13
ttr: 
  week: 2134
  tow: 85266.0
toe: 
  week: 2134
  tow: 86400.0
toc: 
  week: 2134
  tow: 86400.0
toe_tow: 86400.0
week: 2134
iode: 9
iodc: 9
health: 0
code: 0
ura: 2.0
A: 26560408.0696
e: 0.00484015094116
i0: 0.967529986719
omg: 1.05001195006
OMG0: 1.90993987941
M0: -0.645597555592
delta_n: 4.70126725522e-09
OMG_dot: -8.0860511022e-09
i_dot: -3.49657421778e-10
cuc: -1.14738941193e-06
cus: 6.04800879955e-06
crc: 267.96875
crs: -23.5625
cic: -2.60770320892e-08
cis: 6.89178705215e-08
af0: 6.99707306921e-05
af1: 3.86535248253e-12
af2: 0.0
tgd0: -1.16415321827e-08
tgd1: 0.0
A_dot: 0.0
n_dot: 0.0
---

消息处理

在回调函数gnss_ephem_callback中调用

void gnss_ephem_callback(const GnssEphemMsgConstPtr &ephem_msg)
{
    EphemPtr ephem = msg2ephem(ephem_msg);   //看下一节消息处理
    estimator_ptr->inputEphem(ephem);    //同上
}

src/gnss_comm/src/gnss_ros.cpp

// 订阅ROS GPS星历信息，转换成相应的数据
    EphemPtr msg2ephem(const GnssEphemMsgConstPtr &gnss_ephem_msg)
    {
        EphemPtr ephem(new Ephem());
        ephem->sat = gnss_ephem_msg->sat;   // 观测到的卫星数目
        ephem->ttr = gpst2time(gnss_ephem_msg->ttr.week, gnss_ephem_msg->ttr.tow); // GPST中的传输时间
        ephem->toe = gpst2time(gnss_ephem_msg->toe.week, gnss_ephem_msg->toe.tow); //GPST中的星历参考时间
        ephem->toc = gpst2time(gnss_ephem_msg->toc.week, gnss_ephem_msg->toc.tow); //GPST中的时钟校正参考时间

        //在一周内的几秒钟内
        ephem->toe_tow = gnss_ephem_msg->toe_tow; 
        ephem->week = gnss_ephem_msg->week;
        ephem->iode = gnss_ephem_msg->iode;
        ephem->iodc = gnss_ephem_msg->iodc;

        // 卫星健康
        ephem->health = gnss_ephem_msg->health;
        ephem->code = gnss_ephem_msg->code;
        
        // 卫星信号精度
        ephem->ura = gnss_ephem_msg->ura;

        // SV轨道参数
        ephem->A = gnss_ephem_msg->A;
        ephem->e = gnss_ephem_msg->e;
        ephem->i0 = gnss_ephem_msg->i0;
        ephem->omg = gnss_ephem_msg->omg;
        ephem->OMG0 = gnss_ephem_msg->OMG0;
        ephem->M0 = gnss_ephem_msg->M0;
        ephem->delta_n = gnss_ephem_msg->delta_n;
        ephem->OMG_dot = gnss_ephem_msg->OMG_dot;
        ephem->i_dot = gnss_ephem_msg->i_dot;
        ephem->cuc = gnss_ephem_msg->cuc;
        ephem->cus = gnss_ephem_msg->cus;
        ephem->crc = gnss_ephem_msg->crc;
        ephem->crs = gnss_ephem_msg->crs;
        ephem->cic = gnss_ephem_msg->cic;
        ephem->cis = gnss_ephem_msg->cis;

        // SV时钟参数
        ephem->af0 = gnss_ephem_msg->af0;
        ephem->af1 = gnss_ephem_msg->af1;
        ephem->af2 = gnss_ephem_msg->af2;

        // 群时延参数
        ephem->tgd[0] = gnss_ephem_msg->tgd0;
        ephem->tgd[1] = gnss_ephem_msg->tgd1;

        // Adot, ndot for CNAV
        ephem->A_dot = gnss_ephem_msg->A_dot;
        ephem->n_dot = gnss_ephem_msg->n_dot;
        return ephem;
    }

src/GVINS/estimator/src/estimator.cpp

// 星历信息
void Estimator::inputEphem(EphemBasePtr ephem_ptr)
{
    // 时间转换乘秒
    double toe = time2sec(ephem_ptr->toe);
    // if a new ephemeris comes
    if (sat2time_index.count(ephem_ptr->sat) == 0 || sat2time_index.at(ephem_ptr->sat).count(toe) == 0)
    {
        sat2ephem[ephem_ptr->sat].emplace_back(ephem_ptr);
        sat2time_index[ephem_ptr->sat].emplace(toe, sat2ephem.at(ephem_ptr->sat).size()-1);
    }
}

订阅GNSS观测信息

卫星观测信息 : /ublox_driver/range_meas 15103 msgs : gnss_comm/GnssMeasMsg

1）src/GVINS/estimator/src/estimator_node.cpp

sub_gnss_meas = n.subscribe(GNSS_MEAS_TOPIC, 100, gnss_meas_callback);//卫星观测信息

// 订阅观测信息（伪距、多普勒频率），得到GNSS观测值的秒时间，并把观测信息放在全局变量gnss_meas_buf里面，供后面使用
void gnss_meas_callback(const GnssMeasMsgConstPtr &meas_msg)
{
    // 从ros消息解析GNSS测量值
    std::vector<ObsPtr> gnss_meas = msg2meas(meas_msg);

    latest_gnss_time = time2sec(gnss_meas[0]->time);

    // cerr << "gnss ts is " << std::setprecision(20) << time2sec(gnss_meas[0]->time) << endl;
    if (!time_diff_valid)   return;

    m_buf.lock();
    gnss_meas_buf.push(std::move(gnss_meas));
    m_buf.unlock();
    con.notify_one();
}

2）src/GVINS/estimator/src/parameters.cpp

fsSettings["gnss_meas_topic"] >> GNSS_MEAS_TOPIC;

3）src/GVINS/config/visensor_f9p/visensor_left_f9p_config.yaml

gnss_meas_topic: "/ublox_driver/range_meas"           # GNSS raw measurement topic