04 运行模式

模式子类只改“回调里何时处理、走哪条 Process*”。公共层不变。

4.1 异步建图 AsynchronousSlamToolbox

文件：src/slam_toolbox_async.cpp。

laserCallback：取 odom → getLaser → shouldProcessScan → 立刻 addScan。

• 匹配/回环慢时，回调堵住 executor，后续扫描由 MessageFilter 按 scan_queue_size（应设 1）丢掉。
• 不会“越积越多”，所以在线建图用这个。
• 反序列化拒绝 LOCALIZE_AT_POSE。

Launch：launch/online_async_launch.py → async_slam_toolbox_node + mapper_params_online_async.yaml。

4.2 同步建图 SynchronousSlamToolbox

文件：src/slam_toolbox_sync.cpp。

laserCallback：同样取 odom / laser / shouldProcess，然后 q_.push(PosedScan)。

on_activate 额外起 run()：

1
2
3
4

100 Hz 循环
  若未暂停 PROCESSING：
    出队 → getLaser → addScanImpl
  队列 >10：打警告，建议停车等处理完

• 录包离线、或要求尽量不丢帧时用。
• 在线跑同步且 CPU 不够，队列会无限涨。
• 服务 slam_toolbox/clear_queue 清空队列。
• reset 会先清队列再调基类 reset。

addScan 基类会再锁一次 smapper_mutex_；同步路径直接调 addScanImpl，锁在 run() 里已经拿着。

4.3 定位 LocalizationSlamToolbox

文件：src/slam_toolbox_localization.cpp。

on_configure：

• processor_type_ = PROCESS_LOCALIZATION
• 关交互模式、拆掉 MapSaver（定位不存图）

on_activate 额外：

• 订 /initialpose（与 AMCL / RViz “2D Pose Estimate” 同接口）
• 服务 slam_toolbox/clear_localization_buffer

loadPoseGraphByParams：若给了 map_file_name，按 LOCALIZE_AT_POSE 反序列化。map_start_at_dock 在定位里明确不支持（会 warn）。

覆盖的 addScan：

1. 若当前是 LOCALIZATION 且已有 process_near_pose_，先改成 PROCESS_NEAR_REGION（响应 /initialpose）
2. PROCESS_NEAR_REGION：把扫描位姿设成给定 pose，ProcessAgainstNodesNearBy(..., addScanToLocalizationBuffer=true)，然后回到 LOCALIZATION，并更新 reprocessing_transform_
3. PROCESS_LOCALIZATION：ProcessLocalization，不更新 reprocessing
4. 成功则只更新 TF 和 pose，不 dataset_->Add

serialize_map 在定位模式直接报错。反序列化必须是 LOCALIZE_AT_POSE。

官方 README 也写了：定位对里程计质量要求高，新手仍可用 AMCL。

4.4 实验性 Lifelong

文件：src/experimental/slam_toolbox_lifelong.cpp。

启动会 warn：实验性。思路是继续往图里加节点的同时，按 扫描重叠 IoU 给旧节点打分，低于 lifelong_node_removal_score 的删掉，让计算量有上界。

关键参数：lifelong_minimum_score、lifelong_iou_match、lifelong_node_removal_score、lifelong_overlap_score_scale、lifelong_search_use_tree。

产品上更稳妥的做法：用普通建图把图画完，再切定位，不要在边缘设备上开 lifelong。

4.5 建图/定位切换

MapAndLocalizationSlamToolbox 继承 Localization，允许运行时在 mapping / localization 之间切。参数 localization_on_configure 决定 configure 时落在哪边。给 Nav2 里“先建图后定位”的同一进程用。

4.6 去中心化多机

decentralized_multirobot_slam_toolbox_node：每台车独立跑一份 toolbox，交换 LocalizedLaserScan，在共享全局系里对齐位姿图。细节见源码 docs/decentralized_multi_robot_slam.md。第一阶段不必碰。

4.7 怎么选

05 Karto 内核

源码：lib/karto_sdk/include/karto_sdk/Mapper.h、lib/karto_sdk/src/Mapper.cpp。
这是 slam_toolbox 的算法本体。ROS 层只负责把 LocalizedRangeScan 喂进来，再把 CorrectedPose 变成 TF。

5.1 主要对象

SMapper（slam_mapper.cpp）只是包一层：configure() 把 YAML 写进 Mapper 的 setter，getOccupancyGrid() 调 OccupancyGrid::CreateFromScans。

5.2 Mapper::Process（建图主路径，约 L2731）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

校验 LaserRangeFinder::Validate
未初始化 → Initialize(rangeThreshold)
取该传感器 LastScan
若有上一帧：
    T = (odom_last → corrected_last)
    本帧 corrected = T * odom_curr      // 用上一帧的修正量预测
