apollo::localization::MSFLocalization类 参考

generate localization info based on MSF 更多...

#include <msf_localization.h>

apollo::localization::MSFLocalization 的协作图:

Public 成员函数
	MSFLocalization ()
apollo::common::Status	Init ()
void	InitParams ()
void	OnPointCloud (const std::shared_ptr< drivers::PointCloud > &message)
void	OnRawImu (const std::shared_ptr< drivers::gnss::Imu > &imu_msg)
void	OnRawImuCache (const std::shared_ptr< drivers::gnss::Imu > &imu_msg)
void	OnGnssRtkObs (const std::shared_ptr< drivers::gnss::EpochObservation > &raw_obs_msg)
void	OnGnssRtkEph (const std::shared_ptr< drivers::gnss::GnssEphemeris > &gnss_orbit_msg)
void	OnGnssBestPose (const std::shared_ptr< drivers::gnss::GnssBestPose > &bestgnsspos_msg)
void	OnGnssHeading (const std::shared_ptr< drivers::gnss::Heading > &gnss_heading_msg)
void	SetPublisher (const std::shared_ptr< LocalizationMsgPublisher > &publisher)
void	OnLocalizationTimer ()

详细描述

generate localization info based on MSF

在文件 msf_localization.h 第 58 行定义.

构造及析构函数说明

MSFLocalization()

apollo::localization::MSFLocalization::MSFLocalization

(

)

在文件 msf_localization.cc 第 35 行定义.

    : monitor_logger_(
          apollo::common::monitor::MonitorMessageItem::LOCALIZATION),
      localization_state_(msf::LocalizationMeasureState::OK),
      pcd_msg_index_(-1),
      raw_imu_msg_(nullptr) {}

成员函数说明

Init()

Status apollo::localization::MSFLocalization::Init

(

)

在文件 msf_localization.cc 第 42 行定义.

                             {
  InitParams();
 
  return localization_integ_.Init(localization_param_);
}

InitParams()

void apollo::localization::MSFLocalization::InitParams

(

)

