1、里程计数据走向：
里程计的回调函数为：
/** * @brief 处理里程计数据有两个方向 * 第1个是传入PoseExtrapolator,用于位置预测 * 第2个是传入SensorBridge,里程计数据处理采用传感器处理函数 * * @param[in] trajectory_id 轨迹id * @param[in] sensor_id 里程计的topic名字 * @param[in] msg 里程计的ros格式的数据 */ void Node::HandleOdometryMessage(const int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { absl::MutexLock lock(&mutex_);//互斥锁 ///判断里程计采样器是否处于暂停状态，如果处于暂停状态，则返回 if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { return; } //如果采样器在工作，首先通过map_builder_bridge_.sensor_bridge进入轨迹id并获得指针sensor_bridge_ptr ///然后用指针调用ToOdometryData(msg)传入参数为函数msg里程计参数。获得转换数据odometry_data_ptr auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg); // extrapolators_用里程计数据预测位置 if (odometry_data_ptr != nullptr) { ///然后将转换数据传输到位置推测器AddOdometryData函数 extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr); } ///原始数据传输HandleOdometryMessage处理 sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg); }
里程计数据有两个方向：一个是方向位置估计器，另一个是给出HandleOdometryMessage里程计数据处理采用传感器处理函数。
2.里程计数据方向:
/** * @brief 处理imu数据,imu有两个数据方向 * 第1个是传入PoseExtrapolator,用于确定位置预测和重力方向 * 第2个是传入SensorBridge,使用传感器处理函数imu数据处理 * * @param[in] trajectory_id 轨迹id * @param[in] sensor_id imu的topic名字 * @param[in] msg imu的ros格式的数据 */ void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) {
absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return; } auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); // extrapolators_使用里程计数据进行位姿预测 if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); } sensor_bridge_ptr->HandleImuMessage(sensor_id, msg); } 里程计的数据走向与IMU的数据走向很相似，两个基本上是一样的。所以这两个传感器都有两个数据走向。
node里面的位姿推测器与前端的进行位姿估计的位姿推测器是独立的。node类里面的位姿推测器的作用主要是融合IMU和里程计输出来一个推测出来的位姿。根据参数决定是否拿着这个结果去发布tf。这里的参数就是对应着lua文件中的use-pose-extrapolator=false这个参数。当这个参数设置为true时，算法在使用tf变换时会使用位姿推测器推测出来的位姿。
注意这是一个先验的位姿，是通过IMU与里程计推测出来的位姿，没有经过激光数据验证的，所以不一定完全准确。因此一般情况下我们不使用这个位姿进行tf发布。当然如果你的里程计跟IMU特别特别准确的话可以设置位true使用。
3、GPS数据走向：
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleNavSatFixMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleNavSatFixMessage(sensor_id, msg);
}
这里跟前面两个不太一样，数据走向只有一条路到HandleNavSatFixMessage(sensor_id, msg)。
4、Landmark数据走向：
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLandmarkMessage(sensor_id, msg);
}
Landmark数据跟GPS相同，也是只经历一个处理。
5、激光雷达数据走向：
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 根据配置,是否将传感器数据跳过
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
6、超声波雷达数据走向：
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleMultiEchoLaserScanMessage(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}
7、点云数据走向：
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandlePointCloud2Message(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandlePointCloud2Message(sensor_id, msg);
}
后面3-7的数据处理基本上是一样的，就是数据类型的传入有点区别然后处理的函数有点区别而已。
另外点云输入是传入到sensor_bridge函数中，这个函数在map_builder_bridge.h中：
// 获取对应轨迹id的SensorBridge的指针
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get();
}
该函数传入的是一个轨迹ID，输出一个指向sensor_bridge的指针，然后后面通过指针指向新的函数。这里思考一个问题：如果不用指针能不能指到这个函数？区别是什么？