上一篇的Node::LaunchSubscribers的编程技巧比较复杂，只需要知道是执行 Node::Handle***Message那几个函数即可
然后就进入了 GlobalTrajectoryBuilder::AddSensorData1
2
3
4
5
6
7
8
9
10
11void 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);
}
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
8point.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)
HandleLaserScan 和 num_subdivisions_per_laser_scan 分段
接下来的HandleLaserScan按照参数num_subdivisions_per_laser_scan把点云分成了很多小段，每一段数据的时间偏移调整为以这一段的最后一条数据为基准，也就是把一帧scan拆成几段，每段按一个TimedPointCloudData发送，到了LocalTrajectoryBuilder2D::AddRangeData又把它们拼接起来，作为当前帧输入。
num_subdivisions_per_laser_scan一般设置为1，也就是不分段，可以不必仔细研究，直接看HandleRangefinder1
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
47void 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
20void 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
19class 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_;
};