HasMovedEnough？不够则 false
若 use_scan_matching 且有上一帧：
    SequentialScanMatcher::MatchScan(本帧, RunningScans, bestPose, cov)
    SetSensorPose(bestPose)             // 同时更新 corrected
AddScan（赋 id）
AddVertex + AddEdges(cov)               // 与近邻连边
AddRunningScan
若 do_loop_closing：
    对每个传感器 TryCloseLoop
SetLastScan
return true

MatchScan 是前端：在局部 running scans 上做相关网格搜索，得到相对里程计更好的 R,t 和协方差。
AddEdges 把相邻约束交给 ScanSolver::AddConstraint（先积累，不算）。
TryCloseLoop 成功才会 CorrectPoses() → ScanSolver::Compute()。

5.3 回环：MapperGraph::TryCloseLoop（约 L1515）

1
2
3
4
5
6
7
8
9
10
11
12

FindPossibleLoopClosure          // 距离 loop_search_maximum_distance 内的候选链
                                 // 链长 ≥ loop_match_minimum_chain_size
while 还有候选：
    粗匹配 LoopScanMatcher::MatchScan(..., coarse=false 细搜关)
    粗响应 > loop_match_minimum_response_coarse
    且 cov xx/yy < loop_match_maximum_variance_coarse
        细匹配 SequentialScanMatcher
        细响应 ≥ loop_match_minimum_response_fine
            SetSensorPose(bestPose)
            LinkChainToScan      // 加回环边
            CorrectPoses()       // 调 solver Compute，写回各节点 pose
            FireEndLoopClosure   // ROS 层 LoopClosureListener 发 event

误回环会把整图拧断。阈值宁高勿低。关掉 do_loop_closing 可先验证里程计+前端。

ROS 层 LoopClosureListener 挂在 FireEndLoopClosure：发 /slam_toolbox/loop_closure_event，并 requestPoseGraphPublish()。整图只在回环后发，平时用 new_node_event 做增量。

5.4 定位：ProcessLocalization（约 L2883）

前半与 Process 几乎一样（预测、HasMovedEnough、MatchScan、加顶点加边、可回环），最后：

1

AddScanToLocalizationBuffer(scan, vertex)

缓冲区是队列，长度 = scan_buffer_size。超出则：

• RemoveNodeFromGraph（拆邻边，并从 Ceres RemoveNode / RemoveConstraint）
• delete 旧扫描

所以定位时图上只有“旧地图节点 + 最近 N 个窗口节点”。窗口过期不影响序列化进来的底图。

ClearLocalizationBuffer 在 /initialpose 时调用，窗口清空，LastScan / RunningScans 也清。

5.5 附近重定位：ProcessAgainstNodesNearBy

FindNearByScan 按当前（指定的）位姿找最近顶点，把 RunningScans 重置成该邻域，再 MatchScan。用于：

• 续建：START_AT_GIVEN_POSE
• 定位：/initialpose 后的第一帧

5.6 匹配在搜什么

Karto 的 scan matching 是 相关网格搜索，不是 ICP 迭代最近点：

• 把参考扫描 smearing 成查找表
• 在 correlation_search_space_dimension × 角度范围内离散搜
• 粗角度步长 coarse_angle_resolution，细角度 fine_search_angle_offset
• 响应低于阈值或 use_response_expansion 时扩大搜索窗
• distance_variance_penalty / angle_variance_penalty：偏离里程计越多惩罚越大

走廊等退化场景响应会变平，匹配可能沿走廊滑。这是前端固有问题，不是 YAML 能彻底消灭的。

5.7 和 ROS 门槛的关系

shouldProcessScan 先滤一遍，HasMovedEnough 再用 minimum_travel_distance / minimum_travel_heading 滤一遍。
默认 shouldProcessScan 用 0.8 * dist²，所以多数时候 ROS 层先挡住。把 check_min_dist_and_heading_precisely 打开后，原地旋转也能过 ROS 层，再交给 Karto 的航向门槛。

06 Ceres 求解器

源码：solvers/ceres_solver.hpp、solvers/ceres_solver.cpp。
通过 pluginlib 注册为 karto::ScanSolver（solver_plugins.xml）。

G2O / SPA / GTSAM 的代码还在树里，默认不编译。作者对比后认为 Ceres + SPARSE_NORMAL_CHOLESKY + SCHUR_JACOBI 足够。

6.1 插件如何挂上

SlamToolbox::setSolver()：

1
2
3
4

solver_plugin 默认 "solver_plugins::CeresSolver"
solver_loader_.createSharedInstance(...)
solver_->Configure(shared_from_this())   // 读 YAML
smapper_->getMapper()->SetScanSolver(solver_.get())

