apollo::localization::msf::LocalizationGnssProcess类 参考

#include <localization_gnss_process.h>

apollo::localization::msf::LocalizationGnssProcess 的协作图:

Public 成员函数
	LocalizationGnssProcess ()
	~LocalizationGnssProcess ()
apollo::common::Status	Init (const LocalizationIntegParam &param)
void	RawObservationProcess (const drivers::gnss::EpochObservation &raw_obs)
void	RawEphemerisProcess (const drivers::gnss::GnssEphemeris &gnss_orbit)
void	IntegSinsPvaProcess (const InsPva &sins_pva, const double variance[9][9])
LocalizationMeasureState	GetResult (MeasureData *gnss_measure)

详细描述

在文件 localization_gnss_process.h 第 101 行定义.

构造及析构函数说明

LocalizationGnssProcess()

apollo::localization::msf::LocalizationGnssProcess::LocalizationGnssProcess

(

)

在文件 localization_gnss_process.cc 第 33 行定义.

    : gnss_solver_(new GnssSolver()),
      enable_ins_aid_rtk_(true),
      gnss_lever_arm_{0.0, 0.0, 0.0},
      sins_align_finish_(false),
      double_antenna_solver_(new GnssSolver()),
      current_obs_time_(0.0),
      gnss_state_(LocalizationMeasureState::NOT_VALID) {
  SetDefaultOption();
}

~LocalizationGnssProcess()

apollo::localization::msf::LocalizationGnssProcess::~LocalizationGnssProcess

(

)

在文件 localization_gnss_process.cc 第 44 行定义.

                                                  {
  map_gnss_eph_.clear();
 
  delete gnss_solver_;
  gnss_solver_ = nullptr;
  delete double_antenna_solver_;
  double_antenna_solver_ = nullptr;
}

成员函数说明

GetResult()

LocalizationMeasureState apollo::localization::msf::LocalizationGnssProcess::GetResult

(

MeasureData *

gnss_measure

)

在文件 localization_gnss_process.cc 第 196 行定义.

                           {
  CHECK_NOTNULL(gnss_msg);
 
  // convert GnssPntResult to IntegMeasure
  // double sec_s = Clock::NowInSeconds(); //
  // ros::Time::now().toSec();
  const unsigned int second_per_week = 604800;
  double sec_s = gnss_pnt_result_.gnss_week() * second_per_week +
                 gnss_pnt_result_.gnss_second_s();
  gnss_msg->time = sec_s;
  gnss_msg->frame_type = FrameType::ECEF;
 
  // velocity must be in ENU frame and position in frame WGS84
  const int dim = 9;
  for (int i = 0; i < dim; ++i) {
    for (int j = 0; j < dim; ++j) {
      gnss_msg->variance[i][j] = 0.0;
    }
  }
 
  bool b_pos = false;
  if (gnss_pnt_result_.has_pos_x_m() && gnss_pnt_result_.has_pos_y_m() &&
      gnss_pnt_result_.has_pos_z_m()) {
    b_pos = true;
    gnss_msg->gnss_pos.longitude = gnss_pnt_result_.pos_x_m();
    gnss_msg->gnss_pos.latitude = gnss_pnt_result_.pos_y_m();
    gnss_msg->gnss_pos.height = gnss_pnt_result_.pos_z_m();
    gnss_msg->measure_type = MeasureType::GNSS_POS_ONLY;
 
    gnss_msg->is_have_variance = true;
    // std
    gnss_msg->variance[0][0] =
        gnss_pnt_result_.std_pos_x_m() * gnss_pnt_result_.std_pos_x_m();
    gnss_msg->variance[1][1] =
        gnss_pnt_result_.std_pos_y_m() * gnss_pnt_result_.std_pos_y_m();
    gnss_msg->variance[2][2] =
        gnss_pnt_result_.std_pos_z_m() * gnss_pnt_result_.std_pos_z_m();
  }
  bool b_vel = false;
  if (gnss_pnt_result_.has_vel_x_m() && gnss_pnt_result_.has_vel_y_m() &&
      gnss_pnt_result_.has_vel_z_m()) {
    // output velocity whenever possible.
    b_vel = true;
    gnss_msg->gnss_vel.ve = gnss_pnt_result_.vel_x_m();
    gnss_msg->gnss_vel.vn = gnss_pnt_result_.vel_y_m();
    gnss_msg->gnss_vel.vu = gnss_pnt_result_.vel_z_m();
    gnss_msg->measure_type = MeasureType::ENU_VEL_ONLY;
    gnss_msg->is_have_variance = true;
    // std
    gnss_msg->variance[3][3] =
        gnss_pnt_result_.std_vel_x_m() * gnss_pnt_result_.std_vel_x_m();
    gnss_msg->variance[4][4] =
        gnss_pnt_result_.std_vel_y_m() * gnss_pnt_result_.std_vel_y_m();
    gnss_msg->variance[5][5] =
        gnss_pnt_result_.std_vel_z_m() * gnss_pnt_result_.std_vel_z_m();
  }
  if (b_pos && b_vel) {
    gnss_msg->measure_type = MeasureType::GNSS_POS_VEL;
  }
 
  return gnss_state_;
}

