cartographer代码学习-回调函数与传感器数据走向

1、里程计数据走向：

里程计的回调函数为：

/**
 * @brief 处理里程计数据,里程计的数据走向有2个
 * 第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的数据走向有2个
 * 第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的指针，然后后面通过指针指向新的函数。这里思考一个问题：如果不用指针能不能指到这个函数？区别是什么？