反序列化时 loadSerializedPoseGraph：Reset → 遍历顶点 AddNode → 遍历边 AddConstraint → SetScanSolver → Compute 一次，把图落到优化器里。

6.2 问题定义

每个图节点是 2D 位姿 (x, y, θ)，存在 nodes_[unique_id] = Eigen::Vector3d。
θ 用 AngleManifold 参数化，避免角度跳变。

每条边是 LinkInfo：相对位姿 GetPoseDifference() + 协方差。
信息矩阵 = cov.Inverse()，再 llt().matrixU() 得到 sqrt_information，残差是标准 PoseGraph2dErrorTerm（ceres_utils.h）。

第一个节点的 (x,y,θ) 在第一次 Compute 时被 SetParameterBlockConstant，钉住规范，消除刚体自由度。

6.3 何时 Optimize

• AddNode / AddConstraint：只往 ceres::Problem 里加块，不算
• TryCloseLoop → CorrectPoses → ScanSolver::Compute：ceres::Solve
• 成功则 corrections_ 填所有节点新位姿，Karto 写回 LocalizedRangeScan::SetCorrectedPose

所以日常前端匹配不跑 Ceres，只有回环或加载旧图才跑。这是能在弱 CPU 上建大图的原因之一。

6.4 mode: localization 的含义

YAML 的 mode 主要给 Ceres 看，不是选节点类型。

mode == "localization" 时：Problem::Options.enable_fast_removal = true，方便窗口过期时 RemoveNode / RemoveConstraint。内存大约翻倍。
建图 YAML 应写 mapping，定位 YAML 写 localization。和用哪个可执行文件是两件事，但应对齐。

6.5 推荐与可调项

求解容差约 function_tolerance=1e-3（相对 5 cm 栅格是 50 倍）。num_threads=50 只是上限，实际按核数。

6.6 二次开发

换求解器：改 solver_plugin，不要改 Mapper.cpp。
真要改残差：动 PoseGraph2dErrorTerm，属于 L5。
ISlam 管理层不要直接碰 Ceres。

07 TF、雷达模型与序列化

7.1 坐标系约定

默认（setParams）：

1
2
3
4

map_frame    = map
odom_frame   = odom
base_frame   = base_footprint
scan_topic   = /scan          // 必须绝对路径

树：

1

map ──(toolbox 发)──► odom ──(底盘发)──► base_footprint ──(URDF)──► laser

GetPoseHelper 查的是 odom ← base_footprint。
LaserAssistant::readLaserPose 查的是 base_footprint ← laser（扫描 stamp）。
getLaser 失败日志：Failed to compute laser pose —— 静态 TF / URDF 没把 laser 接到 base_frame。

不要两个节点抢发 map→odom。

7.2 雷达模型：LaserAssistant

文件：src/laser_utils.cpp。

第一次见到某个 frame_id：

1. readLaserPose()：tf->transform(laser恒等, base_frame_)
2. isInverted：把 base 的 z 轴变到雷达系，变完后 z≤0 视为倒置，后续距离倒序
3. makeLaser：
   • SetOffsetPose(x, y, mountingYaw) —— 只用平面 x,y 和 yaw，忽略雷达相对底盘的 z/pitch/roll（2D 假设）
   • min_laser_range / 消息 range_min 取合理下界
   • max_laser_range 同时是匹配用的 RangeThreshold（超消息 range_max 会警告并截断）
   • 角范围接近 2π 标成 360° 雷达；若多一个点（[0,360] 而不是 [0,360)）会强制非 360，避免 2021-03 那类坐标系 bug

dataset_->Add(laser) 后，序列化会带上这份雷达模型。换雷达或改外参应重新建图，不要混用旧 .data。

7.3 序列化文件

serialization.hpp：

服务 serialize_map 的 filename 不要带扩展名。
save_map 是另一回事：包 map_saver 出 pgm + yaml，给 AMCL / 显示用，不能拿来 deserialize_map。

定位 / 续建必须用 .posegraph + .data。

2021-03 ABI

扫描在优化器里的存储坐标系修过一次。360° 或斜装雷达的旧文件，在新代码里可能炸图或翻转。新项目只用新代码建的图。选项见官方 README。

7.4 反序列化后如何对齐

deserializePoseGraphCallback 读文件 → loadSerializedPoseGraph（节点/边灌进 Ceres 并 Compute）→ updateMap → 按 match_type 设 processor_type_：

YAML 启动加载：map_file_name + (map_start_pose: [x,y,yaw] 或 map_start_at_dock: true)。两者都给时用 pose。

7.5 续建时的 reprocessing_transform_