Init()

Status apollo::localization::msf::LocalizationGnssProcess::Init

(

const LocalizationIntegParam &

param

)

在文件 localization_gnss_process.cc 第 53 行定义.

                                                                        {
  // set launch parameter
  enable_ins_aid_rtk_ = param.enable_ins_aid_rtk;
  gnss_solver_->set_enable_external_prediction(enable_ins_aid_rtk_);
 
  gnss_lever_arm_.arm_x = param.imu_to_ant_offset.offset_x;
  gnss_lever_arm_.arm_y = param.imu_to_ant_offset.offset_y;
  gnss_lever_arm_.arm_z = param.imu_to_ant_offset.offset_z;
 
  map_gnss_eph_.clear();
  sins_align_finish_ = false;
 
  return Status::OK();
}

IntegSinsPvaProcess()

void apollo::localization::msf::LocalizationGnssProcess::IntegSinsPvaProcess	(	const InsPva &	sins_pva,
		const double	variance[9][9]
	)

在文件 localization_gnss_process.cc 第 163 行定义.

                                                                               {
  if (!sins_pva_msg.init_and_alignment) {
    return;
  }
 
  sins_align_finish_ = true;
 
  // feed into GNSS-RTK Engine
  double sec_s = sins_pva_msg.time;
  double llh[3] = {sins_pva_msg.pos.longitude, sins_pva_msg.pos.latitude,
                   sins_pva_msg.pos.height};
  double velocity[3] = {sins_pva_msg.vel.ve, sins_pva_msg.vel.vn,
                        sins_pva_msg.vel.vu};
  double lever_arm[3] = {gnss_lever_arm_.arm_x, gnss_lever_arm_.arm_y,
                         gnss_lever_arm_.arm_z};
  double euler[3] = {sins_pva_msg.att.pitch, sins_pva_msg.att.roll,
                     sins_pva_msg.att.yaw};
 
  double std_pos[3][3] = {0.0};
  double std_vel[3][3] = {0.0};
  // const unsigned int dim = 9;
  for (unsigned int i = 0; i < 3; ++i) {
    for (unsigned int j = 0; j < 3; ++j) {
      std_pos[i][j] = variance[i][j];
      std_vel[i][j] = variance[i + 3][j + 3];
    }
  }
 
  gnss_solver_->motion_update(sec_s, llh, std_pos, velocity, std_vel, euler,
                              lever_arm);
}

RawEphemerisProcess()

void apollo::localization::msf::LocalizationGnssProcess::RawEphemerisProcess

(

const drivers::gnss::GnssEphemeris &

gnss_orbit

)