在文件 msf_localization.cc 第 48 行定义.

                                 {
  // integration module
  localization_param_.is_ins_can_self_align = FLAGS_integ_ins_can_self_align;
  localization_param_.is_sins_align_with_vel = FLAGS_integ_sins_align_with_vel;
  localization_param_.is_sins_state_check = FLAGS_integ_sins_state_check;
  localization_param_.sins_state_span_time = FLAGS_integ_sins_state_span_time;
  localization_param_.sins_state_pos_std = FLAGS_integ_sins_state_pos_std;
  localization_param_.vel_threshold_get_yaw = FLAGS_vel_threshold_get_yaw;
  localization_param_.is_trans_gpstime_to_utctime =
      FLAGS_trans_gpstime_to_utctime;
  localization_param_.gnss_mode = FLAGS_gnss_mode;
  localization_param_.is_using_raw_gnsspos = true;
 
  // gnss module
  localization_param_.enable_ins_aid_rtk = FLAGS_enable_ins_aid_rtk;
 
  // lidar module
  localization_param_.map_path = FLAGS_map_dir + "/" + FLAGS_local_map_name;
  localization_param_.lidar_extrinsic_file = FLAGS_lidar_extrinsics_file;
  localization_param_.lidar_height_file = FLAGS_lidar_height_file;
  localization_param_.lidar_height_default = FLAGS_lidar_height_default;
  localization_param_.localization_mode = FLAGS_lidar_localization_mode;
  localization_param_.lidar_yaw_align_mode = FLAGS_lidar_yaw_align_mode;
  localization_param_.lidar_filter_size = FLAGS_lidar_filter_size;
  localization_param_.map_coverage_theshold = FLAGS_lidar_map_coverage_theshold;
  localization_param_.imu_lidar_max_delay_time = FLAGS_lidar_imu_max_delay_time;
  localization_param_.if_use_avx = FLAGS_if_use_avx;
 
  AINFO << "map: " << localization_param_.map_path;
  AINFO << "lidar_extrin: " << localization_param_.lidar_extrinsic_file;
  AINFO << "lidar_height: " << localization_param_.lidar_height_file;
 
  localization_param_.utm_zone_id = FLAGS_local_utm_zone_id;
  // try load zone id from local_map folder
  if (FLAGS_if_utm_zone_id_from_folder) {
    bool success = LoadZoneIdFromFolder(localization_param_.map_path,
                                        &localization_param_.utm_zone_id);
    if (!success) {
      AWARN << "Can't load utm zone id from map folder, use default value.";
    }
  }
  AINFO << "utm zone id: " << localization_param_.utm_zone_id;
 
  // vehicle imu extrinsic
  imu_vehicle_quat_.x() = FLAGS_imu_vehicle_qx;
  imu_vehicle_quat_.y() = FLAGS_imu_vehicle_qy;
  imu_vehicle_quat_.z() = FLAGS_imu_vehicle_qz;
  imu_vehicle_quat_.w() = FLAGS_imu_vehicle_qw;
  imu_vehicle_translation_[0] = 0.0;
  imu_vehicle_translation_[1] = 0.0;
  imu_vehicle_translation_[2] = 0.0;
  // try to load imu vehicle quat from file
  if (FLAGS_if_vehicle_imu_from_file) {
    double qx = 0.0;
    double qy = 0.0;
    double qz = 0.0;
    double qw = 0.0;
 
    AINFO << "Vehile imu file: " << FLAGS_vehicle_imu_file;
    if (LoadImuVehicleExtrinsic(FLAGS_vehicle_imu_file, &qx, &qy, &qz, &qw,
                                &imu_vehicle_translation_)) {
      imu_vehicle_quat_.x() = qx;
      imu_vehicle_quat_.y() = qy;
      imu_vehicle_quat_.z() = qz;
      imu_vehicle_quat_.w() = qw;
    } else {
      AWARN << "Can't load imu vehicle quat from file, use default value.";
    }
  }
  AINFO << "imu_vehicle_quat: " << imu_vehicle_quat_.x() << " "
        << imu_vehicle_quat_.y() << " " << imu_vehicle_quat_.z() << " "
        << imu_vehicle_quat_.w();
 
  // common
  localization_param_.enable_lidar_localization =
      FLAGS_enable_lidar_localization;
 
  if (!FLAGS_if_imuant_from_file) {
    localization_param_.imu_to_ant_offset.offset_x = FLAGS_imu_to_ant_offset_x;
    localization_param_.imu_to_ant_offset.offset_y = FLAGS_imu_to_ant_offset_y;
    localization_param_.imu_to_ant_offset.offset_z = FLAGS_imu_to_ant_offset_z;
    localization_param_.imu_to_ant_offset.uncertainty_x =
        FLAGS_imu_to_ant_offset_ux;
    localization_param_.imu_to_ant_offset.uncertainty_y =
        FLAGS_imu_to_ant_offset_uy;
    localization_param_.imu_to_ant_offset.uncertainty_z =
        FLAGS_imu_to_ant_offset_uz;
  } else {
    double offset_x = 0.0;
    double offset_y = 0.0;
    double offset_z = 0.0;
    double uncertainty_x = 0.0;
    double uncertainty_y = 0.0;
    double uncertainty_z = 0.0;
    AINFO << "Ant imu lever arm file: " << FLAGS_ant_imu_leverarm_file;
    ACHECK(LoadGnssAntennaExtrinsic(FLAGS_ant_imu_leverarm_file, &offset_x,
                                    &offset_y, &offset_z, &uncertainty_x,
                                    &uncertainty_y, &uncertainty_z));
    localization_param_.ant_imu_leverarm_file = FLAGS_ant_imu_leverarm_file;
 
    localization_param_.imu_to_ant_offset.offset_x = offset_x;
    localization_param_.imu_to_ant_offset.offset_y = offset_y;
    localization_param_.imu_to_ant_offset.offset_z = offset_z;
    localization_param_.imu_to_ant_offset.uncertainty_x = uncertainty_x;
    localization_param_.imu_to_ant_offset.uncertainty_y = uncertainty_y;
    localization_param_.imu_to_ant_offset.uncertainty_z = uncertainty_z;
 
    AINFO << localization_param_.imu_to_ant_offset.offset_x << " "
          << localization_param_.imu_to_ant_offset.offset_y << " "
          << localization_param_.imu_to_ant_offset.offset_z << " "
          << localization_param_.imu_to_ant_offset.uncertainty_x << " "
          << localization_param_.imu_to_ant_offset.uncertainty_y << " "
          << localization_param_.imu_to_ant_offset.uncertainty_z;
  }
 
  localization_param_.imu_delay_time_threshold_1 =
      FLAGS_imu_delay_time_threshold_1;
  localization_param_.imu_delay_time_threshold_2 =
      FLAGS_imu_delay_time_threshold_2;
  localization_param_.imu_delay_time_threshold_3 =
      FLAGS_imu_delay_time_threshold_3;
 
  localization_param_.imu_missing_time_threshold_1 =
      FLAGS_imu_missing_time_threshold_1;
  localization_param_.imu_missing_time_threshold_2 =
      FLAGS_imu_missing_time_threshold_2;
  localization_param_.imu_missing_time_threshold_3 =
      FLAGS_imu_missing_time_threshold_3;
 
  localization_param_.bestgnsspose_loss_time_threshold =
      FLAGS_bestgnsspose_loss_time_threshold;
  localization_param_.lidar_loss_time_threshold =
      FLAGS_lidar_loss_time_threshold;
 
  localization_param_.localization_std_x_threshold_1 =
      FLAGS_localization_std_x_threshold_1;
  localization_param_.localization_std_y_threshold_1 =
      FLAGS_localization_std_y_threshold_1;
 
  localization_param_.localization_std_x_threshold_2 =
      FLAGS_localization_std_x_threshold_2;
  localization_param_.localization_std_y_threshold_2 =
      FLAGS_localization_std_y_threshold_2;
 
  localization_timer_.reset(new cyber::Timer(
      10, [this]() { this->OnLocalizationTimer(); }, false));
  localization_timer_->Start();
}

