robot_localization 源码解析（1）ekf_localization_node

1. 简介

1.1 坐标系定义

状态向量为：
$$X=（x,y,z,{\theta }_x,{\theta }_y,{\theta }_z,\dot{x},\dot{y},\dot{z},\dot{{\theta }_x},\dot{{\theta }_y},\dot{{\theta }_z},\ddot{x},\ddot{y},\ddot{z}{)}^T$$
状态向量中$（x,y,z,{\theta }_x,{\theta }_y,{\theta }_z)$为世界坐标系下的绝对姿态，$（\dot{x},\dot{y},\dot{z},\dot{{\theta }_x},\dot{{\theta }_y},\dot{{\theta }_z},\ddot{x},\ddot{y},\ddot{z})$为载体坐标系下的姿态变化；

ekf_localization_node 对应的文件为ekf_localization_node.cpp，其以Ekf为参数实例化了模板类 RosFilter：

namespace RobotLocalization
{
   
   

typedef RosFilter<Ekf> RosEkf;//以Ekf为参数实例化了模板类 RosFilter
typedef RosFilter<Ukf> RosUkf;

}

ekf_localization_node.cpp代码内容如下,主要分为实例化RosEkf和初始化RosEkf两个部分，下面将分为这两个部分进行展开：

int main(int argc, char **argv)
{
   
   
  ros::init(argc, argv, "ekf_navigation_node");

  ros::NodeHandle nh;
  ros::NodeHandle nh_priv("~");

  RobotLocalization::RosEkf ekf(nh, nh_priv);//实例化RosEkf
  ekf.initialize();//初始化RosEkf
  ros::spin();

  return EXIT_SUCCESS;
}

2. 实例化RosEkf

实例化RosEkf通过构造函数完成，构造函数进行普通成员变量初始化，以及状态向量名初始化:

  template<typename T>
  RosFilter<T>::RosFilter(ros::NodeHandle nh,
                          ros::NodeHandle nh_priv,
                          std::string node_name,
                          std::vector<double> args) :
      disabledAtStartup_(false),
      enabled_(false),
      predictToCurrentTime_(false),
      printDiagnostics_(true),
      publishAcceleration_(false),
      publishTransform_(true),
      resetOnTimeJump_(false),
      smoothLaggedData_(false),
      toggledOn_(true),
      twoDMode_(false),
      useControl_(false),
      silentTfFailure_(false),
      dynamicDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK),
      staticDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK),
      frequency_(30.0),
      gravitationalAcc_(9.80665),
      historyLength_(0),
      minFrequency_(frequency_ - 2.0),
      maxFrequency_(frequency_ + 2.0),
      baseLinkFrameId_("base_link"),
      mapFrameId_("map"),
      odomFrameId_("odom"),
      worldFrameId_(odomFrameId_),
      lastDiagTime_(0),
      lastSetPoseTime_(0),
      latestControlTime_(0),
      tfTimeOffset_(ros::Duration(0)),
      tfTimeout_(ros::Duration(0)),
      filter_(args),
      nh_(nh),
      nhLocal_(nh_priv),
      diagnosticUpdater_(nh, nh_priv, node_name),
      tfListener_(tfBuffer_)
  {
   
   
    stateVariableNames_.push_back("X");//状态向量
    stateVariableNames_.push_back("Y");
    stateVariableNames_.push_back("Z");
    stateVariableNames_.push_back("ROLL");
    stateVariableNames_.push_back("PITCH");
    stateVariableNames_.push_back("YAW");
    stateVariableNames_.push_back("X_VELOCITY");
    stateVariableNames_.push_back("Y_VELOCITY");
    stateVariableNames_.push_back("Z_VELOCITY");
    stateVariableNames_.push_back("ROLL_VELOCITY");
    stateVariableNames_.push_back("PITCH_VELOCITY");
    stateVariableNames_.push_back("YAW_VELOCITY");
    stateVariableNames_.push_back("X_ACCELERATION");
    stateVariableNames_.push_back("Y_ACCELERATION");
    stateVariableNames_.push_back("Z_ACCELERATION");

    diagnosticUpdater_.setHardwareID("none");
  }

