ublox_f9p—— GNSS定位結果(ENU)在rviz上成功顯示的詳細步驟

一、GNSS定位結果處理

1.1 訂閱 ublox 釋出的topic

ros::Subscriber sub_GPS = n.subscribe("/ublox_driver/receiver_lla", 100, GPS_callback);
           

其中，具體的 GPS_callback() 函數如下：

void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
    m_buf.lock();
    gpsQueue.push(GPS_msg);
    m_buf.unlock();
    con.notify_one();
    
    glo_gnss_time = GPS_msg->header.stamp.toSec();
    
    glo_gnss_lat = GPS_msg->latitude;
    glo_gnss_lon = GPS_msg->longitude;
    glo_gnss_h   = GPS_msg->altitude;
    
    // (1) llh to xyz
    Eigen::Vector3d lla,xyz,enu;
    lla << glo_gnss_lat, glo_gnss_lon, glo_gnss_h;
    xyz = geo2ecef(lla);
    
    if(fist_gnss == 1)
    {
      first_GNSSxyz = xyz;
      fist_gnss = 0;
    }
    
    // (2