这里有个感觉，就是百川汇海。即IMU数据和相机的消息数据都汇集到msf_core进行处理。接上一篇，
1. 查看IMUHandler_ROS::IMUCallback和IMUHandler_ROS::StateCallback回调函数。
MUHandler_ROS::IMUCallback，传入的消息sensor_msgs::ImuConstPtr。
void IMUCallback(const sensor_msgs::ImuConstPtr & msg)
{
static int lastseq = constants::INVALID_SEQUENCE;
if (static_cast<int>(msg->header.seq) != lastseq +
&& lastseq != constants::INVALID_SEQUENCE) {
MSF_WARN_STREAM(
"msf_core: imu message drop curr seq:" << msg->header.seq
<< " expected: " << lastseq + );
}
lastseq = msg->header.seq; msf_core::Vector3 linacc;
linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
->linear_acceleration.z; msf_core::Vector3 angvel;
angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
->angular_velocity.z; this->ProcessIMU(linacc, angvel, msg->header.stamp.toSec(),
msg->header.seq);
}
IMUHandler_ROS::StateCallback，传入的参数sensor_fusion_comm::ExtEkfConstPtr，这个需要理解一下。
void StateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg)
{
static_cast<MSF_SensorManagerROS<EKFState_T>&>(this->manager_)
.SetHLControllerStateBuffer(*msg); // Get the imu values.
msf_core::Vector3 linacc;
linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
->linear_acceleration.z; msf_core::Vector3 angvel;
angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
->angular_velocity.z; int32_t flag = msg->flag;
// Make sure we tell the HL to ignore if data playback is on.
if (this->manager_.GetDataPlaybackStatus())
flag = sensor_fusion_comm::ExtEkf::ignore_state; bool isnumeric = true;
if (flag == sensor_fusion_comm::ExtEkf::current_state) {
isnumeric = CheckForNumeric(
Eigen::Map<const Eigen::Matrix<float, , > >(msg->state.data()),
"before prediction p,v,q");
} // Get the propagated states.
msf_core::Vector3 p, v;
msf_core::Quaternion q; p = Eigen::Matrix<double, , >(msg->state[], msg->state[],
msg->state[]);
v = Eigen::Matrix<double, , >(msg->state[], msg->state[],
msg->state[]);
q = Eigen::Quaternion<double>(msg->state[], msg->state[], msg->state[],
msg->state[]);
q.normalize(); bool is_already_propagated = false;
if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) {
is_already_propagated = true;
} this->ProcessState(linacc, angvel, p, v, q, is_already_propagated,
msg->header.stamp.toSec(), msg->header.seq);
}
查看IMUHandler_ROS类的父类IMUHandler的ProcessIMU和ProcessState方法，如下：
void ProcessIMU(const msf_core::Vector3& linear_acceleration,
const msf_core::Vector3& angular_velocity,
const double& msg_stamp, size_t msg_seq) {
core_->ProcessIMU(linear_acceleration, angular_velocity, msg_stamp,
msg_seq);
}
void ProcessState(const msf_core::Vector3& linear_acceleration,
const msf_core::Vector3& angular_velocity,
const msf_core::Vector3& p, const msf_core::Vector3& v,
const msf_core::Quaternion& q, bool is_already_propagated,
const double& msg_stamp, size_t msg_seq) {
core_->ProcessExternallyPropagatedState(linear_acceleration,
angular_velocity, p, v, q,
is_already_propagated,
msg_stamp, msg_seq);
}
可以发现对应了msf_core_的ProcessIMU和ProcessExternallyPropagatedState方法。
2. 查看PoseSensorHandler::MeasurementCallback回调函数。注意，在构造函数中挂载了三个不同的MeasurementCallback函数。
geometry_msgs::PoseWithCovarianceStamped，geometry_msgs::TransformStamped，geometry_msgs::PoseStamped三种消息类型。
template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
{
this->SequenceWatchDog(msg->header.seq,
subPoseWithCovarianceStamped_.getTopic());
MSF_INFO_STREAM_ONCE(
"*** pose sensor got first measurement from topic "
<< this->topic_namespace_ << "/"
<< subPoseWithCovarianceStamped_.getTopic() << " ***");
ProcessPoseMeasurement(msg);//注意
}
查看 ProcessPoseMeasurement(msg)函数：
template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::ProcessPoseMeasurement(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
{
received_first_measurement_ = true; // Get the fixed states.
int fixedstates = ;
static_assert(msf_updates::EKFState::nStateVarsAtCompileTime < , "Your state "
"has more than 32 variables. The code needs to be changed here to have a "
"larger variable to mark the fixed_states");
// Do not exceed the 32 bits of int. // Get all the fixed states and set flag bits.
MANAGER_TYPE* mngr = dynamic_cast<MANAGER_TYPE*>(&manager_); // TODO(acmarkus): if we have multiple sensor handlers, they all share the same dynparams,
// which me maybe don't want. E.g. if we have this for multiple AR Markers, we
// may want to keep one fix --> move this to fixed parameters? Could be handled
// with parameter namespace then.
if (mngr) {
if (mngr->Getcfg().pose_fixed_scale) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::L;
}
if (mngr->Getcfg().pose_fixed_p_ic) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::p_ic;
}
if (mngr->Getcfg().pose_fixed_q_ic) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::q_ic;
}
if (mngr->Getcfg().pose_fixed_p_wv) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::p_wv;
}
if (mngr->Getcfg().pose_fixed_q_wv) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::q_wv;
}
} shared_ptr<MEASUREMENT_TYPE> meas(new MEASUREMENT_TYPE(
n_zp_, n_zq_, measurement_world_sensor_, use_fixed_covariance_,
provides_absolute_measurements_, this->sensorID,
enable_mah_outlier_rejection_, mah_threshold_, fixedstates, distorter_)); meas->MakeFromSensorReading(msg, msg->header.stamp.toSec() - delay_); z_p_ = meas->z_p_; //store this for the init procedure
z_q_ = meas->z_q_; this->manager_.msf_core_->AddMeasurement(meas);
}
这里调用了this->manager_.msf_core_->AddMeasurement(meas)，查看AddMeasurement方法。
3.以上，最终对应于MSF_Core类的三个函数，即
ProcessIMU、ProcessExternallyPropagatedState、AddMeasurement。
4.MSF_Core类，MSF_core类负责汇集IMU消息和位姿观测值，同时实现了状态预测，而msf_updates::pose_measurement::PoseMeasurement<>实现了状态的更新。
这个在分析MSF_Core三个方法的时候再说明。