沉默杀手
(四) LaunchSubscribers流程
|激光SLAMCartographer源码解读|
Word count: 1.3k|Reading time: 5 min

上一篇的Node::LaunchSubscribers的编程技巧比较复杂，只需要知道是执行 Node::Handle***Message那几个函数即可

流程图

流程图

然后就进入了 GlobalTrajectoryBuilder::AddSensorData

1
2
3
4
5
6
7
8
9
10
11
void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg)
{
  // 转换之后的点云数据和时间戳
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  // 将ROS的消息转换成点云数据，函数在 msg_conversion.cc
  // 只有一句 LaserScanToPointCloudWithIntensities(msg)
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

只要一个时间是 carto::common::Time 类型，那么一定是绝对时间戳。

TimedPointCloud是后面一直使用的类型，转换流程在这里
类型转换

类型转换

LaserScanToPointCloudWithIntensities

不考虑强度时，用雷达扫描只获得range和angle两个参数，LaserScanToPointCloudWithIntensities(msg)实际是把这两个参数转换为扫描点在laser坐标系下的坐标，时间戳以最后一束激光为基准。 同时每个激光点也包含一个相对最后一个点的时间偏移。这 样通过这个偏移就可以求出每个点的时间戳了。

不考虑多回波雷达，直接看时间戳处理部分：

1
2
3
4
5
6
7
8
9
10
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty())
{
  const double duration = point_cloud.points.back()[3];
  // scan 最后一束激光的时间戳
  timestamp += cartographer::common::FromSeconds(duration);
  // 时间戳变成以最后一个测量数据为基准
  for (Eigen::Vector4f& point : point_cloud.points)
    point[3] -= duration;
}

这里把一帧激光的时间戳进行处理，有了绝对时间戳和相对时间戳两个概念。

我所用的雷达一帧有1040个点， 参数 time_increment: 1.73611115315e-05,     duration: 0.0180382

time_increment * 1040 = duration

timestamp: 634369239333347047

每个点的时间戳在处理前:

1
2
3
4
5
6
7
8
point.time:  0
point.time:  1.73611e-05
point.time:  3.47222e-05
point.time:  5.20833e-05
        ......
point.time:  0.0180035
point.time:  0.0180208
point.time:  0.0180382  # 同duration必然相同

然后每个时间戳减去 duration，这样最后一个点的时间戳就是0，之前的全是负值。 timestamp开始是第一个点的时间，最后处理为最后一个点的时间戳。

这里的时间戳计算是以雷达每束激光的 时间差相等和第一束激光的时间戳最小 为前提的，如果雷达的时间戳相反(逆向)或时间差不均匀，就会出错。

坐标如此处理：比如range为3，角度为45°，相应的坐标是(2.12132,2.12132,0)
转换示意图.png

转换示意图.png


HandleLaserScan 和 num_subdivisions_per_laser_scan 分段

接下来的HandleLaserScan按照参数num_subdivisions_per_laser_scan把点云分成了很多小段，每一段数据的时间偏移调整为以这一段的最后一条数据为基准，也就是把一帧scan拆成几段，每段按一个TimedPointCloudData发送，到了LocalTrajectoryBuilder2D::AddRangeData又把它们拼接起来，作为当前帧输入。

num_subdivisions_per_laser_scan一般设置为1，也就是不分段，可以不必仔细研究，直接看HandleRangefinder

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
36
37
38
39
40
41
42
43
44
45
46
47
void SensorBridge::HandleLaserScan(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,
    const carto::sensor::PointCloudWithIntensities& points)
{
  if (points.points.empty())    return;
  CHECK_LE(points.points.back()[3], 0);  //最后一个点云数据的时间小于等于0
  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i)
  {
    const size_t start_index =
        points.points.size() * i / num_subdivisions_per_laser_scan_;
    const size_t end_index =
        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
        // 每一小段
    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    if (start_index == end_index)
    {
      continue;
    }
    // 参考分段中最后一个数据的时间，调整其他数据的时间
    const double time_to_subdivision_end = subdivision.back()[3];
    // subdivision_time is the end of the measurement so sensor::Collator will
    // send all other sensor data first.
    // 先确认当前的数据没有过时，如果分段的时间落后于记录值，将抛弃所对应的数据
    const carto::common::Time subdivision_time =
        time + carto::common::FromSeconds(time_to_subdivision_end);
    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
    if (it != sensor_to_previous_subdivision_time_.end() &&
        it->second >= subdivision_time)
    {
      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                   << sensor_id << " because previous subdivision time "
                   << it->second << " is not before current subdivision time "
                   << subdivision_time;
      continue;
    }
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    for (Eigen::Vector4f& point : subdivision)
    {
      point[3] -= time_to_subdivision_end;
    }
    CHECK_EQ(subdivision.back()[3], 0);
    // 将分段数据喂给Cartographer
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  }
}

HandleRangefinder

查询tracking_frame--->laser坐标系的变换， 把之前在laser坐标系的点云，转换为在tracking_frame坐标系(一般是base_footprint)的点云，类型变成时间点云数据TimedPointCloudData。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,  const carto::sensor::TimedPointCloud& ranges)
{
  // 查询在time时间，tracking_frame--->laser坐标系的变换，记录到对象 sensor_to_tracking
  // 这里有时会卡住
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
  // 如果找到变换，通过trajectory_builder_ 添加传感器数据
  if (sensor_to_tracking != nullptr)
  {
    trajectory_builder_->AddSensorData(
    // 第一个转换是 TransformTimedPointCloud, 把数据从 sensor frame到tracking frame
    // 第二个转换是从 TimedPointCloud 转到了 TimedPointCloudData
    sensor_id, carto::sensor::TimedPointCloudData{
                   time, sensor_to_tracking->translation().cast<float>(),
                   carto::sensor::TransformTimedPointCloud(
                       ranges, sensor_to_tracking->cast<float>() )  }  );
  }
}

类TfBridge在SensorBridge的构造函数里初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
class TfBridge {
 public:
  TfBridge(const std::string& tracking_frame,
           double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
  ~TfBridge() {}

  TfBridge(const TfBridge&) = delete;
  TfBridge& operator=(const TfBridge&) = delete;

  // 查询在time时间，frame_id--->tracking_frame 的变换，如果存在的话
  // 也就是说 tracking_frame 是 target frame
  std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
      ::cartographer::common::Time time, const std::string& frame_id) const;

 private:
  const std::string tracking_frame_;
  const double lookup_transform_timeout_sec_;
  const tf2_ros::Buffer* const buffer_;
};