2. 初始化RosEkf

  template<typename T>
  void RosFilter<T>::initialize()
  {
   
   
    loadParams();//加载参数

    if (printDiagnostics_)
    {
   
   
      diagnosticUpdater_.add("Filter diagnostic updater", this, &RosFilter<T>::aggregateDiagnostics);
    }

    // Set up the frequency diagnostic
    minFrequency_ = frequency_ - 2;
    maxFrequency_ = frequency_ + 2;
    freqDiag_ = std::make_unique<diagnostic_updater::HeaderlessTopicDiagnostic>(
      "odometry/filtered",
      diagnosticUpdater_,
      diagnostic_updater::FrequencyStatusParam(
        &minFrequency_,
        &maxFrequency_,
        0.1,
        10));

    // Publisher
    positionPub_ = nh_.advertise<nav_msgs::Odometry>("odometry/filtered", 20);

    // Optional acceleration publisher
    if (publishAcceleration_)
    {
   
   
      accelPub_ = nh_.advertise<geometry_msgs::AccelWithCovarianceStamped>("accel/filtered", 20);
    }

    lastDiagTime_ = ros::Time::now();

    periodicUpdateTimer_ = nh_.createTimer(ros::Duration(1./frequency_), &RosFilter<T>::periodicUpdate, this);
  }

2.1 loadParams()加载参数