新旧会话的轮速 odom 原点不同。dock / near-region 对齐成功后，setTransformFromPoses(..., update_reprocessing_transform=true) 估一个齐，之后 getLocalizedRangeScan 先乘它再进 Karto。没对齐就续建，图会在旧图旁边另起一摊。

08 接口、参数与排障

8.1 订阅

/scan 的 QoS 是 SensorDataQoS（Best Effort）。/map、pose 是 Transient Local + Reliable。

8.2 发布

协方差可乘 position_covariance_scale_ / yaw_covariance_scale_，给下游 EKF 降权。

8.3 服务

管理层（未来 ISlam）优先包：serialize / deserialize / save / pause / reset / pose。

8.4 参数分组（调 YAML，不改代码）

坐标系与 IO：odom_frame map_frame base_frame scan_topic resolution min/max_laser_range

何时处理：throttle_scans minimum_time_interval minimum_travel_distance minimum_travel_heading check_min_dist_and_heading_precisely

前端匹配：use_scan_matching correlation_search_space_* fine/coarse_search_angle_* distance/angle_variance_penalty use_response_expansion

回环：do_loop_closing loop_search_maximum_distance loop_match_minimum_chain_size loop_match_minimum_response_coarse/fine loop_search_space_*

定位窗口：scan_buffer_size scan_buffer_maximum_scan_distance mode: localization

Ceres：见 06-Ceres求解器.md

栅格：map_update_interval min_pass_through occupancy_threshold

完整默认值见 config/mapper_params_online_async.yaml。SMapper::configure 是参数进入 Karto 的唯一入口。

8.5 日志 → 原因

8.6 建议的排查顺序

传感器频率 → 时间戳是否单调、是否同一时钟 → /scan.frame_id → view_frames 是否只有一处 map→odom → 静止是否还在爬（里程计）→ 再动回环阈值。

09 读码顺序与二次开发

9.1 建议读码顺序

不要从 Mapper.cpp 第 1 行读起。按调用链：

1. launch/online_async_launch.py
   Lifecycle 如何 CONFIGURE/ACTIVATE；默认 YAML。
2. config/mapper_params_online_async.yaml
   和 SMapper::configure、setParams 对一下名字。
3. include/slam_toolbox/slam_toolbox_common.hpp
   把成员表对照 02。
4. src/slam_toolbox_async.cpp 的 laserCallback（约 30 行）
   整条产品链的入口。
5. shouldProcessScan / getLocalizedRangeScan / addScanImpl / setTransformFromPoses
src/slam_toolbox_common.cpp。
6. get_pose_helper.hpp、laser_utils.cpp 的 toLaserMetadata / isInverted。
7. lib/karto_sdk/src/Mapper.cpp 的 Process（约 2731）和 TryCloseLoop（约 1515）。
8. solvers/ceres_solver.cpp 的 AddNode / AddConstraint / Compute。
9. src/slam_toolbox_localization.cpp 的 addScan 与 localizePoseCallback。
10. 需要时再读 lifelong、多机、merge_maps_kinematic、RViz 插件。

9.2 文件速查

9.3 二次开发切面

第一阶段继续：系统包或本树原样跑，用 service 做管理层。

ISlam 建议对应：

1
2
3
4
5

start/stop     → lifecycle 或 launch
save_map       → serialize_map（产品图）+ save_map（pgm 可选）
load_map       → deserialize_map
getPose        → /pose 或查 TF map←base
getState       → 看 /map 是否在更、有无 Lost（需自己根据 pose 协方差定义）

9.4 和本仓库其它文档

• 产品主线：../../docs/SLAM开发知识.md
• 学习模块 E3：../../docs/学习知识模块.md
• 较短速览：../../docs/slam_toolbox源码分析.md（指向本目录）

9.5 编译这份源码

步骤、Humble 补丁、overlay 和冒烟见 10-编译与测试.md。

系统 apt 已是 2.6.10 时，编 2.10 必须 overlay，不要和系统 launch 混用。

10 编译与测试

对应源码：../slam_toolbox（ros2 分支，包版本 2.10.0）。
本机环境：Ubuntu 22.04 · ROS 2 Humble · CMake 3.22 · Ceres 2.0.0。

官方 ros2 分支默认对着 Jazzy / Ubuntu 24.04 / Ceres ≥ 2.1。在 Humble 上能编过，是本仓库加过兼容层，不是上游原样。系统 apt 仍是 2.6.10（launch 还不是 Lifecycle），编完必须 overlay，不要和 apt 包混用。

10.1 工作空间

colcon 在仓库根目录，不在 third_party/slam_toolbox 里直接 cmake：

1
2
3
4
5
6