OnGnssBestPose()

void apollo::localization::MSFLocalization::OnGnssBestPose

(

const std::shared_ptr< drivers::gnss::GnssBestPose > &

bestgnsspos_msg

)

在文件 msf_localization.cc 第 257 行定义.

                                                                   {
  if ((localization_state_ == msf::LocalizationMeasureState::OK ||
       localization_state_ == msf::LocalizationMeasureState::VALID) &&
      FLAGS_gnss_only_init) {
    return;
  }
 
  localization_integ_.GnssBestPoseProcess(*bestgnsspos_msg);
 
  const auto &result = localization_integ_.GetLastestGnssLocalization();
 
  if (result.state() == msf::LocalizationMeasureState::OK ||
      result.state() == msf::LocalizationMeasureState::VALID) {
    publisher_->PublishLocalizationMsfGnss(result.localization());
  }
}

OnGnssHeading()

void apollo::localization::MSFLocalization::OnGnssHeading

(

const std::shared_ptr< drivers::gnss::Heading > &

gnss_heading_msg

)

在文件 msf_localization.cc 第 304 行定义.

                                                               {
  if ((localization_state_ == msf::LocalizationMeasureState::OK ||
       localization_state_ == msf::LocalizationMeasureState::VALID) &&
      FLAGS_gnss_only_init) {
    return;
  }
  localization_integ_.GnssHeadingProcess(*gnss_heading_msg);
}

OnGnssRtkEph()

void apollo::localization::MSFLocalization::OnGnssRtkEph

(

const std::shared_ptr< drivers::gnss::GnssEphemeris > &

gnss_orbit_msg

)

在文件 msf_localization.cc 第 293 行定义.

                                                                   {
  if ((localization_state_ == msf::LocalizationMeasureState::OK ||
       localization_state_ == msf::LocalizationMeasureState::VALID) &&
      FLAGS_gnss_only_init) {
    return;
  }
 
  localization_integ_.RawEphemerisProcess(*gnss_orbit_msg);
}

OnGnssRtkObs()

void apollo::localization::MSFLocalization::OnGnssRtkObs

