Ethzasl MSF源码阅读：程序入口和主题订阅

Posted 2020-10-26 岁寒然后知松柏之后凋也！

1.程序入口：ethzasl_msf\msf_updates\src\pose_msf\main.cpp

1 #include "pose_sensormanager.h"
2 
3 int main(int argc, char** argv)
4 {
5   ros::init(argc, argv, "msf_pose_sensor");
6   msf_pose_sensor::PoseSensorManager manager;
7   ros::spin();
8   return 0;
9 }

PoseSensorManager类，查看构造函数。PoseSensorManager继承自msf_core::MSF_SensorManagerROS，继承自msf_core::MSF_SensorManager<EKFState_T>

 1 PoseSensorManager(ros::NodeHandle pnh = ros::NodeHandle("~/pose_sensor"))
 2 {
 3     bool distortmeas = false;  //< Distort the pose measurements.
 4     imu_handler_.reset(new msf_core::IMUHandler_ROS<msf_updates::EKFState>(*this, "msf_core", "imu_handler"));
 5     pose_handler_.reset(new PoseSensorHandler_T(*this, "", "pose_sensor", distortmeas)); AddHandler(pose_handler_);
 6 
 7     reconf_server_.reset(new ReconfigureServer(pnh));
 8     ReconfigureServer::CallbackType f = boost::bind(&PoseSensorManager::Config, this, _1, _2);//回调
 9     reconf_server_->setCallback(f);
10 
11     init_scale_srv_ = pnh.advertiseService("initialize_msf_scale",
12                                            &PoseSensorManager::InitScale, this);
13     init_height_srv_ = pnh.advertiseService("initialize_msf_height",
14                                             &PoseSensorManager::InitHeight, this);
15   }

 2. 注意其中的imu_handler_和pose_handler_，分别是IMUHandler_ROS对象和PoseSensorHandler类对象。IMUHandler_ROS继承自IMUHandler。

注意IMUHandler和PoseSensorHandler最终都继承自msf_core::SensorHandler<typename msf_updates::EKFState>类。

这两个类对象构造的时候都传入了*this指针，即PoseSensorManager对象自身。这很重要，两个类中都调用了PoseSensorManager的成员函数。

可以进入这两个类进行查看构造函数，这2个构造函数都进行了一些主题的订阅。

 IMUHandler_ROS构造，同时查看IMUHandler_ROS::IMUCallback回调函数。

1 IMUHandler_ROS(MSF_SensorManager<EKFState_T>& mng,
2                  const std::string& topic_namespace, const std::string& parameternamespace)
3        : IMUHandler<EKFState_T>(mng, topic_namespace, parameternamespace) 
4 {
5     ros::NodeHandle nh(topic_namespace);
6     subImu_ = nh.subscribe("imu_state_input", 100, &IMUHandler_ROS::IMUCallback, this);
7     subState_ = nh.subscribe("hl_state_input", 10,   &IMUHandler_ROS::StateCallback, this);
8 }

  PoseSensorHandler构造，同时查看PoseSensorHandler::MeasurementCallback回调函数。

 1 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
 2 PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::PoseSensorHandler(
 3     MANAGER_TYPE& meas, std::string topic_namespace,
 4     std::string parameternamespace, bool distortmeas)
 5     : SensorHandler<msf_updates::EKFState>(meas, topic_namespace, parameternamespace),
 6       n_zp_(1e-6),
 7       n_zq_(1e-6),
 8       delay_(0),
 9       timestamp_previous_pose_(0) 