订阅odom0: example/odom、odom1: example/another_odom等话题，并生成相关更新向量，观测以及观测协方差矩阵；
订阅pose0: example/pose等话题，并生成相关更新向量，观测以及观测协方差矩阵；
订阅imu0: example/imu等话题，并生成相关更新向量，观测以及观测协方差矩阵；
加载过程噪声
加载初始估计噪声

  template<typename T>
  void RosFilter<T>::loadParams()
  {
   
   
    /* For diagnostic purposes, collect information about how many different
     * sources are measuring each absolute pose variable and do not have
     * differential integration enabled.
     */
    std::map<StateMembers, int> absPoseVarCounts;
    absPoseVarCounts[StateMemberX] = 0;
    absPoseVarCounts[StateMemberY] = 0;
    absPoseVarCounts[StateMemberZ] = 0;
    absPoseVarCounts[StateMemberRoll] = 0;
    absPoseVarCounts[StateMemberPitch] = 0;
    absPoseVarCounts[StateMemberYaw] = 0;
    // Same for twist variables
    std::map<StateMembers, int> twistVarCounts;
    twistVarCounts[StateMemberVx] = 0;
    twistVarCounts[StateMemberVy] = 0;
    twistVarCounts[StateMemberVz] = 0;
    twistVarCounts[StateMemberVroll] = 0;
    twistVarCounts[StateMemberVpitch] = 0;
    twistVarCounts[StateMemberVyaw] = 0;

    // Determine if we'll be printing diagnostic information
    nhLocal_.param("print_diagnostics", printDiagnostics_, true);

    // Check for custom gravitational acceleration value
    nhLocal_.param("gravitational_acceleration", gravitationalAcc_, 9.80665);

    // Grab the debug param. If true, the node will produce a LOT of output.
    bool debug;
    nhLocal_.param("debug", debug, false);

    if (debug)
    {
   
   
      std::string debugOutFile;

      try
      {
   
   
        nhLocal_.param("debug_out_file", debugOutFile, std::string("robot_localization_debug.txt"));
        debugStream_.open(debugOutFile.c_str());

        // Make sure we succeeded
        if (debugStream_.is_open())
        {
   
   
          filter_.setDebug(debug, &debugStream_);
        }
        else
        {
   
   
          ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file " << debugOutFile);
        }
      }
      catch(const std::exception &e)
      {
   
   
        ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file" << debugOutFile
                        << ". Error was " << e.what() << "\n");
      }
    }

    // These params specify the name of the robot's body frame (typically
    // base_link) and odometry frame (typically odom)
    nhLocal_.param("map_frame", mapFrameId_, std::string("map"));
    nhLocal_.param("odom_frame", odomFrameId_, std::string("odom"));
    nhLocal_.param("base_link_frame", baseLinkFrameId_, std::string("base_link"));
    nhLocal_.param("base_link_frame_output", baseLinkOutputFrameId_, baseLinkFrameId_);

    /*
     * These parameters are designed to enforce compliance with REP-105:
     * http://www.ros.org/reps/rep-0105.html
     * When fusing absolute position data from sensors such as GPS, the state
     * estimate can undergo discrete jumps. According to REP-105, we want three
     * coordinate frames: map, odom, and base_link. The map frame can have
     * discontinuities, but is the frame with the most accurate position estimate
     * for the robot and should not suffer from drift. The odom frame drifts over
     * time, but is guaranteed to be continuous and is accurate enough for local
     * planning and navigation. The base_link frame is affixed to the robot. The
     * intention is that some odometry source broadcasts the odom->base_link
     * transform. The localization software should broadcast map->base_link.
     * However, tf does not allow multiple parents for a coordinate frame, so
     * we must *compute* map->base_link, but then use the existing odom->base_link
     * transform to compute *and broadcast* map->odom.
     *
     * The state estimation nodes in robot_localization therefore have two "modes."
     * If your world_frame parameter value matches the odom_frame parameter value,
     * then robot_localization will assume someone else is broadcasting a transform
     * from odom_frame->base_link_frame, and it will compute the
     * map_frame->odom_frame transform. Otherwise, it will simply compute the
     * odom_frame->base_link_frame transform.
     *
     * The default is the latter behavior (broadcast of odom->base_link).
     */
    nhLocal_.param("world_frame", worldFrameId_, odomFrameId_);

    ROS_FATAL_COND(mapFrameId_ == odomFrameId_ ||
                   odomFrameId_ == baseLinkFrameId_ ||
                   mapFrameId_ == baseLinkFrameId_ ||
                   odomFrameId_ == baseLinkOutputFrameId_ ||
                   mapFrameId_ == baseLinkOutputFrameId_,
                   "Invalid frame configuration! The values for map_frame, odom_frame, "
                   "and base_link_frame must be unique. If using a base_link_frame_output values, it "
                   "must not match the map_frame or odom_frame.");

    // Try to resolve tf_prefix
    std::string tfPrefix = "";
    std::string tfPrefixPath = "";
    if (nhLocal_.searchParam("tf_prefix", tfPrefixPath))
    {
   
   
      nhLocal_.getParam(tfPrefixPath, tfPrefix);
    }

    // Append the tf prefix in a tf2-friendly manner
    FilterUtilities::appendPrefix(tfPrefix, mapFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, odomFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, baseLinkFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, baseLinkOutputFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, worldFrameId_);

    // Whether we're publshing the world_frame->base_link_frame transform
    nhLocal_.param("publish_tf", publishTransform_, true);

    // Whether we're publishing the acceleration state transform
    nhLocal_.param("publish_acceleration", publishAcceleration_, false);

    // Transform future dating
    double offsetTmp;
    nhLocal_.param("transform_time_offset", offsetTmp, 0.0);
    tfTimeOffset_.fromSec(offsetTmp);

    // Transform timeout
    double timeoutTmp;
    nhLocal_.param("transform_timeout", timeoutTmp, 0.0);
    tfTimeout_.fromSec(timeoutTmp);

    // Update frequency and sensor timeout
    double sensorTimeout;
    nhLocal_.param("frequency", frequency_, 30.0);
    nhLocal_.param("sensor_timeout", sensorTimeout, 1.0 / frequency_);
    filter_.setSensorTimeout(sensorTimeout);

    // Determine if we're in 2D mode
    nhLocal_.param("two_d_mode", twoDMode_, false);

    // Whether or not to print warning for tf lookup failure
    // Note: accesses the root of the parameter tree, not the local parameters
    nh_.param("/silent_tf_failure", silentTfFailure_, false);

    // Smoothing window size
    nhLocal_.param("smooth_lagged_data", smoothLaggedData_, false);
    nhLocal_.param("history_length", historyLength_, 0.0);

    // Wether we reset filter on jump back in time
    nhLocal_.param("reset_on_time_jump", resetOnTimeJump_, false);

    if (!smoothLaggedData_ && ::fabs(historyLength_) > 1e-9)
    {
   
   
      ROS_WARN_STREAM("Filter history interval of " << historyLength_ <<
                      " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed.");
    }

    if (smoothLaggedData_ && historyLength_ < -1e9)
    {
   
   
      ROS_WARN_STREAM("Negative history interval of " << historyLength_ <<
                      " specified. Absolute value will be assumed.");
    }

    historyLength_ = ::fabs(historyLength_);

    nhLocal_.param("predict_to_current_time", predictToCurrentTime_, false);

    // Determine if we're using a control term
    bool stampedControl = false;
    double controlTimeout = sensorTimeout;
    std::vector<int> controlUpdateVector(TWIST_SIZE, 0);
    std::vector<double> accelerationLimits(TWIST_SIZE, 1.0);
    std::vector<double> accelerationGains(TWIST_SIZE, 1.0);
    std::vector<double> decelerationLimits(TWIST_SIZE, 1.0);
    std::vector<double> decelerationGains(TWIST_SIZE, 1.0);

    nhLocal_.param("use_control", useControl_, false);
    nhLocal_.param("stamped_control", stampedControl, false);
    nhLocal_.param("control_timeout", controlTimeout, sensorTimeout);

    if (useControl_)
    {
   
   
      if (nhLocal_.getParam("control_config", controlUpdateVector))
      {
   
   
        if (controlUpdateVector.size() != TWIST_SIZE)
        {
   
   
          ROS_ERROR_STREAM("Control configuration must be of size " << TWIST_SIZE << ". Provided config was of "
            "size " << controlUpdateVector.size() << ". No control term will be used.");
          useControl_ = false;
        }
      }
      else
      {
   
   
        ROS_ERROR_STREAM("use_control is set to true, but control_config is missing. No control term will be used.");
        useControl_ = false;
      }

      if (nhLocal_.getParam("acceleration_limits", accelerationLimits))
      {
   
   
        if (accelerationLimits.size() != TWIST_SIZE)
        {
   
   
          ROS_ERROR_STREAM("Acceleration configuration must be of size " << TWIST_SIZE << ". Provided config was of "
            "size " << accelerationLimits.size() << ". No control term will be used.");
          useControl_ = false;
        }
      }
      else
      {
   
   
        ROS_WARN_STREAM("use_control is set to true, but acceleration_limits is missing. Will use default values.");
      }

      if (nhLocal_.getParam("acceleration_gains", accelerationGains))
      {
   
   
        const int size = accelerationGains.size();
        if (size != TWIST_SIZE)
        {
   
   
          ROS_ERROR_STREAM("Acceleration gain configuration must be of size " << TWIST_SIZE <<
            ". Provided config was of size " << size << ". All gains will be assumed to be 1.");
          std::fill_n(accelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0);
          accelerationGains.resize(TWIST_SIZE, 1.0);
        }
      }

      if (nhLocal_.getParam("deceleration_limits", decelerationLimits))
      {
   
   
        if (decelerationLimits.size() != TWIST_SIZE)
        {
   
   
          ROS_ERROR_STREAM("Deceleration configuration must be of size " << TWIST_SIZE <<
            ". Provided config was of size " << decelerationLimits.size() << ". No control term will be used.");
          useControl_ = false;
        }
      }
      else
      {
   
   
        ROS_INFO_STREAM("use_control is set to true, but no deceleration_limits specified. Will use acceleration "
          "limits.");
        decelerationLimits = accelerationLimits;
      }

      if (nhLocal_.getParam("deceleration_gains", decelerationGains))
      {
   
   
        const int size = decelerationGains.size();
        if (size != TWIST_SIZE)
        {
   
   
          ROS_ERROR_STREAM("Deceleration gain configuration must be of size " << TWIST_SIZE <<
            ". Provided config was of size " << size << ". All gains will be assumed to be 1.");
          std::fill_n(decelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0);
          decelerationGains.resize(TWIST_SIZE, 1.0);
        }
      }
      else
      {
   
   
        ROS_INFO_STREAM("use_control is set to true, but no deceleration_gains specified. Will use acceleration "
          "gains.");
        decelerationGains = accelerationGains;
      }
    }

    bool dynamicProcessNoiseCovariance = false;
    nhLocal_.param("dynamic_process_noise_covariance", dynamicProcessNoiseCovariance, false);
    filter_.setUseDynamicProcessNoiseCovariance(dynamicProcessNoiseCovariance);

    std::vector<double> initialState(STATE_SIZE, 0.0);
    if (nhLocal_.getParam("initial_state", initialState))
    {
   
   
      if (initialState.size() != STATE_SIZE)
      {
   
   
        ROS_ERROR_STREAM("Initial state must be of size " << STATE_SIZE << ". Provided config was of size " <<
          initialState.size() << ". The initial state will be ignored.");
      }
      else
      {
   
   
        Eigen::Map<Eigen::VectorXd> eigenState(initialState.data(), initialState.size());
        filter_.setState(eigenState);
      }
    }

    // Check if the filter should start or not
    nhLocal_.param("disabled_at_startup", disabledAtStartup_, false);
    enabled_ = !disabledAtStartup_;


    // Debugging writes to file
    RF_DEBUG("tf_prefix is " << tfPrefix <<
             "\nmap_frame is " << mapFrameId_