slamDev/
├── src/slam_toolbox          → ../third_party/slam_toolbox
├── build/                    gitignore
├── install/                  gitignore
├── log/                      gitignore
└── third_party/slam_toolbox/ 源码（含本机 Humble 补丁）

建软链（已做过可跳过）：

1
2
3

cd /home/cp/work2/slamDev
mkdir -p src
ln -sfn ../third_party/slam_toolbox src/slam_toolbox

build/ install/ log/ 已在仓库 .gitignore。

10.2 依赖

ros-humble-desktop + 本机已装的求解库即可。缺包时：

1
2
3
4
5
6
7
8
9

source /opt/ros/humble/setup.bash
sudo apt update
sudo apt install -y \
  libceres-dev libsuitesparse-dev liblapack-dev libomp-dev libtbb-dev \
  libboost-serialization-dev libboost-filesystem-dev libboost-thread-dev \
  qtbase5-dev libqt5-core5a \
  ros-humble-nav2-map-server ros-humble-interactive-markers \
  ros-humble-rviz-common ros-humble-rviz-default-plugins \
  python3-colcon-common-extensions

或：

1
2
3

cd /home/cp/work2/slamDev
source /opt/ros/humble/setup.bash
rosdep install --from-paths src --ignore-src -r -y

核对：

10.3 Humble 兼容（本树已改）

2.10 原样在 Humble 上会失败。本仓库改动如下，再编不用手改：

CMake 在 ROS_DISTRO=humble 时定义 SLAM_TOOLBOX_HUMBLE_COMPAT。换 Jazzy 编同一棵树时不要带这个宏，走上游 API。

10.4 编译

1
2
3
4
5
6

cd /home/cp/work2/slamDev
source /opt/ros/humble/setup.bash
colcon build --packages-select slam_toolbox \
  --allow-overriding slam_toolbox \
  --symlink-install \
  --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

--allow-overriding 必须加：underlay 里已有 apt 的同名包。
C++17，CMakeLists.txt 默认 Release。首次大约 5 分钟。

只改过某个 .cpp 时同样命令即可，--symlink-install 下 launch / yaml 改完不用重编。

成功时：

1
2

Finished <<< slam_toolbox
Summary: 1 package finished

产物：

1
2
3
4
5
6
7
8

install/slam_toolbox/lib/slam_toolbox/
  async_slam_toolbox_node
  sync_slam_toolbox_node
  localization_slam_toolbox_node
  lifelong_slam_toolbox_node
  map_and_localization_slam_toolbox_node
  decentralized_multirobot_slam_toolbox_node
  merge_maps_kinematic

库：install/slam_toolbox/lib/libtoolbox_common.so、libkartoSlamToolbox.so、libceres_solver_plugin.so、libSlamToolboxPlugin.so。

跳转定义：把 build/slam_toolbox/compile_commands.json 链到源码根（已链过可跳过）：

1
2

ln -sfn /home/cp/work2/slamDev/build/slam_toolbox/compile_commands.json \
  /home/cp/work2/slamDev/third_party/slam_toolbox/compile_commands.json

IDE / clangd Reload Window。

10.5 环境（overlay）

每个新终端：

1
2

source /opt/ros/humble/setup.bash
source /home/cp/work2/slamDev/install/setup.bash

确认吃到的是 2.10，不是 apt 2.6：

1
2
3
4
5

ros2 pkg prefix slam_toolbox
# 应为 /home/cp/work2/slamDev/install/slam_toolbox
# 若是 /opt/ros/humble，说明没 source overlay

ros2 pkg executables slam_toolbox

10.6 仓库自带测试

CMakeLists.txt 里 gtest 整段注释掉了，colcon test --packages-select slam_toolbox 现在几乎无用例。

1
2
3
4

#if(BUILD_TESTING)
#  ament_add_gtest(lifelong_metrics_test test/lifelong_metrics_test.cpp)
#  ...
#endif()

test/lifelong_metrics_test.cpp 测 lifelong 的包围盒 / IoU，不是建图正确性。要跑：先取消这段注释，再：

1
2
3
4
5
6

cd /home/cp/work2/slamDev
source /opt/ros/humble/setup.bash
colcon build --packages-select slam_toolbox --allow-overriding slam_toolbox \
  --cmake-args -DBUILD_TESTING=ON
colcon test --packages-select slam_toolbox
colcon test-result --verbose

test/process_constraints.py 是画「节点约束数」的离线脚本，要先在 publishGraph 里加它注释里的打印，不是 CI。

10.7 冒烟：包与节点

不接雷达，只确认 overlay 和可执行文件：

1
2
3
4
5
6
7

source /opt/ros/humble/setup.bash
source /home/cp/work2/slamDev/install/setup.bash

