Ethzasl MSF源码阅读(2):百川汇海
这里有个感觉,就是百川汇海。即IMU数据和相机的消息数据都汇集到msf_core进行处理。接上一篇,
1. 查看IMUHandler_ROS::IMUCallback和IMUHandler_ROS::StateCallback回调函数。
MUHandler_ROS::IMUCallback,传入的消息sensor_msgs::ImuConstPtr。
1 void IMUCallback(const sensor_msgs::ImuConstPtr & msg) 2 { 3 static int lastseq = constants::INVALID_SEQUENCE; 4 if (static_cast<int>(msg->header.seq) != lastseq + 1 5 && lastseq != constants::INVALID_SEQUENCE) { 6 MSF_WARN_STREAM( 7 "msf_core: imu message drop curr seq:" << msg->header.seq 8 << " expected: " << lastseq + 1); 9 } 10 lastseq = msg->header.seq; 11 12 msf_core::Vector3 linacc; 13 linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg 14 ->linear_acceleration.z; 15 16 msf_core::Vector3 angvel; 17 angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg 18 ->angular_velocity.z; 19 20 this->ProcessIMU(linacc, angvel, msg->header.stamp.toSec(), 21 msg->header.seq); 22 }
IMUHandler_ROS::StateCallback,传入的参数sensor_fusion_comm::ExtEkfConstPtr,这个需要理解一下。
1 void StateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg) 2 { 3 static_cast<MSF_SensorManagerROS<EKFState_T>&>(this->manager_) 4 .SetHLControllerStateBuffer(*msg); 5 6 // Get the imu values. 7 msf_core::Vector3 linacc; 8 linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg 9 ->linear_acceleration.z; 10 11 msf_core::Vector3 angvel; 12 angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg 13 ->angular_velocity.z; 14 15 int32_t flag = msg->flag; 16 // Make sure we tell the HL to ignore if data playback is on. 17 if (this->manager_.GetDataPlaybackStatus()) 18 flag = sensor_fusion_comm::ExtEkf::ignore_state; 19 20 bool isnumeric = true; 21 if (flag == sensor_fusion_comm::ExtEkf::current_state) { 22 isnumeric = CheckForNumeric( 23 Eigen::Map<const Eigen::Matrix<float, 10, 1> >(msg->state.data()), 24 "before prediction p,v,q"); 25 } 26 27 // Get the propagated states. 28 msf_core::Vector3 p, v; 29 msf_core::Quaternion q; 30 31 p = Eigen::Matrix<double, 3, 1>(msg->state[0], msg->state[1], 32 msg->state[2]); 33 v = Eigen::Matrix<double, 3, 1>(msg->state[3], msg->state[4], 34 msg->state[5]); 35 q = Eigen::Quaternion<double>(msg->state[6], msg->state[7], msg->state[8], 36 msg->state[9]); 37 q.normalize(); 38 39 bool is_already_propagated = false; 40 if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) { 41 is_already_propagated = true; 42 } 43 44 this->ProcessState(linacc, angvel, p, v, q, is_already_propagated, 45 msg->header.stamp.toSec(), msg->header.seq); 46 }
查看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三种消息类型。
1 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE> 2 void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback( 3 const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) 4 { 5 this->SequenceWatchDog(msg->header.seq, 6 subPoseWithCovarianceStamped_.getTopic()); 7 MSF_INFO_STREAM_ONCE( 8 "*** pose sensor got first measurement from topic " 9 << this->topic_namespace_ << "/" 10 << subPoseWithCovarianceStamped_.getTopic() << " ***"); 11 ProcessPoseMeasurement(msg);//注意 12 }
查看 ProcessPoseMeasurement(msg)函数:
1 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE> 2 void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::ProcessPoseMeasurement(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) 3 { 4 received_first_measurement_ = true; 5 6 // Get the fixed states. 7 int fixedstates = 0; 8 static_assert(msf_updates::EKFState::nStateVarsAtCompileTime < 32, "Your state " 9 "has more than 32 variables. The code needs to be changed here to have a " 10 "larger variable to mark the fixed_states"); 11 // Do not exceed the 32 bits of int. 12 13 // Get all the fixed states and set flag bits. 14 MANAGER_TYPE* mngr = dynamic_cast<MANAGER_TYPE*>(&manager_); 15 16 // TODO(acmarkus): if we have multiple sensor handlers, they all share the same dynparams, 17 // which me maybe don't want. E.g. if we have this for multiple AR Markers, we 18 // may want to keep one fix --> move this to fixed parameters? Could be handled 19 // with parameter namespace then. 20 if (mngr) { 21 if (mngr->Getcfg().pose_fixed_scale) { 22 fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::L; 23 } 24 if (mngr->Getcfg().pose_fixed_p_ic) { 25 fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::p_ic; 26 } 27 if (mngr->Getcfg().pose_fixed_q_ic) { 28 fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::q_ic; 29 } 30 if (mngr->Getcfg().pose_fixed_p_wv) { 31 fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::p_wv; 32 } 33 if (mngr->Getcfg().pose_fixed_q_wv) { 34 fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::q_wv; 35 } 36 } 37 38 shared_ptr<MEASUREMENT_TYPE> meas(new MEASUREMENT_TYPE( 39 n_zp_, n_zq_, measurement_world_sensor_, use_fixed_covariance_, 40 provides_absolute_measurements_, this->sensorID, 41 enable_mah_outlier_rejection_, mah_threshold_, fixedstates, distorter_)); 42 43 meas->MakeFromSensorReading(msg, msg->header.stamp.toSec() - delay_); 44 45 z_p_ = meas->z_p_; //store this for the init procedure 46 z_q_ = meas->z_q_; 47 48 this->manager_.msf_core_->AddMeasurement(meas); 49 }
这里调用了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三个方法的时候再说明。