slam_toolbox 一帧数据流

03 一帧数据流

从雷达消息到 map→odom 和 /map，这是读码的主路径。

3.1 总路径

/scan  LaserScan.header.frame_id = laser
   │
   ▼
tf2::MessageFilter<LaserScan>
   目标坐标系 odom_frame_（默认 odom）
   队列长度 scan_queue_size_（异步建议 1）
   超时 transform_timeout_
   │  等不到变换：帧被 Filter 丢掉，进不了回调
   ▼
laserCallback()          子类实现
   │  1) pose_helper_->getOdomPose(pose, stamp)
   │       TF: 把 base_frame 恒等变换变到 odom
   │       失败打 "Failed to compute odom pose"，return
   │  2) getLaser(scan)
   │       按 frame_id 缓存 LaserRangeFinder
   │       第一次：LaserAssistant 查 base←laser 外参
   │  3) shouldProcessScan(scan, pose)
   │  4) addScan / 入队
   ▼
addScanImpl()            common.cpp
   getLocalizedRangeScan()
   Mapper::Process* ()
   成功：setTransformFromPoses + publishPose + dataset_->Add
   失败：delete range_scan
   │
   ├─ publishTransformLoop   周期发 map→odom
   └─ publishVisualizations  周期 OccupancyGrid::CreateFromScans → /map

3.2 取里程计：GetPoseHelper

文件：include/slam_toolbox/get_pose_helper.hpp。

base_ident.header.frame_id = base_frame_     // 默认 base_footprint
base_ident.header.stamp    = scan.stamp
tf_->transform(base_ident, odom_frame_)      // 得到 odom 系下的 base
Pose2(x, y, yaw)

没有 odom → base_footprint，整条链在这里断。 底盘必须发这条动态 TF。时间戳对不上（雷达用传感器钟、里程计用 now()）也会抛 TransformException。

3.3 门槛：shouldProcessScan（common.cpp 约 L813）

按顺序拒绝：

1. 首帧（first_measurement_）：记下 pose/时间，放行，便于启动。
2. 暂停 NEW_MEASUREMENTS。
3. 节流 scan_ctr % throttle_scans_ != 0。
4. 时间 stamp - last < minimum_time_interval_（默认常 0.5 s）。
5. 前 5 帧（scan_ctr < 5）丢掉，等稳定。注意：首帧已把 first_measurement_ 清掉，所以第 2–5 帧会被这条拦掉。
6. 位移：默认 dist² < 0.8 * min_travel² 则丢（给匹配留 20% 余量）。check_min_dist_and_heading_precisely_=true 时，距离和航向都不够才丢。

通过后更新 last_pose、last_scan_time。

Karto 内部还有第二次 HasMovedEnough。两道门槛叠加，车几乎不动时不会出新节点。

3.4 组装扫描：getLocalizedRangeScan

1. scanToReadings：按 LaserMetadata.inverted 决定是否倒序距离
2. reprocessing_transform_ * odom_pose：续建时把新 odom 拧到旧图坐标系
3. new LocalizedRangeScan(laserName, readings)
4. SetOdometricPose / SetCorrectedPose 先都设成变换后的里程计位姿
5. SetTime 用扫描时间戳（秒）

3.5 addScanImpl 分派

PROCESS            → Mapper::Process
PROCESS_FIRST_NODE → ProcessAtDock，然后回到 PROCESS，并更新 reprocessing_transform_
PROCESS_NEAR_REGION→ 把 odom 位姿改成 process_near_pose_，ProcessAgainstNodesNearBy
其它               → FATAL exit

定位节点覆盖 addScan，走 PROCESS_LOCALIZATION / PROCESS_NEAR_REGION，成功后不 dataset_->Add（定位窗口自己管内存）。

建图成功时：

• 交互模式：scan_holder_->addScan 缓存原始 LaserScan 给 RViz
• setTransformFromPoses(corrected, odom, stamp, update_reproc)
• dataset_->Add(range_scan)
• publishPose、publishNewNodeEvent

失败：delete range_scan。Karto 拒绝的扫描不会进图。

3.6 map→odom：setTransformFromPoses

已知：

• corrected_pose：Karto 修正后的 base 在 map 里
• odom_pose：当前 TF 的 base 在 odom 里

做法：

1. 构造 base_to_map = Inverse(map_T_base_corrected)
2. tf_->transform(base_to_map, odom_frame_) 得到 odom_T_map 的一部分
3. map_to_odom_ = Inverse(odom_to_map)
4. TF 线程发出 header.frame_id=map_frame_, child=odom_frame_

回环优化改的是各节点的 corrected pose，下一次 setTransformFromPoses 会让 map→odom 跳一下。odom→base 仍由底盘连续发，控制器看到的跳变在 map→odom。

update_reprocessing_transform=true 时（dock / near region 刚对齐）：

reprocessing_transform_ = odom_to_base_serialized * Inverse(odom_to_base_current)

之后新扫描的里程计都先乘这个齐，才能接到旧图。

TF 时间戳：restamp_tf_=false（默认）用 scan_stamp + transform_timeout_；true 用 now() + timeout。超时加在 stamp 上是为了让下游 lookup 更容易成功。

3.7 /map 怎么来

publishVisualizations 按 map_update_interval 调 updateMap：

• 没有订阅者则直接 return（省 CPU）
• SMapper::getOccupancyGrid(resolution_) → OccupancyGrid::CreateFromScans(所有已处理扫描)
• vis_utils::toNavMap 填 nav_msgs/OccupancyGrid
• 发布 /map 和 /map_metadata

栅格不是增量维护的占据栅格滤波器，而是周期用全部扫描重绘。图很大时这个线程会变重。

参数：min_pass_through（至少几束穿过才标占用/空闲）、occupancy_threshold（击中/穿过比）。