test "$(ros2 pkg prefix slam_toolbox)" = "/home/cp/work2/slamDev/install/slam_toolbox" && echo "overlay OK"

ros2 run slam_toolbox async_slam_toolbox_node --ros-args --params-file \
  $(ros2 pkg prefix slam_toolbox)/share/slam_toolbox/config/mapper_params_online_async.yaml

Lifecycle 节点起来后停在 unconfigured（没有 launch 的 configure/activate）。另开终端：

1
2
3
4
5

source /opt/ros/humble/setup.bash
source /home/cp/work2/slamDev/install/setup.bash
ros2 lifecycle list /slam_toolbox          # 或节点实际名
ros2 lifecycle set /slam_toolbox configure
# 无雷达时 activate 可能卡住等 scan/TF，Ctrl+C 结束即可

看到 CeresSolver: Using ... 或 SlamToolbox: ... 日志，说明插件和公共层加载成功。

10.8 冒烟：launch（需要 scan + TF）

2.10 的 launch 是 Lifecycle：autostart:=true 时自动 configure → activate。

最低输入：

• /scan（sensor_msgs/LaserScan）
• TF odom → base_frame（默认 base_footprint，按 YAML 改）
• 雷达相对 base 的 tf_static

仿真 / bag 记得 use_sim_time:=true，并先播 /clock。

1
2
3
4
5
6
7
8
9

source /opt/ros/humble/setup.bash
source /home/cp/work2/slamDev/install/setup.bash

# 另开：ros2 bag play <bag> --clock
# 或：机器人驱动 + 雷达

ros2 launch slam_toolbox online_async_launch.py \
  use_sim_time:=true \
  autostart:=true

其它入口：

通过标准（async 建图）：

1. ros2 topic hz /map 有输出（有订阅者时才更新，RViz 订 /map）
2. ros2 topic echo /tf --once 里有 map → odom
3. ros2 topic echo /pose --once 有位姿
4. 转一圈墙角大致对齐，没有整图拧成麻花（回环阈值另见 08）

存图：

1
2
3
4
5
6
7

ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
  "{filename: '/tmp/test_map'}"
# 生成 /tmp/test_map.posegraph 和 /tmp/test_map.data

ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap \
  "{name: {data: '/tmp/test_map'}}"
# 栅格，给 Nav2；和上面的位姿图不是一回事

定位模式：YAML 里填 map_file_name，或启动后再 deserialize_map。RViz 2D Pose Estimate 发 /initialpose。

10.9 对照实验（建议新手做）

改一个量，看图怎么变；比空跑 gtest 有用：

10.10 编译失败对照

清掉重编：

1
2

cd /home/cp/work2/slamDev
rm -rf build/slam_toolbox install/slam_toolbox log

不要 rm -rf build 整个目录，除非没有其它包。

10.11 和二次开发的关系

• 改 YAML / launch：--symlink-install 后直接 relaunch。
• 改 src/、solvers/、karto_sdk：按 10.4 重编，再 overlay。
• 上实车若只用 apt 2.6.10：不要用 2.10 的 Lifecycle launch 去喂系统包。
• 读码顺序仍见 09。编译只保证「能跑」，不代替沿 Process() 走一帧。

SLUB 小对象分配机制与实现详解

源码路径：rk3588/kernel-6.1/mm/slub.c、slab_common.c
内核版本：Linux 6.1（RK3588，通常 CONFIG_SLUB=y）
文档目录：linuxDoc/mm/

目录

• 一、原理
• 二、实现方式
• 三、关键数据结构与接口
• 四、与页分配器关系
• 五、调试与观测
• 附录：源码索引

一、原理

1.1 为什么需要 slab

内核大量 固定大小小对象（task_struct、inode、sk_buff 头等），若每对象一次 __alloc_pages(0)：

• 浪费内存（页内碎片）
• 增加 buddy 锁竞争

Slab 分配器 以 页为 slab，在页内切分等长 object，按 cache 复用。

1.2 SLUB 设计思想

SLUB（Six Less Unfair Buddy）相对 SLAB/SLOB：

• per-CPU slab 指针：热路径无中心锁
• 合并 partial slab 管理：减少 per-node 链表遍历
• freelist 在 object 内嵌指针：减少元数据

RK3588 defconfig 一般启用 CONFIG_SLUB，禁用 SLAB/SLOB。

1.3 三级存储层次

1.4 kmalloc

kmalloc(size, gfp) 根据 size 索引到预建 kmalloc-* cache（slab_common.c 中 create_kmalloc_caches），本质仍是 kmem_cache_alloc。

二、实现方式

2.1 核心对象

2.2 分配入口

1
2
3
4

