数据处理-Node类-cartographer
Kong Liangqian Lv6
 2021-09-05 15:59:22  

在node_main.cc中添加轨迹的过程中，对轨迹根据option的配置进行了订阅者的定义和回调注册，即

node_main.cc ->Node::StartTrajectoryWithDefaultTopics -> Node::AddTrajectory -> LaunchSubscribers

针对每一个传感器，都注册了一个处理数据的一个回调函数。所有的数据都进入了sensor_bridge进行处理的。

laser_scan;Node::HandleLaserScanMessage

laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数

1
2
3
4
5
6
7
8
9
// laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
for (const std::string& topic :
     ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
  subscribers_[trajectory_id].push_back(
      {SubscribeWithHandler<sensor_msgs::LaserScan>(
           &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
           this),
       topic});
}

在生成一个subscriber的时候，也顺便把此订阅者和名字一起放入了subscribers_对应的轨迹ID的值中.
初步处理过程：先过滤掉ratio设定的传感器数据，然后将数据传入SensorBridge

1
2
3
4
5
6
7
8
9
10
11
12
13
// 调用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);
}

IMU;Node::HandleImuMessage

Imu的数据首先也是排除滤波的数据，这里数据有两个去向

  • 第1个是传入PoseExtrapolator,用于位姿预测与重力方向的确定
  • 第2个是传入SensorBridge,使用其传感器处理函数进行imu数据处理
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
/**
 * @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 != nul// 调用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);
}
lptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

里程计；Node::HandleOdometryMessage

里程计的数据首先也是排除滤波的数据，这里数据有两个去向

  • 第1个是传入PoseExtrapolator,用于位姿预测与重力方向的确定
  • 第2个是传入SensorBridge,使用其传感器处理函数进行里程计数据处理
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/**
 * @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;
  }
  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) {
    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
  }
  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}

到此为止，位姿估计器接受IMU和里程计的消息，进行了融合。

点云；Node::HandlePointCloud2Message

检测是否需要过滤掉此点，然后传入sensor_bridge进行处理

1
2
3
4
5
6
7
8
9
10
11
// 调用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);
}

GPS;Node::HandleNavSatFixMessage

检测是否需要过滤掉此点，然后传入sensor_bridge进行处理

1
2
3
4
5
6
7
8
9
10
11
// 调用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);
}
 Comments