在文件 localization_gnss_process.cc 第 132 行定义.

                                         {
  if (!msg.has_gnss_type()) {
    return;
  }
  auto gnss_orbit = msg;
  if (gnss_orbit.gnss_type() == drivers::gnss::GnssType::GLO_SYS) {
    /* caros driver (derived from rtklib src) set glonass eph toe as the GPST,
     * and here convert it back to UTC(+0), so leap seconds should be in
     * accordance with the GNSS-Driver
     */
    double leap_sec =
        gnss_solver_->get_leap_second(gnss_orbit.glonass_orbit().week_num(),
                                      gnss_orbit.glonass_orbit().toe());
    double toe = gnss_orbit.glonass_orbit().toe() - leap_sec;
    gnss_orbit.mutable_glonass_orbit()->set_toe(toe);
    gnss_orbit.mutable_glonass_orbit()->set_week_second_s(toe);
  }
  static int eph_counter = 0;
  ++eph_counter;
  // printf("received a gnss ephemeris: %d!\n", eph_counter);
  if (DuplicateEph(gnss_orbit)) {
    AINFO << "received an existed gnss ephemeris!";
    return;
  }
 
  GnssEphemerisMsg gnss_orbit_msg;
  GnssMagTransfer::Transfer(gnss_orbit, &gnss_orbit_msg);
  gnss_solver_->save_gnss_ephemris(gnss_orbit_msg);
}

RawObservationProcess()

void apollo::localization::msf::LocalizationGnssProcess::RawObservationProcess

(

const drivers::gnss::EpochObservation &

raw_obs

)

在文件 localization_gnss_process.cc 第 80 行定义.

                                                {
  if (!raw_obs.has_receiver_id()) {
    AERROR << "Obs data being invalid if without receiver id!";
    return;
  }
  const double unix_to_gps = 315964800;
  const double sec_per_week = 604800;
  double leap_second_s = gnss_solver_->get_leap_second(raw_obs.gnss_week(),
                                                       raw_obs.gnss_second_s());
 
  double sys_secs_to_gnss =
      Clock::NowInSeconds() - unix_to_gps + leap_second_s;
  double obs_secs =
      raw_obs.gnss_second_s() + raw_obs.gnss_week() * sec_per_week;
  double obs_delay = sys_secs_to_gnss - obs_secs;
 
  double obs_xyz[3] = {0.0};
  if (raw_obs.has_position_x() && raw_obs.has_position_y() &&
      raw_obs.has_position_z()) {
    obs_xyz[0] = raw_obs.position_x();
    obs_xyz[1] = raw_obs.position_y();
    obs_xyz[2] = raw_obs.position_z();
  }
  const unsigned int max_msg_size = 256;
  char message[max_msg_size] = {'\0'};
  snprintf(
      message, max_msg_size,
      "user %u time:%12.3f sat_num:%d obs_delay:%12.3f%16.3f%16.3f%16.3f\n",
      raw_obs.receiver_id(), raw_obs.gnss_second_s(), raw_obs.sat_obs_num(),
      obs_delay, obs_xyz[0], obs_xyz[1], obs_xyz[2]);
  AINFO << message;
 
  EpochObservationMsg raw_obs_msg;
  GnssMagTransfer::Transfer(raw_obs, &raw_obs_msg);
  if (raw_obs.receiver_id() != 0) {
    // Notice: the baser coordinate should be binded with obs together anytime.
    gnss_solver_->save_baser_observation(raw_obs_msg);
    // id - 0x80000000 = station_id from gnss_driver
    return;
  }
 
  const unsigned int second_per_week = 604800;
  double new_obs_time =
      raw_obs.gnss_second_s() + raw_obs.gnss_week() * second_per_week;
  if (new_obs_time <= current_obs_time_) {
    return;
  }
  current_obs_time_ = new_obs_time;
  GnssPosition(&raw_obs_msg);
}

---

该类的文档由以下文件生成:

• modules/localization/msf/local_integ/localization_gnss_process.h
• modules/localization/msf/local_integ/localization_gnss_process.cc