void *kmem_cache_alloc(struct kmem_cache *s, gfp_t gfpflags)
{
	return __kmem_cache_alloc_lru(s, NULL, gfpflags);
}

→ slab_alloc() → slab_alloc_node() → ___slab_alloc()。

2.3 快路径（inlined）

slub.c 中 slab_alloc_node 内联路径：

1. 取 cpu_slab = slub_get_cpu_ptr(s->cpu_slab)
2. 若 c->slab 存在且 c->freelist 非空：直接从 freelist 弹对象（get_freepointer）
3. 使用 tid 检测 CPU 迁移，不一致则走慢路径

2.4 慢路径：___slab_alloc

1. get_partial()：从 node partial 链表取 slab
2. 若无：new_slab() → allocate_slab() → alloc_pages（order 由 oo/min_partial 决定）
3. 新 slab 标记 frozen，填入 CPU slab
4. debug 模式走 alloc_single_from_new_slab 与 red zone

2.5 释放：kmem_cache_free

• 快路径：对象放回 CPU freelist
• __slab_free：慢路径，可能将 slab 合并到 partial、或 discard_slab 归还 buddy
• deactivate_slab：flush 当前 CPU slab 到 node 链表

2.6 slab_common.c 引导

2.7 调试与硬化

三、关键数据结构与接口

锁顺序（slub.c 文件头）：slab_mutex → node->list_lock → cpu_slab->lock。

四、与页分配器关系

• SLUB 不替代 buddy；slab 页来自 alloc_pages(gfp, order)。
• 若 buddy 失败，SLUB 返回 NULL，调用方处理；可能间接触发 OOM（见 08 文档）。
• __GFP_COMP：folio/slub 大对象可能使用复合页。

五、调试与观测

附录：源码索引

虚拟内存与缺页机制与实现详解

源码路径：rk3588/kernel-6.1/mm/memory.c、mmap.c、mprotect.c、mremap.c、mlock.c、madvise.c
内核版本：Linux 6.1（RK3588 / ARM64）
文档目录：linuxDoc/mm/

目录

• 一、原理
• 二、实现方式
• 三、关键数据结构与接口
• 四、RK3588 平台说明
• 五、调试与观测
• 附录：源码索引

一、原理

1.1 虚拟地址空间

每个用户进程有 struct mm_struct，包含：

• 页表根（pgd）
• VMA 链表 / maple tree（vm_area_struct）：每段连续虚拟区间的权限、文件映射、堆栈属性
• mmap_lock：保护 VMA 与页表并发

CPU 通过 MMU 将虚拟地址译为物理地址；页表项无效或访问违例触发 缺页异常。

1.2 按需分配（Demand Paging）

匿名映射、文件映射通常 先建立 VMA，不立即分配物理页；首次访问触发 fault：

• 匿名：分配零页或 COW 副本
• 文件：从 page cache 读入或新建 cache 页
• swap：从 swap 读回

1.3 写时复制（COW）

fork() 后父子共享只读 PTE；任一方写入触发 写保护 fault → do_wp_page() 分配新物理页并更新 PTE。

1.4 大页与 THP

若 VMA 允许且策略开启，可在 PMD/PUD 层安装 透明大页（create_huge_pmd 等），减少 TLB miss。

二、实现方式

2.1 mmap：do_mmap()

mmap.c 中 do_mmap()（~1253 行）：

1. get_unmapped_area() 找空洞
2. vm_mmap_pgoff() → do_mmap_pgoff
3. 创建 vm_area_struct，设置 vm_ops、vm_flags（VM_READ/WRITE/EXEC、VM_SHARED 等）
4. 文件映射调用 file->f_op->mmap 或 generic_file_mmap
5. 插入 maple tree / 链表，必要时 userfaultfd 通知

munmap / expand_stack / brk 在同文件修改或删除 VMA。

2.2 缺页总入口

架构层（arch/arm64/mm/fault.c）解析异常后调用：

handle_mm_fault(vma, address, flags, regs) → __handle_mm_fault()

1
2
3
4
5
6
7
8

static vm_fault_t __handle_mm_fault(struct vm_area_struct *vma,
		unsigned long address, unsigned int flags)
{
	// 初始化 vm_fault vmf
	pgd → p4d → pud → pmd 逐级分配/检查
	// THP: create_huge_pud / wp_huge_pud / huge_pmd_set_accessed
	return handle_pte_fault(&vmf);
}

2.3 PTE 级：handle_pte_fault()

根据 PTE 状态分支（同文件）：

返回 vm_fault_t：VM_FAULT_OK、VM_FAULT_RETRY（可能释放 mmap_lock）、VM_FAULT_OOM、VM_FAULT_SIGSEGV 等。

2.4 匿名页：do_anonymous_page

