slam_toolbox 生命周期与公共层

02 生命周期与公共层

源码：include/slam_toolbox/slam_toolbox_common.hpp、src/slam_toolbox_common.cpp。

2.1 为什么是 LifecycleNode

ros2 分支把节点改成 rclcpp_lifecycle::LifecycleNode，以便挂到 Nav2 的 lifecycle manager（bond）。online_async_launch.py 默认 autostart=true 时，launch 自己发 CONFIGURE → ACTIVATE。

Humble 系统包 2.6.10 的 launch 仍是普通 Node，和这份源码不一致。

2.2 状态机

unconfigured
    │ on_configure
    ▼
 inactive
    │ on_activate
    ▼
  active  ← 订阅 /scan、发 TF 和 /map
    │ on_deactivate
    ▼
 inactive
    │ on_cleanup
    ▼
unconfigured

on_configure（约 L110）

1. processor_type_ = PROCESS，first_measurement_ = true，state_.reset()
2. 新建 SMapper、Dataset
3. setParams()、setSolver()（pluginlib 加载 Ceres，Mapper::SetScanSolver）
4. 建 TF buffer / listener / broadcaster
5. 建 LaserAssistant、GetPoseHelper、ScanHolder、MapSaver、LoopClosureAssistant
6. loadPoseGraphByParams()：若 YAML 给了 map_file_name 则立刻反序列化

此时还不订 /scan。

on_activate（约 L157）

1. setROSInterfaces()：publisher、service、MessageFilter 订 scan_topic_
2. 各 LifecyclePublisher on_activate
3. 注册 LoopClosureListener 到 Mapper（回环时发 event、请求发整图）
4. reprocessing_transform_ 置单位阵
5. 启动 TF 线程、可视化线程
6. 若 use_lifecycle_manager，建 bond

on_deactivate / on_cleanup

停线程、拆订阅和服务、拆 helper。再次 configure 是干净会话。

2.3 构造时的栈

序列化大图需要大栈。构造函数读只读参数 stack_size_to_use（默认 40e6），setrlimit(RLIMIT_STACK)。Windows 不可改。

2.4 关键成员（对照 hpp）

成员	类型	作用
smapper_	SMapper	持有 karto::Mapper
dataset_	karto::Dataset	雷达 + 扫描，写 .data
solver_	shared_ptr<ScanSolver>	默认 Ceres
lasers_	map<frame_id, LaserMetadata>	每个雷达 frame 一份模型
pose_helper_	GetPoseHelper	查 odom ← base
laser_assistant_	LaserAssistant	扫描 + TF → Karto 雷达
map_to_odom_	tf2::Transform	TF 线程要发的边
processor_type_	ProcessType	本次扫描走哪条 Process*
process_near_pose_	Pose2*	指定附近重定位的目标位姿
reprocessing_transform_	tf2::Transform	续建时新旧 odom 对齐
state_	PausedState	NEW_MEASUREMENTS / PROCESSING / VISUALIZING_GRAPH
first_measurement_	bool	首帧或复位后强制处理

2.5 ProcessType（toolbox_types.hpp）

枚举	谁设	Mapper 入口
PROCESS	默认建图	Process
PROCESS_FIRST_NODE	反序列化 START_AT_FIRST_NODE	ProcessAtDock，一次后回到 PROCESS
PROCESS_NEAR_REGION	START_AT_GIVEN_POSE 或 /initialpose	ProcessAgainstNodesNearBy
PROCESS_LOCALIZATION	定位节点 / LOCALIZE_AT_POSE	ProcessLocalization

建图节点若收到 LOCALIZE_AT_POSE 会拒绝。定位节点若收到非 LOCALIZE_AT_POSE 也会拒绝。

2.6 暂停

PausedState 三路独立：

• NEW_MEASUREMENTS：shouldProcessScan 直接 false（服务 pause_new_measurements）
• PROCESSING：同步模式的 run() 不出队
• VISUALIZING_GRAPH：不发交互图，省 RViz

reset 服务可在清空图后把 NEW_MEASUREMENTS 置暂停。