(

const std::shared_ptr< drivers::gnss::EpochObservation > &

raw_obs_msg

)

在文件 msf_localization.cc 第 275 行定义.

                                                                   {
  if ((localization_state_ == msf::LocalizationMeasureState::OK ||
       localization_state_ == msf::LocalizationMeasureState::VALID) &&
      FLAGS_gnss_only_init) {
    return;
  }
 
  localization_integ_.RawObservationProcess(*raw_obs_msg);
 
  const auto &result = localization_integ_.GetLastestGnssLocalization();
 
  if (result.state() == msf::LocalizationMeasureState::OK ||
      result.state() == msf::LocalizationMeasureState::VALID) {
    publisher_->PublishLocalizationMsfGnss(result.localization());
  }
}

OnLocalizationTimer()

void apollo::localization::MSFLocalization::OnLocalizationTimer

(

)

在文件 msf_localization.cc 第 314 行定义.

                                          {
  if (!raw_imu_msg_) {
    return;
  }
  std::unique_lock<std::mutex> lock(mutex_imu_msg_);
  OnRawImu(raw_imu_msg_);
}

OnPointCloud()

void apollo::localization::MSFLocalization::OnPointCloud

(

const std::shared_ptr< drivers::PointCloud > &

message

)

在文件 msf_localization.cc 第 197 行定义.

                                                     {
  ++pcd_msg_index_;
  if (pcd_msg_index_ % FLAGS_point_cloud_step != 0) {
    return;
  }
 
  localization_integ_.PcdProcess(*message);
 
  const auto &result = localization_integ_.GetLastestLidarLocalization();
 
  if (result.state() == msf::LocalizationMeasureState::OK ||
      result.state() == msf::LocalizationMeasureState::VALID) {
    // publish lidar message to debug
    publisher_->PublishLocalizationMsfLidar(result.localization());
  }
}

OnRawImu()

void apollo::localization::MSFLocalization::OnRawImu

(

const std::shared_ptr< drivers::gnss::Imu > &

imu_msg

)

在文件 msf_localization.cc 第 215 行定义.

                                                  {
  if (FLAGS_imu_coord_rfu) {
    localization_integ_.RawImuProcessRfu(*imu_msg);
  } else {
    localization_integ_.RawImuProcessFlu(*imu_msg);
  }
 
  const auto &result = localization_integ_.GetLastestIntegLocalization();
 
  // compose localization status
  LocalizationStatus status;
  apollo::common::Header *status_headerpb = status.mutable_header();
  status_headerpb->set_timestamp_sec(
      result.localization().header().timestamp_sec());
  status.set_fusion_status(
      static_cast<MeasureState>(result.integ_status().integ_state));
  status.set_state_message(result.integ_status().state_message);
  status.set_measurement_time(result.localization().measurement_time());
  publisher_->PublishLocalizationStatus(status);
 
  if (result.state() == msf::LocalizationMeasureState::OK ||
      result.state() == msf::LocalizationMeasureState::VALID) {
    // calculate orientation_vehicle_world
    LocalizationEstimate local_result = result.localization();
    CompensateImuVehicleExtrinsic(&local_result);
 
    publisher_->PublishPoseBroadcastTF(local_result);
    publisher_->PublishPoseBroadcastTopic(local_result);
  }
 
  localization_state_ = result.state();
}

OnRawImuCache()

void apollo::localization::MSFLocalization::OnRawImuCache

(

const std::shared_ptr< drivers::gnss::Imu > &

imu_msg

)

在文件 msf_localization.cc 第 249 行定义.

                                                  {
  if (imu_msg) {
    std::unique_lock<std::mutex> lock(mutex_imu_msg_);
    raw_imu_msg_ = const_cast<std::shared_ptr<drivers::gnss::Imu> &>(imu_msg);
  }
}

SetPublisher()

void apollo::localization::MSFLocalization::SetPublisher

(

const std::shared_ptr< LocalizationMsgPublisher > &

publisher

)

在文件 msf_localization.cc 第 322 行定义.

                                                              {
  publisher_ = publisher;
}

---

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

• modules/localization/msf/msf_localization.h
• modules/localization/msf/msf_localization.cc