• alloc_zeroed_user_highpage_movable 或类似分配
• alloc_set_pte 安装 PTE
• memcg charge（若启用）

2.5 写保护：do_wp_page

• 若仍共享：分配新页，copy_page / copy_present_page
• 更新 PTE 为可写
• 与 userfaultfd、KSM、THP 有交叉分支

2.6 相关系统调用实现

2.7 mmu_gather

批量 unmap/munmap 时用 struct mmu_gather（mmu_gather.c）累积 TLB flush，减少 IPI。

三、关键数据结构与接口

锁：mmap_read_lock / mmap_write_lock；fault 中可能 VM_FAULT_RETRY 释放锁后重试。

四、RK3588 平台说明

• ARM64 使用 4 级页表（PGD/P4D/PUD/PMD/PTE，P4D 可能折叠）。
• 编解码进程大量 匿名映射 + mmap(dma-buf fd)：文件类 fault 在 dmabuf/file 路径，匿名堆栈在 do_anonymous_page。
• MADV_DONTNEED 可释放冷数据缓解内存压力（与 07 回收协同）。

五、调试与观测

附录：源码索引

页缓存与写回机制与实现详解

源码路径：rk3588/kernel-6.1/mm/filemap.c、readahead.c、page-writeback.c、backing-dev.c、truncate.c
内核版本：Linux 6.1（RK3588）
文档目录：linuxDoc/mm/

目录

• 一、原理
• 二、实现方式
• 三、关键数据结构与接口
• 四、RK3588 平台说明
• 五、调试与观测
• 附录：源码索引

一、原理

1.1 Page Cache 角色

页缓存缓存 文件数据 的物理页，使：

• 读：优先内存命中，减少块设备 I/O
• 写：先改缓存页，异步 写回（writeback） 到存储
• mmap：文件映射直接 fault 到 cache 中的 folio

同一文件 inode 对应 struct address_space，其 i_pages（xarray）索引 offset → folio。

1.2 Folio（Linux 6.1）

folio 表示一个或多个连续 page 的缓存单元（尤其 THP、大块 I/O），filemap.c 路径以 folio 为中心，legacy API 经 folio-compat.c 适配。

1.3 预读（Readahead）

顺序读检测 访问模式，提前将后续页读入 cache，隐藏磁盘延迟。

1.4 写回与脏页

修改 mapped 或 write() 路径使页 置脏；pdflush/bdi writeback 线程在脏页比例或 sync 时写回设备。

backing_dev 限制每设备的 inflight 写与脏页占比，防止内存被脏页占满。

二、实现方式

2.1 文件映射缺页：filemap_fault()

filemap.c ~3191：

1. 根据 vmf->pgoff 在 mapping->i_pages 查找 folio
2. 未命中：pagecache_get_read / filemap_read_folio 读盘
3. 可能 lock_page，返回 VM_FAULT_RETRY
4. 安装 PTE，folio_mark_accessed

与 __handle_mm_fault 中文件 VMA 的 vm_ops->fault 衔接。

2.2 缓冲读：filemap_read / generic_file_read_iter

• 按 offset 查找或分配 cache folio
• 未 uptodate 则提交 bio 读盘
• 拷贝到用户 iov

2.3 预读：readahead.c

• page_cache_sync_readahead / ondemand_readahead
• 根据 file->f_ra（struct file_ra_state）维护预读窗口
• readahead_expand 扩大窗口；force_page_cache_ra 强制预读

2.4 脏页与写回：page-writeback.c

balance_dirty_pages：进程写超过 dirty_ratio 时节流。

2.5 backing-dev.c

每个块设备/文件系统注册 struct backing_dev_info：

• max_ratio、dirty_bandwidth 等
• cgroup writeback 关联（CONFIG_CGROUP_WRITEBACK）

2.6 truncate.c

文件缩小或删除时 truncate_inode_pages：从 xarray 摘除 folio，若映射存在则通过 rmap unmap（见 06 文档）。

2.7 workingset 交互

频繁回收的 cache 页 refault 由 workingset.c 统计，影响 vmscan 对文件页的回收积极性（见 07 文档）。

三、关键数据结构与接口

a_ops 常用：read_folio、writepages、dirty_folio、migrate_folio。

四、RK3588 平台说明

• 编解码 读配置文件/码流 走 page cache；大文件顺序读可调 fadvise(SEQ)（fadvise.c）。
• dmabuf / GEM 不一定经过传统 page cache，但 VFS 层媒体仍可能用 shmem（见 12 文档）。
• eMMC/UFS 慢时 dirty 内存高 会触发回收与写回竞争，表现为编码卡顿与 NR_WRITEBACK 升高。

五、调试与观测

trace：filemap tracepoints。

附录：源码索引