传感器数据走向-cartographer
Kong Liangqian Lv6
 2021-10-14 10:59:22  

传感器数据走向

Node类的HandleLaserScanMessage函数

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

mapbuilder_bridge 在Node的构造函数赋值

sensor_bridge于Node构造函数之后的StartTrajectoryWithDefaultTopics中赋值，在添加轨迹的过程中，需要建立对应轨迹的sensor_bridge方便处理传感器数据。

SensorBridge类的HandleLaserScanMessage函数

1
2
3
4
5
6
7
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
    carto::sensor::PointCloudWithIntensities point_cloud;
    carto::common::Time time;
    std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
    HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

HandleLaserScan最后会调用HandleRangefinder

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
  if (!ranges.empty()) {
    CHECK_LE(ranges.back().time, 0.f);
  }
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

  // 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin
  // 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, 
                       sensor_to_tracking->translation().cast<float>(),
                       // 将点云从雷达坐标系下转到tracking_frame坐标系系下
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空
  }
}

SensorBridge的trajectorybuilder是指向CollatedTrajectoryBuilder的指针。

trajectorybuilder 过程

trajectorybuilder 在建立SensorBridge的构造函数中赋在这里传入的trajectorybuilder 即从mapbuilder里获取的TrajectoryBuilder值，注意，map_builder 于mapbuilder_bridge 

1
2
3
4
5
6
7
8
SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {}

SensorBridge又是在添加轨迹的时候，为每一条轨迹添加SensorBridge的时候添加的

在文件map_builder.bridge.cc文件的MapBuilderBridge::AddTrajectory 函数

1
2
3
4
5
6
7
// Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, 
      tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder

在这里传入的trajectorybuilder 即从map_builder里获取的TrajectoryBuilder。

GetTrajectoryBuilder(trajectory_id))返回的类型为TrajectoryBuilderInterface, 他可以为任意以TrajectoryBuilderInterface为基类的子类。这里他的类型为CollatedTrajectoryBuilder。

以后再去看trajectory_builder在赋值时候给的类型

CollatedTrajectoryBuilder类的AddSensorData函数

1
2
3
4
5
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}

接下来看CollatedTrajectoryBuilder的具体实现

 Comments