10 {
11   ros::NodeHandle pnh("~/" + parameternamespace);
12 
13   MSF_INFO_STREAM(
14       "Loading parameters for pose sensor from namespace: "
15           << pnh.getNamespace());
16 
17   pnh.param("pose_absolute_measurements", provides_absolute_measurements_,
18             true);
19   pnh.param("pose_measurement_world_sensor", measurement_world_sensor_, true);
20   pnh.param("pose_use_fixed_covariance", use_fixed_covariance_, false);
21   pnh.param("pose_measurement_minimum_dt", pose_measurement_minimum_dt_, 0.05);
22   pnh.param("enable_mah_outlier_rejection", enable_mah_outlier_rejection_, false);
23   pnh.param("mah_threshold", mah_threshold_, msf_core::kDefaultMahThreshold_);
24 
25   MSF_INFO_STREAM_COND(measurement_world_sensor_, "Pose sensor is interpreting "
26                        "measurement as sensor w.r.t. world");
27   MSF_INFO_STREAM_COND(
28       !measurement_world_sensor_,
29       "Pose sensor is interpreting measurement as world w.r.t. "
30       "sensor (e.g. ethzasl_ptam)");
31 
32   MSF_INFO_STREAM_COND(use_fixed_covariance_, "Pose sensor is using fixed "
33                        "covariance");
34   MSF_INFO_STREAM_COND(!use_fixed_covariance_,
35                        "Pose sensor is using covariance "
36                        "from sensor");
37 
38   MSF_INFO_STREAM_COND(provides_absolute_measurements_,
39                        "Pose sensor is handling "
40                        "measurements as absolute values");
41   MSF_INFO_STREAM_COND(!provides_absolute_measurements_, "Pose sensor is "
42                        "handling measurements as relative values");
43 
44   ros::NodeHandle nh("msf_updates/" + topic_namespace);
45   subPoseWithCovarianceStamped_ =
46       nh.subscribe < geometry_msgs::PoseWithCovarianceStamped
47           > ("pose_with_covariance_input", 20, &PoseSensorHandler::MeasurementCallback, this);
48   subTransformStamped_ = nh.subscribe < geometry_msgs::TransformStamped
49       > ("transform_input", 20, &PoseSensorHandler::MeasurementCallback, this);
50   subPoseStamped_ = nh.subscribe < geometry_msgs::PoseStamped
51       > ("pose_input", 20, &PoseSensorHandler::MeasurementCallback, this);
52 
53   z_p_.setZero();
54   z_q_.setIdentity();
55 
56   if (distortmeas)
57  {
58     Eigen::Vector3d meanpos;
59     double distortpos_mean;
60     pnh.param("distortpos_mean", distortpos_mean, 0.0);
61     meanpos.setConstant(distortpos_mean);
62 
63     Eigen::Vector3d stddevpos;
64     double distortpos_stddev;
65     pnh.param("distortpos_stddev", distortpos_stddev, 0.0);
66     stddevpos.setConstant(distortpos_stddev);
67 
68     Eigen::Vector3d meanatt;
69     double distortatt_mean;
70     pnh.param("distortatt_mean", distortatt_mean, 0.0);
71     meanatt.setConstant(distortatt_mean);
72 
73     Eigen::Vector3d stddevatt;
74     double distortatt_stddev;
75     pnh.param("distortatt_stddev", distortatt_stddev, 0.0);
76     stddevatt.setConstant(distortatt_stddev);
77 
78     double distortscale_mean;
79     pnh.param("distortscale_mean", distortscale_mean, 0.0);
80     double distortscale_stddev;
81     pnh.param("distortscale_stddev", distortscale_stddev, 0.0);
82 
83     distorter_.reset( new msf_updates::PoseDistorter(meanpos, stddevpos, meanatt, stddevatt, distortscale_mean, distortscale_stddev));
84   }
85 }

 3.reconf_server_->setCallback(f)绑定了一个回调，回调函数PoseSensorManager::Config(xx).

 1 virtual void Config(Config_T &config, uint32_t level)
 2 {
 3     config_ = config;
 4     pose_handler_->SetNoises(config.pose_noise_meas_p,
 5                              config.pose_noise_meas_q);
 6     pose_handler_->SetDelay(config.pose_delay);
 7     if ((level & msf_updates::SinglePoseSensor_INIT_FILTER)
 8         && config.core_init_filter == true) 
 9     {
10       Init(config.pose_initial_scale);
11       config.core_init_filter = false;
12     }
13     // Init call with "set height" checkbox.
14     if ((level & msf_updates::SinglePoseSensor_SET_HEIGHT)
15         && config.core_set_height == true)
16      {
17       Eigen::Matrix<double, 3, 1> p = pose_handler_->GetPositionMeasurement();
18       if (p.norm() == 0) 
19       {
20         MSF_WARN_STREAM(
21             "No measurements received yet to initialize position. Height init "
22             "not allowed.");
23         return;
24       }
25       double scale = p[2] / config.core_height;
26       Init(scale);
27       config.core_set_height = false;
28     }
29 }